From: Mikko Rasa Date: Mon, 30 Mar 2015 20:18:38 +0000 (+0300) Subject: Add two new telemetry values to the arducontrol driver X-Git-Url: http://git.tdb.fi/?a=commitdiff_plain;h=b85fa11e45805cd71383577642030d8d01f43447;p=r2c2.git Add two new telemetry values to the arducontrol driver --- 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 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 &);