]> git.tdb.fi Git - r2c2.git/blobdiff - source/libr2c2/arducontrol.cpp
Use a common implementation for the three different queues
[r2c2.git] / source / libr2c2 / arducontrol.cpp
index 201d9a2ebdd73e7871527c2f09c87b3a7f29cba2..e29614f6646a5c5fd3068d819c7e89c071a0c154 100644 (file)
@@ -27,7 +27,7 @@ ArduControl::ArduControl(const string &dev):
        PendingCommand cmd;
        cmd.command[0] = READ_POWER_STATE;
        cmd.length = 1;
-       push_command(cmd);
+       command_queue.push(cmd);
 
        cmd.command[0] = MFX_SET_STATION_ID;
        cmd.command[1] = 'R';
@@ -35,7 +35,7 @@ ArduControl::ArduControl(const string &dev):
        cmd.command[3] = 'C';
        cmd.command[4] = '2';
        cmd.length = 5;
-       push_command(cmd);
+       command_queue.push(cmd);
 }
 
 ArduControl::~ArduControl()
@@ -51,7 +51,7 @@ void ArduControl::set_power(bool p)
                cmd.tag.serial = power.serial;
                cmd.command[0] = (p ? POWER_ON : POWER_OFF);
                cmd.length = 1;
-               push_command(cmd);
+               command_queue.push(cmd);
        }
 }
 
@@ -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)
@@ -115,7 +123,7 @@ void ArduControl::set_loco_speed(unsigned id, unsigned speed)
        if(loco.speed.set(speed))
        {
                PendingCommand cmd(loco, Locomotive::SPEED);
-               push_command(cmd);
+               command_queue.push(cmd);
 
                refresh.add_loco(loco);
        }
@@ -127,7 +135,7 @@ void ArduControl::set_loco_reverse(unsigned id, bool rev)
        if(loco.reverse.set(rev))
        {
                PendingCommand cmd(loco, Locomotive::REVERSE);
-               push_command(cmd);
+               command_queue.push(cmd);
 
                refresh.add_loco(loco);
        }
@@ -145,7 +153,7 @@ void ArduControl::set_loco_function(unsigned id, unsigned func, bool state)
                if(func>0 || loco.proto!=MM)
                {
                        PendingCommand cmd(loco, Locomotive::FUNCTIONS, func);
-                       push_command(cmd);
+                       command_queue.push(cmd);
                }
 
                refresh.add_loco(loco);
@@ -265,7 +273,8 @@ bool ArduControl::get_sensor(unsigned addr) const
 
 void ArduControl::tick()
 {
-       while(Tag tag = pop_completed_tag())
+       Tag tag;
+       while(completed_commands.pop(tag))
        {
                if(tag.type==Tag::GENERAL)
                {
@@ -274,6 +283,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)
                {
@@ -360,7 +386,7 @@ void ArduControl::tick()
                        for(i=0; (lowest_bit>>i)>1; ++i) ;
                        acc.state.set(acc.state^lowest_bit);
                        PendingCommand cmd(acc, Accessory::ACTIVATE, i);
-                       push_command(cmd);
+                       command_queue.push(cmd);
                }
                else
                        accessory_queue.pop_front();
@@ -373,7 +399,7 @@ void ArduControl::tick()
                {
                        off_timeout = Time::TimeStamp();
                        PendingCommand cmd(*active_accessory, Accessory::DEACTIVATE);
-                       push_command(cmd);
+                       command_queue.push(cmd);
                }
        }
 }
@@ -382,38 +408,6 @@ void ArduControl::flush()
 {
 }
 
-void ArduControl::push_command(const PendingCommand &cmd)
-{
-       MutexLock lock(mutex);
-       command_queue.push_back(cmd);
-}
-
-bool ArduControl::pop_command(PendingCommand &cmd)
-{
-       MutexLock lock(mutex);
-       if(command_queue.empty())
-               return false;
-       cmd = command_queue.front();
-       command_queue.pop_front();
-       return true;
-}
-
-void ArduControl::push_completed_tag(const Tag &tag)
-{
-       MutexLock lock(mutex);
-       completed_commands.push_back(tag);
-}
-
-ArduControl::Tag ArduControl::pop_completed_tag()
-{
-       MutexLock lock(mutex);
-       if(completed_commands.empty())
-               return Tag();
-       Tag tag = completed_commands.front();
-       completed_commands.pop_front();
-       return tag;
-}
-
 
 ArduControl::Tag::Tag():
        type(NONE),
@@ -575,6 +569,26 @@ ArduControl::PendingCommand::PendingCommand(Accessory &acc, Accessory::Command c
 }
 
 
+template<typename T>
+void ArduControl::Queue<T>::push(const T &item)
+{
+       MutexLock lock(mutex);
+       items.push_back(item);
+}
+
+template<typename T>
+bool ArduControl::Queue<T>::pop(T &item)
+{
+       MutexLock lock(mutex);
+       if(items.empty())
+               return false;
+
+       item = items.front();
+       items.pop_front();
+       return true;
+}
+
+
 ArduControl::RefreshTask::RefreshTask():
        next(cycle.end()),
        round(0),
@@ -708,7 +722,7 @@ void ArduControl::S88Task::process_reply(const char *reply, unsigned length)
                        tag.command = Sensor::STATE;
                        tag.serial = i->second.state.serial;
                        tag.id = i->first;
-                       control.push_completed_tag(tag);
+                       control.completed_commands.push(tag);
                }
 
                if(count>octets_remaining)
@@ -760,16 +774,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;
+               queue.push(info);
+
                cmd.command[0] = MFX_ASSIGN_ADDRESS;
                cmd.command[1] = next_address>>8;
                cmd.command[2] = next_address;
@@ -777,6 +798,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 +820,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 +830,33 @@ 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;
                }
        }
 }
 
+bool ArduControl::MfxSearchTask::pop_info(MfxInfo &info)
+{
+       return queue.pop(info);
+}
+
 
 ArduControl::ControlThread::ControlThread(ArduControl &c):
        control(c),
@@ -856,6 +878,8 @@ void ArduControl::ControlThread::exit()
 
 void ArduControl::ControlThread::main()
 {
+       init_baud_rate();
+
        while(!done)
        {
                PendingCommand cmd;
@@ -865,16 +889,67 @@ void ArduControl::ControlThread::main()
                        for(unsigned i=0; (success && i<cmd.repeat_count); ++i)
                                success = (do_command(cmd)==COMMAND_OK);
                        if(success && cmd.tag)
-                               control.push_completed_tag(cmd.tag);
+                               control.completed_commands.push(cmd.tag);
                }
                else
                        Time::sleep(10*Time::msec);
        }
 }
 
+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(!rate)
+       {
+               done = true;
+               return;
+       }
+
+       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]);
+                       Time::sleep(Time::sec);
+                       if(do_command(cmd)==COMMAND_OK)
+                       {
+                               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))
+       if(control.command_queue.pop(cmd))
                return true;
 
        for(vector<Task *>::iterator i=tasks.begin(); i!=tasks.end(); ++i)
@@ -961,7 +1036,7 @@ unsigned ArduControl::ControlThread::process_reply(const char *reply, unsigned r
                tag.type = Tag::GENERAL;
                tag.command = POWER;
                tag.serial = control.power.serial;
-               control.push_completed_tag(tag);
+               control.completed_commands.push(tag);
        }
        else
        {