Provide some telemetry values from ArduControl

[r2c2.git] / source / libr2c2 / arducontrol.cpp

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;