From 25aacc9bfe66f1a2bffd196c735d3444adac6fe3 Mon Sep 17 00:00:00 2001
From: Mikko Rasa <tdb@tdb.fi>
Date: Thu, 19 Feb 2015 15:29:34 +0200
Subject: [PATCH] Provide some telemetry values from ArduControl

---
 source/libr2c2/arducontrol.cpp | 38 +++++++++++++++++++++++++++++++++-
 source/libr2c2/arducontrol.h   |  9 +++++++-
 2 files changed, 45 insertions(+), 2 deletions(-)

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;
diff --git a/source/libr2c2/arducontrol.h b/source/libr2c2/arducontrol.h
index 292700d..a48f6f0 100644
--- 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();
-- 
2.43.0