Store and export information about detected MFX locomotives

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

diff --git a/source/libr2c2/arducontrol.cpp b/source/libr2c2/arducontrol.cpp
index 201d9a2ebdd73e7871527c2f09c87b3a7f29cba2..47584f44bb8b145496a0b6d9a52c118f4c68fcd9 100644 (file)
--- a/source/libr2c2/arducontrol.cpp
+++ b/source/libr2c2/arducontrol.cpp
@@ -84,6 +84,14 @@ unsigned ArduControl::get_protocol_speed_steps(const string &proto_name) const
        return protocol_info[map_protocol(proto_name)].max_speed;
 }
 
+const Driver::DetectedLocomotive *ArduControl::enumerate_detected_locos(unsigned i) const
+{
+       if(i>=mfx_info.size())
+               return 0;
+
+       return &mfx_info[i];
+}
+
 unsigned ArduControl::add_loco(unsigned addr, const string &proto_name, const VehicleType &)
 {
        if(!addr)
@@ -274,6 +282,23 @@ void ArduControl::tick()
                                if(power.commit(tag.serial))
                                        signal_power.emit(power.current);
                        }
+                       else if(tag.command==NEW_LOCO)
+                       {
+                               MfxInfo info;
+                               if(mfx_search.pop_info(info))
+                               {
+                                       MfxInfoArray::iterator i;
+                                       for(i=mfx_info.begin(); (i!=mfx_info.end() && i->id!=info.id); ++i) ;
+                                       if(i==mfx_info.end())
+                                       {
+                                               mfx_info.push_back(info);
+                                               i = --mfx_info.end();
+                                       }
+                                       else
+                                               *i = info;
+                                       signal_locomotive_detected.emit(*i);
+                               }
+                       }
                }
                else if(tag.type==Tag::LOCOMOTIVE)
                {
@@ -760,16 +785,23 @@ ArduControl::MfxSearchTask::MfxSearchTask(ArduControl &c):
        next_address(1),
        size(0),
        bits(0),
-       pending(false)
+       misses(0)
 { }
 
 bool ArduControl::MfxSearchTask::get_work(PendingCommand &cmd)
 {
-       if(size==32)
+       if(size>32)
        {
                if(control.debug>=1)
                        IO::print("Assigning MFX address %d to decoder %08X\n", next_address, bits);
 
+               MfxInfo info;
+               info.protocol = "MFX";
+               info.address = next_address;
+               info.name = format("%08X", bits);
+               info.id = bits;
+               push_info(info);
+
                cmd.command[0] = MFX_ASSIGN_ADDRESS;
                cmd.command[1] = next_address>>8;
                cmd.command[2] = next_address;
@@ -777,6 +809,10 @@ bool ArduControl::MfxSearchTask::get_work(PendingCommand &cmd)
                        cmd.command[3+i] = bits>>(24-i*8);
                cmd.length = 7;
 
+               cmd.tag.type = Tag::GENERAL;
+               cmd.tag.command = NEW_LOCO;
+               cmd.tag.id = bits;
+
                size = 0;
                bits = 0;
                ++next_address;
@@ -795,7 +831,6 @@ bool ArduControl::MfxSearchTask::get_work(PendingCommand &cmd)
        cmd.length = 6;
 
        next = t+200*Time::msec;
-       pending = true;
 
        if(control.debug>=1)
                IO::print("Search %08X/%d\n", bits, size);
@@ -806,35 +841,44 @@ bool ArduControl::MfxSearchTask::get_work(PendingCommand &cmd)
 void ArduControl::MfxSearchTask::process_reply(const char *reply, unsigned length)
 {
        unsigned char type = reply[0];
-       if(type==MFX_FEEDBACK && length==2 && pending)
+       if(type==MFX_SEARCH_FEEDBACK && length==2)
        {
-               pending = false;
-               bool finished = true;
                if(reply[1])
                {
-                       if(size<32)
-                               ++size;
-                       finished = false;
+                       misses = 0;
+                       ++size;
                }
-               else if(size>0)
+               else if(size>0 && misses<6)
                {
-                       unsigned mask = 1<<(32-size);
-                       if(!(bits&mask))
-                       {
-                               bits |= mask;
-                               finished = false;
-                       }
+                       ++misses;
+                       bits ^= 1<<(32-size);
                }
-
-               if(finished)
+               else
                {
                        next = Time::now()+2*Time::sec;
                        bits = 0;
                        size = 0;
+                       misses = 0;
                }
        }
 }
 
+void ArduControl::MfxSearchTask::push_info(const MfxInfo &info)
+{
+       MutexLock lock(mutex);
+       queue.push_back(info);
+}
+
+bool ArduControl::MfxSearchTask::pop_info(MfxInfo &info)
+{
+       MutexLock lock(mutex);
+       if(queue.empty())
+               return false;
+       info = queue.back();
+       queue.pop_back();
+       return true;
+}
+
 
 ArduControl::ControlThread::ControlThread(ArduControl &c):
        control(c),
@@ -856,6 +900,8 @@ void ArduControl::ControlThread::exit()
 
 void ArduControl::ControlThread::main()
 {
+       init_baud_rate();
+
        while(!done)
        {
                PendingCommand cmd;
@@ -872,6 +918,51 @@ void ArduControl::ControlThread::main()
        }
 }
 
+void ArduControl::ControlThread::init_baud_rate()
+{
+       static unsigned rates[] = { 57600, 9600, 19200, 38400, 0 };
+       unsigned rate = 0;
+       control.serial.set_data_bits(8);
+       control.serial.set_parity(IO::Serial::NONE);
+       control.serial.set_stop_bits(1);
+       for(unsigned i=0; rates[i]; ++i)
+       {
+               control.serial.set_baud_rate(rates[i]);
+               control.serial.put('\xFF');
+               if(IO::poll(control.serial, IO::P_INPUT, 500*Time::msec))
+               {
+                       int c = control.serial.get();
+                       if(c==0xFF)
+                       {
+                               rate = rates[i];
+                               break;
+                       }
+               }
+       }
+
+       if(control.debug>=1)
+               IO::print("ArduControl detected at %d bits/s\n", rate);
+
+       if(rate!=rates[0])
+       {
+               PendingCommand cmd;
+               cmd.command[0] = SET_BAUD_RATE;
+               cmd.command[1] = rates[0]>>8;
+               cmd.command[2] = rates[0];
+               cmd.length = 3;
+               if(do_command(cmd)==COMMAND_OK)
+               {
+                       control.serial.set_baud_rate(rates[0]);
+                       if(do_command(cmd)==COMMAND_OK)
+                       {
+                               Time::sleep(Time::sec);
+                               if(control.debug>=1)
+                                       IO::print("Rate changed to %d bits/s\n", rates[0]);
+                       }
+               }
+       }
+}
+
 bool ArduControl::ControlThread::get_work(PendingCommand &cmd)
 {
        if(control.pop_command(cmd))