1 #include <msp/core/maputils.h>
2 #include <msp/io/print.h>
3 #include <msp/time/utils.h>
4 #include "arducontrol.h"
12 ArduControl::ProtocolInfo ArduControl::protocol_info[2] =
15 { 0x3FFF, 126, 15 } // MFX
18 ArduControl::ArduControl(const string &dev):
22 next_refresh(refresh_cycle.end()),
26 mfx_announce_serial(0),
31 cmd.command[0] = READ_POWER_STATE;
35 cmd.command[0] = MFX_SET_STATION_ID;
44 ArduControl::~ArduControl()
49 void ArduControl::set_power(bool p)
53 PendingCommand cmd(POWER);
54 cmd.tag.serial = power.serial;
55 cmd.command[0] = (p ? POWER_ON : POWER_OFF);
61 void ArduControl::halt(bool)
65 const char *ArduControl::enumerate_protocols(unsigned i) const
75 ArduControl::Protocol ArduControl::map_protocol(const string &proto_name)
79 else if(proto_name=="MFX")
82 throw invalid_argument("ArduControl::map_protocol");
85 unsigned ArduControl::get_protocol_speed_steps(const string &proto_name) const
87 return protocol_info[map_protocol(proto_name)].max_speed;
90 unsigned ArduControl::add_loco(unsigned addr, const string &proto_name, const VehicleType &)
93 throw invalid_argument("ArduControl::add_loco");
95 Protocol proto = map_protocol(proto_name);
96 if(addr>protocol_info[proto].max_address)
97 throw invalid_argument("ArduControl::add_loco");
99 Locomotive loco(proto, addr);
100 insert_unique(locomotives, loco.id, loco);
105 void ArduControl::remove_loco(unsigned id)
107 Locomotive &loco = get_item(locomotives, id);
108 remove_loco_from_refresh(loco);
109 locomotives.erase(id);
112 void ArduControl::set_loco_speed(unsigned id, unsigned speed)
114 Locomotive &loco = get_item(locomotives, id);
115 if(speed>protocol_info[loco.proto].max_speed)
116 throw invalid_argument("ArduControl::set_loco_speed");
118 if(loco.speed.set(speed))
120 PendingCommand cmd(loco, Locomotive::SPEED);
123 add_loco_to_refresh(loco);
127 void ArduControl::set_loco_reverse(unsigned id, bool rev)
129 Locomotive &loco = get_item(locomotives, id);
130 if(loco.reverse.set(rev))
132 PendingCommand cmd(loco, Locomotive::REVERSE);
135 add_loco_to_refresh(loco);
139 void ArduControl::set_loco_function(unsigned id, unsigned func, bool state)
141 Locomotive &loco = get_item(locomotives, id);
142 if(func>protocol_info[loco.proto].max_func)
143 throw invalid_argument("ArduControl::set_loco_function");
145 unsigned mask = 1<<func;
146 if(loco.funcs.set((loco.funcs&~mask)|(mask*state)))
148 if(func>0 || loco.proto!=MM)
150 PendingCommand cmd(loco, Locomotive::FUNCTIONS, func);
154 add_loco_to_refresh(loco);
158 void ArduControl::add_loco_to_refresh(Locomotive &loco)
160 MutexLock lock(mutex);
161 refresh_cycle.push_back(&loco);
162 if(refresh_cycle.size()>15)
164 LocomotivePtrList::iterator oldest = refresh_cycle.begin();
165 for(LocomotivePtrList::iterator i=refresh_cycle.begin(); ++i!=refresh_cycle.end(); )
166 if((*i)->last_change_age>(*oldest)->last_change_age)
168 if(oldest==next_refresh)
169 advance_next_refresh();
170 refresh_cycle.erase(oldest);
172 if(next_refresh==refresh_cycle.end())
173 next_refresh = refresh_cycle.begin();
176 void ArduControl::remove_loco_from_refresh(Locomotive &loco)
178 MutexLock lock(mutex);
179 for(LocomotivePtrList::iterator i=refresh_cycle.begin(); i!=refresh_cycle.end(); ++i)
184 if(refresh_cycle.size()>1)
185 advance_next_refresh();
187 next_refresh = refresh_cycle.end();
189 refresh_cycle.erase(i);
194 ArduControl::Locomotive *ArduControl::get_loco_to_refresh()
196 MutexLock lock(mutex);
197 if(refresh_cycle.empty())
200 Locomotive *loco = *next_refresh;
201 advance_next_refresh();
205 void ArduControl::advance_next_refresh()
208 if(next_refresh==refresh_cycle.end())
210 next_refresh = refresh_cycle.begin();
215 unsigned ArduControl::add_turnout(unsigned addr, const TrackType &type)
217 if(!addr || !type.is_turnout())
218 throw invalid_argument("ArduControl::add_turnout");
220 return add_accessory(Accessory::TURNOUT, addr, type.get_state_bits());
223 void ArduControl::remove_turnout(unsigned addr)
225 remove_accessory(Accessory::TURNOUT, addr);
228 void ArduControl::set_turnout(unsigned addr, unsigned state)
230 set_accessory(Accessory::TURNOUT, addr, state);
233 unsigned ArduControl::get_turnout(unsigned addr) const
235 return get_accessory(Accessory::TURNOUT, addr);
238 unsigned ArduControl::add_signal(unsigned addr, const SignalType &)
240 return add_accessory(Accessory::SIGNAL, addr, 1);
243 void ArduControl::remove_signal(unsigned addr)
245 remove_accessory(Accessory::SIGNAL, addr);
248 void ArduControl::set_signal(unsigned addr, unsigned state)
250 set_accessory(Accessory::SIGNAL, addr, state);
253 unsigned ArduControl::get_signal(unsigned addr) const
255 return get_accessory(Accessory::SIGNAL, addr);
258 unsigned ArduControl::add_accessory(Accessory::Kind kind, unsigned addr, unsigned bits)
260 AccessoryMap::iterator i = accessories.lower_bound(addr);
261 AccessoryMap::iterator j = accessories.upper_bound(addr+bits-1);
263 throw key_error(addr);
264 if(i!=accessories.begin())
267 if(i->first+i->second.bits>addr)
268 throw key_error(addr);
271 insert_unique(accessories, addr, Accessory(kind, addr, bits));
275 void ArduControl::remove_accessory(Accessory::Kind kind, unsigned addr)
277 Accessory &acc = get_item(accessories, addr);
279 throw key_error(addr);
280 accessories.erase(addr);
283 void ArduControl::set_accessory(Accessory::Kind kind, unsigned addr, unsigned state)
285 Accessory &acc = get_item(accessories, addr);
287 throw key_error(addr);
289 if(state!=acc.target)
292 accessory_queue.push_back(&acc);
296 unsigned ArduControl::get_accessory(Accessory::Kind kind, unsigned addr) const
298 const Accessory &acc = get_item(accessories, addr);
300 throw key_error(addr);
304 unsigned ArduControl::add_sensor(unsigned addr)
307 throw invalid_argument("ArduControl::add_sensor");
309 insert_unique(sensors, addr, Sensor(addr));
310 unsigned octet_index = (addr-1)/8;
311 if(octet_index>=n_s88_octets)
312 n_s88_octets = octet_index+1;
317 void ArduControl::remove_sensor(unsigned addr)
319 remove_existing(sensors, addr);
320 // TODO update n_s88_octets
323 bool ArduControl::get_sensor(unsigned addr) const
325 return get_item(sensors, addr).state;
328 void ArduControl::tick()
330 while(Tag tag = pop_completed_tag())
332 if(tag.type==Tag::GENERAL)
334 if(tag.command==POWER)
336 if(power.commit(tag.serial))
337 signal_power.emit(power.current);
340 else if(tag.type==Tag::LOCOMOTIVE)
342 LocomotiveMap::iterator i = locomotives.find(tag.id);
343 if(i==locomotives.end())
346 Locomotive &loco = i->second;
347 if(tag.command==Locomotive::SPEED)
349 if(loco.speed.commit(tag.serial))
350 signal_loco_speed.emit(loco.id, loco.speed, loco.reverse);
352 else if(tag.command==Locomotive::REVERSE)
354 if(loco.reverse.commit(tag.serial))
355 signal_loco_speed.emit(loco.id, loco.speed, loco.reverse);
357 else if(tag.command==Locomotive::FUNCTIONS)
359 unsigned old = loco.funcs;
360 if(loco.funcs.commit(tag.serial))
362 unsigned changed = old^loco.funcs;
363 for(unsigned j=0; changed>>j; ++j)
365 signal_loco_function.emit(loco.id, j, (loco.funcs>>j)&1);
369 else if(tag.type==Tag::ACCESSORY)
371 AccessoryMap::iterator i = accessories.find(tag.id);
372 if(i==accessories.end())
375 Accessory &acc = i->second;
376 if(tag.command==Accessory::ACTIVATE)
378 off_timeout = Time::now()+acc.active_time;
380 else if(tag.command==Accessory::DEACTIVATE)
382 if(acc.state.commit(tag.serial))
384 if(acc.state==acc.target)
386 if(acc.kind==Accessory::TURNOUT)
387 signal_turnout.emit(acc.address, acc.state);
388 else if(acc.kind==Accessory::SIGNAL)
389 signal_signal.emit(acc.address, acc.state);
391 if(&acc==active_accessory)
392 active_accessory = 0;
396 else if(tag.type==Tag::SENSOR)
398 SensorMap::iterator i = sensors.find(tag.id);
402 Sensor &sensor = i->second;
403 if(tag.command==Sensor::STATE)
405 if(sensor.state.commit(tag.serial))
406 signal_sensor.emit(sensor.address, sensor.state);
411 while(!active_accessory && !accessory_queue.empty())
413 Accessory &acc = *accessory_queue.front();
415 if(acc.state!=acc.target)
417 active_accessory = &acc;
419 unsigned changes = acc.state^acc.target;
420 unsigned lowest_bit = changes&~(changes-1);
422 for(i=0; (lowest_bit>>i)>1; ++i) ;
423 acc.state.set(acc.state^lowest_bit);
424 PendingCommand cmd(acc, Accessory::ACTIVATE, i);
428 accessory_queue.pop_front();
431 if(active_accessory && off_timeout)
433 Time::TimeStamp t = Time::now();
436 off_timeout = Time::TimeStamp();
437 PendingCommand cmd(*active_accessory, Accessory::DEACTIVATE);
443 void ArduControl::flush()
447 void ArduControl::push_command(const PendingCommand &cmd)
449 MutexLock lock(mutex);
450 command_queue.push_back(cmd);
453 bool ArduControl::pop_command(PendingCommand &cmd)
455 MutexLock lock(mutex);
456 if(command_queue.empty())
458 cmd = command_queue.front();
459 command_queue.pop_front();
463 void ArduControl::push_completed_tag(const Tag &tag)
465 MutexLock lock(mutex);
466 completed_commands.push_back(tag);
469 ArduControl::Tag ArduControl::pop_completed_tag()
471 MutexLock lock(mutex);
472 if(completed_commands.empty())
474 Tag tag = completed_commands.front();
475 completed_commands.pop_front();
480 ArduControl::Tag::Tag():
488 ArduControl::Locomotive::Locomotive(Protocol p, unsigned a):
498 unsigned ArduControl::Locomotive::create_speed_dir_command(char *buffer) const
502 buffer[0] = MOTOROLA_SPEED_DIRECTION;
504 buffer[2] = funcs.pending&1;
505 buffer[3] = speed.pending+reverse.pending*0x80;
510 buffer[0] = MFX_SPEED;
511 buffer[1] = address>>8;
513 buffer[3] = speed.pending+reverse.pending*0x80;
520 unsigned ArduControl::Locomotive::create_speed_func_command(unsigned f, char *buffer) const
525 throw invalid_argument("Locomotive::create_speed_func_command");
527 buffer[0] = MOTOROLA_SPEED_FUNCTION;
529 buffer[2] = (f<<4)|(((funcs.pending>>f)&1)<<1)|(funcs.pending&1);
530 buffer[3] = speed.pending;
535 bool f16 = (funcs.pending>0xFF);
536 buffer[0] = (f16 ? MFX_SPEED_FUNCS16 : MFX_SPEED_FUNCS8);
537 buffer[1] = address>>8;
539 buffer[3] = speed.pending+reverse.pending*0x80;
542 buffer[4] = funcs.pending>>8;
543 buffer[5] = funcs.pending;
548 buffer[4] = funcs.pending;
557 ArduControl::Accessory::Accessory(Kind k, unsigned a, unsigned b):
562 active_time(500*Time::msec)
565 unsigned ArduControl::Accessory::create_state_command(unsigned b, bool c, char *buffer) const
568 throw invalid_argument("Accessory::create_state_command");
570 unsigned a = (address+b+3)*2;
571 if(!((state.pending>>b)&1))
573 buffer[0] = MOTOROLA_SOLENOID;
575 buffer[2] = ((a&7)<<4)|c;
580 ArduControl::Sensor::Sensor(unsigned a):
586 ArduControl::PendingCommand::PendingCommand():
591 ArduControl::PendingCommand::PendingCommand(GeneralCommand cmd):
595 tag.type = Tag::GENERAL;
599 ArduControl::PendingCommand::PendingCommand(Locomotive &loco, Locomotive::Command cmd, unsigned index):
602 tag.type = Tag::LOCOMOTIVE;
605 if(cmd==Locomotive::SPEED)
607 tag.serial = loco.speed.serial;
608 length = loco.create_speed_dir_command(command);
610 else if(cmd==Locomotive::REVERSE)
612 tag.serial = loco.reverse.serial;
613 length = loco.create_speed_dir_command(command);
615 else if(cmd==Locomotive::FUNCTIONS)
617 tag.serial = loco.funcs.serial;
618 length = loco.create_speed_func_command(index, command);
621 throw invalid_argument("PendingCommand");
624 ArduControl::PendingCommand::PendingCommand(Accessory &acc, Accessory::Command cmd, unsigned index):
627 tag.type = Tag::ACCESSORY;
629 tag.id = acc.address;
630 if(cmd==Accessory::ACTIVATE || cmd==Accessory::DEACTIVATE)
632 tag.serial = acc.state.serial;
633 length = acc.create_state_command(index, (cmd==Accessory::ACTIVATE), command);
636 throw invalid_argument("PendingCommand");
640 ArduControl::RefreshTask::RefreshTask(ArduControl &c):
646 bool ArduControl::RefreshTask::get_work(PendingCommand &cmd)
648 if(loco && loco->proto==MM && phase==0)
650 cmd.length = loco->create_speed_func_command(control.refresh_counter%4+1, cmd.command);
651 cmd.repeat_count = 2;
656 loco = control.get_loco_to_refresh();
663 cmd.length = loco->create_speed_dir_command(cmd.command);
664 cmd.repeat_count = 2;
666 else if(loco->proto==MFX)
667 cmd.length = loco->create_speed_func_command(0, cmd.command);
675 ArduControl::S88Task::S88Task(ArduControl &c):
680 bool ArduControl::S88Task::get_work(PendingCommand &cmd)
682 if(octets_remaining || !control.n_s88_octets)
685 cmd.command[0] = S88_READ;
686 cmd.command[1] = control.n_s88_octets;
688 octets_remaining = control.n_s88_octets;
693 void ArduControl::S88Task::process_reply(const char *reply, unsigned length)
695 unsigned char type = reply[0];
696 if(type==S88_DATA && length>2)
698 unsigned offset = static_cast<unsigned char>(reply[1]);
699 unsigned count = length-2;
701 SensorMap::iterator begin = control.sensors.lower_bound(offset*8+1);
702 SensorMap::iterator end = control.sensors.upper_bound((offset+count)*8);
703 for(SensorMap::iterator i=begin; i!=end; ++i)
705 unsigned bit_index = i->first-1-offset*8;
706 bool state = (reply[2+bit_index/8]>>(7-bit_index%8))&1;
707 i->second.state.set(state);
710 tag.type = Tag::SENSOR;
711 tag.command = Sensor::STATE;
712 tag.serial = i->second.state.serial;
714 control.push_completed_tag(tag);
717 if(count>octets_remaining)
718 octets_remaining = 0;
720 octets_remaining -= count;
725 ArduControl::MfxAnnounceTask::MfxAnnounceTask(ArduControl &c):
729 bool ArduControl::MfxAnnounceTask::get_work(PendingCommand &cmd)
731 Time::TimeStamp t = Time::now();
735 cmd.command[0] = MFX_ANNOUNCE;
736 cmd.command[1] = control.mfx_announce_serial>>8;
737 cmd.command[2] = control.mfx_announce_serial;
739 next = t+400*Time::msec;
745 ArduControl::MfxSearchTask::MfxSearchTask(ArduControl &c):
752 bool ArduControl::MfxSearchTask::get_work(PendingCommand &cmd)
757 IO::print("Assigning MFX address %d to decoder %08X\n", control.next_mfx_address, bits);
759 cmd.command[0] = MFX_ASSIGN_ADDRESS;
760 cmd.command[1] = control.next_mfx_address>>8;
761 cmd.command[2] = control.next_mfx_address;
762 for(unsigned i=0; i<4; ++i)
763 cmd.command[3+i] = bits>>(24-i*8);
768 ++control.next_mfx_address;
773 Time::TimeStamp t = Time::now();
777 cmd.command[0] = MFX_SEARCH;
778 for(unsigned i=0; i<4; ++i)
779 cmd.command[1+i] = bits>>(24-i*8);
780 cmd.command[5] = size;
783 next = t+200*Time::msec;
787 IO::print("Search %08X/%d\n", bits, size);
792 void ArduControl::MfxSearchTask::process_reply(const char *reply, unsigned length)
794 unsigned char type = reply[0];
795 if(type==MFX_FEEDBACK && length==2 && pending)
798 bool finished = true;
807 unsigned mask = 1<<(32-size);
817 next = Time::now()+2*Time::sec;
825 ArduControl::ControlThread::ControlThread(ArduControl &c):
829 tasks.push_back(new MfxAnnounceTask(control));
830 tasks.push_back(new MfxSearchTask(control));
831 tasks.push_back(new S88Task(control));
832 tasks.push_back(new RefreshTask(control));
837 void ArduControl::ControlThread::exit()
843 void ArduControl::ControlThread::main()
851 for(unsigned i=0; (success && i<cmd.repeat_count); ++i)
852 success = (do_command(cmd)==COMMAND_OK);
853 if(success && cmd.tag)
854 control.push_completed_tag(cmd.tag);
857 Time::sleep(10*Time::msec);
861 bool ArduControl::ControlThread::get_work(PendingCommand &cmd)
863 if(control.pop_command(cmd))
866 for(vector<Task *>::iterator i=tasks.begin(); i!=tasks.end(); ++i)
867 if((*i)->get_work(cmd))
870 // As fallback, send an idle packet for the MM protocol
871 cmd.command[0] = MOTOROLA_SPEED;
880 unsigned ArduControl::ControlThread::do_command(const PendingCommand &cmd)
885 for(unsigned i=0; i<cmd.length; ++i)
886 cmd_hex += format(" %02X", static_cast<unsigned char>(cmd.command[i]));
887 IO::print("< %02X%s\n", cmd.length^0xFF, cmd_hex);
890 control.serial.put(cmd.length^0xFF);
891 control.serial.write(cmd.command, cmd.length);
898 got_data = IO::poll(control.serial, IO::P_INPUT, Time::zero);
900 got_data = IO::poll(control.serial, IO::P_INPUT);
905 unsigned rlength = control.serial.get()^0xFF;
908 IO::print("Invalid length %02X\n", rlength);
915 pos += control.serial.read(reply+pos, rlength-pos);
920 for(unsigned i=0; i<rlength; ++i)
921 reply_hex += format(" %02X", static_cast<unsigned char>(reply[i]));
922 IO::print("> %02X%s\n", rlength^0xFF, reply_hex);
925 unsigned r = process_reply(reply, rlength);
933 unsigned ArduControl::ControlThread::process_reply(const char *reply, unsigned rlength)
935 unsigned char type = reply[0];
936 if((type&0xE0)==0x80)
939 IO::print("Error %02X\n", type);
942 else if(type==POWER_STATE && rlength==2)
944 control.power.set(reply[1]);
947 tag.type = Tag::GENERAL;
949 tag.serial = control.power.serial;
950 control.push_completed_tag(tag);
954 for(vector<Task *>::iterator i=tasks.begin(); i!=tasks.end(); ++i)
955 (*i)->process_reply(reply, rlength);