From b85fa11e45805cd71383577642030d8d01f43447 Mon Sep 17 00:00:00 2001
From: Mikko Rasa <tdb@tdb.fi>
Date: Mon, 30 Mar 2015 23:18:38 +0300
Subject: [PATCH] Add two new telemetry values to the arducontrol driver

---
 source/libr2c2/arducontrol.cpp | 27 +++++++++++++++++++++++----
 source/libr2c2/arducontrol.h   |  7 ++++++-
 2 files changed, 29 insertions(+), 5 deletions(-)

diff --git a/source/libr2c2/arducontrol.cpp b/source/libr2c2/arducontrol.cpp
index 4135df3..7b5fc10 100644
--- a/source/libr2c2/arducontrol.cpp
+++ b/source/libr2c2/arducontrol.cpp
@@ -18,11 +18,13 @@ ArduControl::ProtocolInfo ArduControl::protocol_info[2] =
 	{ 0x3FFF, 126, 15 }  // MFX
 };
 
-Driver::TelemetryInfo ArduControl::telemetry_info[4] =
+Driver::TelemetryInfo ArduControl::telemetry_info[6] =
 {
 	{ "voltage", "Voltage", "V", 1 },
 	{ "current", "Current", "A", 2 },
+	{ "cmd-rate", "Cmd rate", "/ s", 0 },
 	{ "cmd-queue-depth", "Cmd queue", "", 0 },
+	{ "acc-queue-depth", "Acc queue", "", 0 },
 	{ "s88-latency", "S88 latency", "ms", 0 }
 };
 
@@ -345,7 +347,7 @@ bool ArduControl::get_sensor(unsigned addr) const
 
 const Driver::TelemetryInfo *ArduControl::enumerate_telemetry(unsigned i) const
 {
-	if(i<4)
+	if(i<6)
 		return telemetry_info+i;
 	else
 		return 0;
@@ -358,8 +360,12 @@ float ArduControl::get_telemetry_value(const string &name) const
 	else if(name==telemetry_info[1].name)
 		return monitor.get_current();
 	else if(name==telemetry_info[2].name)
-		return command_queue.size();
+		return thread.get_command_rate();
 	else if(name==telemetry_info[3].name)
+		return command_queue.size();
+	else if(name==telemetry_info[4].name)
+		return accessory_queue.size();
+	else if(name==telemetry_info[5].name)
 		return s88.get_latency()/Time::msec;
 	else
 		throw key_error(name);
@@ -1199,7 +1205,9 @@ void ArduControl::MonitorTask::reset_peak()
 
 ArduControl::ControlThread::ControlThread(ArduControl &c):
 	control(c),
-	done(false)
+	done(false),
+	cmd_rate(20),
+	cmd_count(0)
 {
 	tasks.push_back(&control.command_queue);
 	tasks.push_back(&control.monitor);
@@ -1220,6 +1228,7 @@ void ArduControl::ControlThread::exit()
 void ArduControl::ControlThread::main()
 {
 	init_baud_rate();
+	cmd_rate_start = Time::now();
 
 	while(!done)
 	{
@@ -1262,6 +1271,14 @@ void ArduControl::ControlThread::main()
 						control.command_queue.push(cmd);
 				}
 			}
+
+			if(cmd_count>=cmd_rate)
+			{
+				Time::TimeStamp t = Time::now();
+				cmd_rate = cmd_count/((t-cmd_rate_start)/Time::sec);
+				cmd_rate_start = t;
+				cmd_count = 0;
+			}
 		}
 		else
 			Time::sleep(10*Time::msec);
@@ -1415,6 +1432,8 @@ unsigned ArduControl::ControlThread::do_command(const PendingCommand &cmd, const
 			result = r;
 	}
 
+	++cmd_count;
+
 	return result;
 }
 
diff --git a/source/libr2c2/arducontrol.h b/source/libr2c2/arducontrol.h
index 78b3600..fffaf8c 100644
--- a/source/libr2c2/arducontrol.h
+++ b/source/libr2c2/arducontrol.h
@@ -377,10 +377,15 @@ private:
 		ArduControl &control;
 		bool done;
 		std::vector<Task *> tasks;
+		unsigned cmd_rate;
+		unsigned cmd_count;
+		Msp::Time::TimeStamp cmd_rate_start;
 
 	public:
 		ControlThread(ArduControl &);
 
+		unsigned get_command_rate() const { return cmd_rate; }
+
 		void exit();
 	private:
 		virtual void main();
@@ -425,7 +430,7 @@ private:
 	ControlThread thread;
 
 	static ProtocolInfo protocol_info[2];
-	static TelemetryInfo telemetry_info[4];
+	static TelemetryInfo telemetry_info[6];
 
 public:
 	ArduControl(const Options &);
-- 
2.45.2