Provide some telemetry values from ArduControl

diff --git a/source/libr2c2/arducontrol.cpp b/source/libr2c2/arducontrol.cpp
index 1af7fcad3bed831bcfd474628cc7ad5265691a87..8446294f8562619b416ecd439bcc2cd5d8acfbd3 100644 (file)
--- a/source/libr2c2/arducontrol.cpp
+++ b/source/libr2c2/arducontrol.cpp
@@ -18,6 +18,14 @@ ArduControl::ProtocolInfo ArduControl::protocol_info[2] =
        { 0x3FFF, 126, 15 }  // MFX
 };
 
+Driver::TelemetryInfo ArduControl::telemetry_info[4] =
+{
+       { "voltage", "Voltage", "V", 1 },
+       { "current", "Current", "A", 2 },
+       { "cmd-queue-depth", "Cmd queue", "", 0 },
+       { "s88-latency", "S88 latency", "ms", 0 }
+};
+
 ArduControl::ArduControl(const Options &opts):
        serial(opts.get<string>(string(), "ttyUSB0")),
        debug(opts.get<unsigned>("debug")),
@@ -327,9 +335,26 @@ bool ArduControl::get_sensor(unsigned addr) const
        return get_item(sensors, addr).state;
 }
 
+const Driver::TelemetryInfo *ArduControl::enumerate_telemetry(unsigned i) const
+{
+       if(i<4)
+               return telemetry_info+i;
+       else
+               return 0;
+}
+
 float ArduControl::get_telemetry_value(const string &name) const
 {
-       throw key_error(name);
+       if(name==telemetry_info[0].name)
+               return monitor.get_voltage();
+       else if(name==telemetry_info[1].name)
+               return monitor.get_current();
+       else if(name==telemetry_info[2].name)
+               return command_queue.size();
+       else if(name==telemetry_info[3].name)
+               return s88.get_latency()/Time::msec;
+       else
+               throw key_error(name);
 }
 
 void ArduControl::tick()
@@ -700,6 +725,12 @@ bool ArduControl::Queue<T>::pop(T &item)
        return true;
 }
 
+template<typename T>
+unsigned ArduControl::Queue<T>::size() const
+{
+       return items.size();
+}
+
 template<typename T>
 bool ArduControl::Queue<T>::empty() const
 {
@@ -840,6 +871,11 @@ bool ArduControl::S88Task::get_work(PendingCommand &cmd)
        if(octets_remaining || !n_octets)
                return false;
 
+       Time::TimeStamp t = Time::now();
+       if(last_poll)
+               latency = t-last_poll;
+       last_poll = t;
+
        octets_remaining = n_octets;
        cmd.command[0] = S88_READ;
        cmd.command[1] = octets_remaining;

diff --git a/source/libr2c2/arducontrol.h b/source/libr2c2/arducontrol.h
index 292700d5c452d8daee03e2815542e34b7297404e..a48f6f0990aa59997b9c45bdde7778cd3506c17c 100644 (file)
--- a/source/libr2c2/arducontrol.h
+++ b/source/libr2c2/arducontrol.h
@@ -220,6 +220,7 @@ private:
        public:
                void push(const T &);
                bool pop(T &);
+               unsigned size() const;
                bool empty() const;
        };
 
@@ -256,6 +257,7 @@ private:
                virtual bool get_work(PendingCommand &);
 
                void push(const PendingCommand &);
+               unsigned size() const { return queue.size(); }
                bool empty() const { return queue.empty(); }
        };
 
@@ -289,6 +291,8 @@ private:
                ArduControl &control;
                unsigned n_octets;
                unsigned octets_remaining;
+               Msp::Time::TimeStamp last_poll;
+               Msp::Time::TimeDelta latency;
 
        public:
                S88Task(ArduControl &);
@@ -298,6 +302,8 @@ private:
 
                void set_n_octets(unsigned);
                void grow_n_octets(unsigned);
+
+               const Msp::Time::TimeDelta &get_latency() const { return latency; }
        };
 
        class MfxAnnounceTask: public Task
@@ -409,6 +415,7 @@ private:
        ControlThread thread;
 
        static ProtocolInfo protocol_info[2];
+       static TelemetryInfo telemetry_info[4];
 
 public:
        ArduControl(const Options &);
@@ -458,7 +465,7 @@ public:
        virtual void set_sensor(unsigned, bool) { }
        virtual bool get_sensor(unsigned) const;
 
-       virtual const TelemetryInfo *enumerate_telemetry(unsigned) const { return 0; }
+       virtual const TelemetryInfo *enumerate_telemetry(unsigned) const;
        virtual float get_telemetry_value(const std::string &) const;
 
        virtual void tick();