Add two new telemetry values to the arducontrol driver

[r2c2.git] / source / libr2c2 / arducontrol.cpp

diff --git a/source/libr2c2/arducontrol.cpp b/source/libr2c2/arducontrol.cpp
index 3d49fabf4784c6b02ac278d67d4086b5f6d0e918..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
 };
 
-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 }
 };
 
@@ -154,6 +156,14 @@ ArduControl::MfxInfoArray::iterator ArduControl::add_mfx_info(const MfxInfo &inf
        return i;
 }
 
+ArduControl::MfxInfo *ArduControl::find_mfx_info(unsigned id)
+{
+       for(MfxInfoArray::iterator i=mfx_info.begin(); i!=mfx_info.end(); ++i)
+               if(i->id==id)
+                       return &*i;
+       return 0;
+}
+
 void ArduControl::remove_loco(unsigned id)
 {
        Locomotive &loco = get_item(locomotives, id);
@@ -337,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;
@@ -350,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);
@@ -1002,18 +1016,24 @@ bool ArduControl::MfxSearchTask::get_work(PendingCommand &cmd)
 
        if(size>32)
        {
+               unsigned address = 0;
+               if(MfxInfo *existing = control.find_mfx_info(bits))
+                       address = existing->address;
+               else
+                       address = next_address++;
+
                if(control.debug>=1)
-                       IO::print("Assigning MFX address %d to decoder %08X\n", next_address, bits);
+                       IO::print("Assigning MFX address %d to decoder %08X\n", address, bits);
 
                pending_info = new MfxInfo;
                pending_info->protocol = "MFX";
-               pending_info->address = next_address;
+               pending_info->address = address;
                pending_info->name = format("%08X", bits);
                pending_info->id = bits;
 
                cmd.command[0] = MFX_ASSIGN_ADDRESS;
-               cmd.command[1] = next_address>>8;
-               cmd.command[2] = next_address;
+               cmd.command[1] = address>>8;
+               cmd.command[2] = address;
                for(unsigned i=0; i<4; ++i)
                        cmd.command[3+i] = bits>>(24-i*8);
                cmd.length = 7;
@@ -1021,7 +1041,6 @@ bool ArduControl::MfxSearchTask::get_work(PendingCommand &cmd)
                size = 0;
                bits = 0;
                misses = 0;
-               ++next_address;
 
                read_array = 0;
                read_offset = 0;
@@ -1186,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);
@@ -1207,6 +1228,7 @@ void ArduControl::ControlThread::exit()
 void ArduControl::ControlThread::main()
 {
        init_baud_rate();
+       cmd_rate_start = Time::now();
 
        while(!done)
        {
@@ -1249,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);
@@ -1402,6 +1432,8 @@ unsigned ArduControl::ControlThread::do_command(const PendingCommand &cmd, const
                        result = r;
        }
 
+       ++cmd_count;
+
        return result;
 }