Add two new telemetry values to the arducontrol driver

diff --git a/source/libr2c2/arducontrol.cpp b/source/libr2c2/arducontrol.cpp
index 4135df3b54585e26ccd293f7f0101d309d1676a4..7b5fc108f09bf0864776c67b0a9ce7f717edd4c7 100644 (file)
--- a/source/libr2c2/arducontrol.cpp
+++ b/source/libr2c2/arducontrol.cpp
@@ -18,11 +18,13 @@ ArduControl::ProtocolInfo ArduControl::protocol_info[2] =
        { 0x3FFF, 126, 15 }  // MFX
 };
 
        { 0x3FFF, 126, 15 }  // MFX
 };
 

-Driver::TelemetryInfo ArduControl::telemetry_info[4] =
+Driver::TelemetryInfo ArduControl::telemetry_info[6] =

 {
        { "voltage", "Voltage", "V", 1 },
        { "current", "Current", "A", 2 },
 {
        { "voltage", "Voltage", "V", 1 },
        { "current", "Current", "A", 2 },

+       { "cmd-rate", "Cmd rate", "/ s", 0 },

        { "cmd-queue-depth", "Cmd queue", "", 0 },
        { "cmd-queue-depth", "Cmd queue", "", 0 },

+       { "acc-queue-depth", "Acc queue", "", 0 },

        { "s88-latency", "S88 latency", "ms", 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
 {
 
 const Driver::TelemetryInfo *ArduControl::enumerate_telemetry(unsigned i) const
 {

-       if(i<4)
+       if(i<6)

                return telemetry_info+i;
        else
                return 0;
                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)
        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)
        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);
                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),
 
 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);
 {
        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();
 void ArduControl::ControlThread::main()
 {
        init_baud_rate();

+       cmd_rate_start = Time::now();

 
        while(!done)
        {
 
        while(!done)
        {


@@ -1262,6 +1271,14 @@ void ArduControl::ControlThread::main()
                                                control.command_queue.push(cmd);
                                }
                        }
                                                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);
                }
                else
                        Time::sleep(10*Time::msec);


@@ -1415,6 +1432,8 @@ unsigned ArduControl::ControlThread::do_command(const PendingCommand &cmd, const
                        result = r;
        }
 
                        result = r;
        }
 

+       ++cmd_count;
+

        return result;
 }
 
        return result;
 }
 

diff --git a/source/libr2c2/arducontrol.h b/source/libr2c2/arducontrol.h
index 78b360018de17c245629059b789ab6be55c9ba4d..fffaf8cc36854d691860e8064548fcd589579aac 100644 (file)
--- a/source/libr2c2/arducontrol.h
+++ b/source/libr2c2/arducontrol.h
@@ -377,10 +377,15 @@ private:
                ArduControl &control;
                bool done;
                std::vector<Task *> tasks;
                ArduControl &control;
                bool done;
                std::vector<Task *> tasks;

+               unsigned cmd_rate;
+               unsigned cmd_count;
+               Msp::Time::TimeStamp cmd_rate_start;

 
        public:
                ControlThread(ArduControl &);
 
 
        public:
                ControlThread(ArduControl &);
 

+               unsigned get_command_rate() const { return cmd_rate; }
+

                void exit();
        private:
                virtual void main();
                void exit();
        private:
                virtual void main();


@@ -425,7 +430,7 @@ private:
        ControlThread thread;
 
        static ProtocolInfo protocol_info[2];
        ControlThread thread;
 
        static ProtocolInfo protocol_info[2];

-       static TelemetryInfo telemetry_info[4];
+       static TelemetryInfo telemetry_info[6];

 
 public:
        ArduControl(const Options &);
 
 public:
        ArduControl(const Options &);