X-Git-Url: http://git.tdb.fi/?p=r2c2.git;a=blobdiff_plain;f=source%2Flibr2c2%2Farducontrol.cpp;h=8446294f8562619b416ecd439bcc2cd5d8acfbd3;hp=1af7fcad3bed831bcfd474628cc7ad5265691a87;hb=25aacc9bfe66f1a2bffd196c735d3444adac6fe3;hpb=b78b49d85fbb9b2901c77e6450cfd41c0a818ac1

diff --git a/source/libr2c2/arducontrol.cpp b/source/libr2c2/arducontrol.cpp
index 1af7fca..8446294 100644
--- 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;