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] =
17 ArduControl::ArduControl(const string &dev):
21 next_refresh(refresh_cycle.end()),
28 cmd.command[0] = READ_POWER_STATE;
33 ArduControl::~ArduControl()
38 void ArduControl::set_power(bool p)
42 QueuedCommand cmd(POWER);
43 cmd.tag.serial = power.serial;
44 cmd.command[0] = (p ? POWER_ON : POWER_OFF);
50 void ArduControl::halt(bool)
54 const char *ArduControl::enumerate_protocols(unsigned i) const
62 ArduControl::Protocol ArduControl::map_protocol(const string &proto_name)
66 else if(proto_name=="MFX")
69 throw invalid_argument("ArduControl::map_protocol");
72 unsigned ArduControl::get_protocol_speed_steps(const string &proto_name) const
74 return protocol_info[map_protocol(proto_name)].max_speed;
77 unsigned ArduControl::add_loco(unsigned addr, const string &proto_name, const VehicleType &)
80 throw invalid_argument("ArduControl::add_loco");
82 Protocol proto = map_protocol(proto_name);
83 if(addr>protocol_info[proto].max_address)
84 throw invalid_argument("ArduControl::add_loco");
86 Locomotive loco(proto, addr);
87 insert_unique(locomotives, loco.id, loco);
92 void ArduControl::remove_loco(unsigned id)
94 Locomotive &loco = get_item(locomotives, id);
95 remove_loco_from_refresh(loco);
96 locomotives.erase(id);
99 void ArduControl::set_loco_speed(unsigned id, unsigned speed)
101 Locomotive &loco = get_item(locomotives, id);
102 if(speed>protocol_info[loco.proto].max_speed)
103 throw invalid_argument("ArduControl::set_loco_speed");
105 if(loco.speed.set(speed))
107 QueuedCommand cmd(loco, Locomotive::SPEED);
110 add_loco_to_refresh(loco);
114 void ArduControl::set_loco_reverse(unsigned id, bool rev)
116 Locomotive &loco = get_item(locomotives, id);
117 if(loco.reverse.set(rev))
119 QueuedCommand cmd(loco, Locomotive::REVERSE);
122 add_loco_to_refresh(loco);
126 void ArduControl::set_loco_function(unsigned id, unsigned func, bool state)
128 Locomotive &loco = get_item(locomotives, id);
129 if(func>protocol_info[loco.proto].max_func)
130 throw invalid_argument("ArduControl::set_loco_function");
132 unsigned mask = 1<<func;
133 if(loco.funcs.set((loco.funcs&~mask)|(mask*state)))
137 QueuedCommand cmd(loco, Locomotive::FUNCTIONS, func);
141 add_loco_to_refresh(loco);
145 void ArduControl::add_loco_to_refresh(Locomotive &loco)
147 MutexLock lock(mutex);
148 refresh_cycle.push_back(&loco);
149 if(refresh_cycle.size()>15)
151 LocomotivePtrList::iterator oldest = refresh_cycle.begin();
152 for(LocomotivePtrList::iterator i=refresh_cycle.begin(); ++i!=refresh_cycle.end(); )
153 if((*i)->last_change_age>(*oldest)->last_change_age)
155 if(oldest==next_refresh)
156 advance_next_refresh();
157 refresh_cycle.erase(oldest);
159 if(next_refresh==refresh_cycle.end())
160 next_refresh = refresh_cycle.begin();
163 void ArduControl::remove_loco_from_refresh(Locomotive &loco)
165 MutexLock lock(mutex);
166 for(LocomotivePtrList::iterator i=refresh_cycle.begin(); i!=refresh_cycle.end(); ++i)
171 if(refresh_cycle.size()>1)
172 advance_next_refresh();
174 next_refresh = refresh_cycle.end();
176 refresh_cycle.erase(i);
181 ArduControl::Locomotive *ArduControl::get_loco_to_refresh()
183 MutexLock lock(mutex);
184 if(refresh_cycle.empty())
187 Locomotive *loco = *next_refresh;
188 advance_next_refresh();
192 void ArduControl::advance_next_refresh()
195 if(next_refresh==refresh_cycle.end())
197 next_refresh = refresh_cycle.begin();
202 unsigned ArduControl::add_turnout(unsigned addr, const TrackType &type)
204 if(!addr || !type.is_turnout())
205 throw invalid_argument("ArduControl::add_turnout");
207 return add_accessory(Accessory::TURNOUT, addr, type.get_state_bits());
210 void ArduControl::remove_turnout(unsigned addr)
212 remove_accessory(Accessory::TURNOUT, addr);
215 void ArduControl::set_turnout(unsigned addr, unsigned state)
217 set_accessory(Accessory::TURNOUT, addr, state);
220 unsigned ArduControl::get_turnout(unsigned addr) const
222 return get_accessory(Accessory::TURNOUT, addr);
225 unsigned ArduControl::add_signal(unsigned addr, const SignalType &)
227 return add_accessory(Accessory::SIGNAL, addr, 1);
230 void ArduControl::remove_signal(unsigned addr)
232 remove_accessory(Accessory::SIGNAL, addr);
235 void ArduControl::set_signal(unsigned addr, unsigned state)
237 set_accessory(Accessory::SIGNAL, addr, state);
240 unsigned ArduControl::get_signal(unsigned addr) const
242 return get_accessory(Accessory::SIGNAL, addr);
245 unsigned ArduControl::add_accessory(Accessory::Kind kind, unsigned addr, unsigned bits)
247 AccessoryMap::iterator i = accessories.lower_bound(addr);
248 AccessoryMap::iterator j = accessories.upper_bound(addr+bits-1);
250 throw key_error(addr);
251 if(i!=accessories.begin())
254 if(i->first+i->second.bits>addr)
255 throw key_error(addr);
258 insert_unique(accessories, addr, Accessory(kind, addr, bits));
262 void ArduControl::remove_accessory(Accessory::Kind kind, unsigned addr)
264 Accessory &acc = get_item(accessories, addr);
266 throw key_error(addr);
267 accessories.erase(addr);
270 void ArduControl::set_accessory(Accessory::Kind kind, unsigned addr, unsigned state)
272 Accessory &acc = get_item(accessories, addr);
274 throw key_error(addr);
276 if(state!=acc.target)
279 accessory_queue.push_back(&acc);
283 unsigned ArduControl::get_accessory(Accessory::Kind kind, unsigned addr) const
285 const Accessory &acc = get_item(accessories, addr);
287 throw key_error(addr);
291 unsigned ArduControl::add_sensor(unsigned addr)
294 throw invalid_argument("ArduControl::add_sensor");
296 insert_unique(sensors, addr, Sensor(addr));
297 unsigned octet_index = (addr-1)/8;
298 if(octet_index>=n_s88_octets)
299 n_s88_octets = octet_index+1;
304 void ArduControl::remove_sensor(unsigned addr)
306 remove_existing(sensors, addr);
307 // TODO update n_s88_octets
310 bool ArduControl::get_sensor(unsigned addr) const
312 return get_item(sensors, addr).state;
315 void ArduControl::tick()
317 while(Tag tag = pop_completed_tag())
319 if(tag.type==Tag::GENERAL)
321 if(tag.command==POWER)
323 if(power.commit(tag.serial))
324 signal_power.emit(power.current);
327 else if(tag.type==Tag::LOCOMOTIVE)
329 LocomotiveMap::iterator i = locomotives.find(tag.id);
330 if(i==locomotives.end())
333 Locomotive &loco = i->second;
334 if(tag.command==Locomotive::SPEED)
336 if(loco.speed.commit(tag.serial))
337 signal_loco_speed.emit(loco.id, loco.speed, loco.reverse);
339 else if(tag.command==Locomotive::REVERSE)
341 if(loco.reverse.commit(tag.serial))
342 signal_loco_speed.emit(loco.id, loco.speed, loco.reverse);
344 else if(tag.command==Locomotive::FUNCTIONS)
346 unsigned old = loco.funcs;
347 if(loco.funcs.commit(tag.serial))
349 unsigned changed = old^loco.funcs;
350 for(unsigned j=0; changed>>j; ++j)
352 signal_loco_function.emit(loco.id, j, (loco.funcs>>j)&1);
356 else if(tag.type==Tag::ACCESSORY)
358 AccessoryMap::iterator i = accessories.find(tag.id);
359 if(i==accessories.end())
362 Accessory &acc = i->second;
363 if(tag.command==Accessory::ACTIVATE)
365 off_timeout = Time::now()+acc.active_time;
367 else if(tag.command==Accessory::DEACTIVATE)
369 if(acc.state.commit(tag.serial))
371 if(acc.state==acc.target)
373 if(acc.kind==Accessory::TURNOUT)
374 signal_turnout.emit(acc.address, acc.state);
375 else if(acc.kind==Accessory::SIGNAL)
376 signal_signal.emit(acc.address, acc.state);
378 if(&acc==active_accessory)
379 active_accessory = 0;
383 else if(tag.type==Tag::SENSOR)
385 SensorMap::iterator i = sensors.find(tag.id);
389 Sensor &sensor = i->second;
390 if(tag.command==Sensor::STATE)
392 if(sensor.state.commit(tag.serial))
393 signal_sensor.emit(sensor.address, sensor.state);
398 while(!active_accessory && !accessory_queue.empty())
400 Accessory &acc = *accessory_queue.front();
402 if(acc.state!=acc.target)
404 active_accessory = &acc;
406 unsigned changes = acc.state^acc.target;
407 unsigned lowest_bit = changes&~(changes-1);
409 for(i=0; (lowest_bit>>i)>1; ++i) ;
410 acc.state.set(acc.state^lowest_bit);
411 QueuedCommand cmd(acc, Accessory::ACTIVATE, i);
415 accessory_queue.pop_front();
418 if(active_accessory && off_timeout)
420 Time::TimeStamp t = Time::now();
423 off_timeout = Time::TimeStamp();
424 QueuedCommand cmd(*active_accessory, Accessory::DEACTIVATE);
430 void ArduControl::flush()
434 void ArduControl::push_command(const QueuedCommand &cmd)
436 MutexLock lock(mutex);
437 command_queue.push_back(cmd);
440 bool ArduControl::pop_command(QueuedCommand &cmd)
442 MutexLock lock(mutex);
443 if(command_queue.empty())
445 cmd = command_queue.front();
446 command_queue.pop_front();
450 void ArduControl::push_completed_tag(const Tag &tag)
452 MutexLock lock(mutex);
453 completed_commands.push_back(tag);
456 ArduControl::Tag ArduControl::pop_completed_tag()
458 MutexLock lock(mutex);
459 if(completed_commands.empty())
461 Tag tag = completed_commands.front();
462 completed_commands.pop_front();
467 ArduControl::Tag::Tag():
475 ArduControl::Locomotive::Locomotive(Protocol p, unsigned a):
485 unsigned ArduControl::Locomotive::create_speed_dir_command(char *buffer) const
489 buffer[0] = MOTOROLA_SPEED_DIRECTION;
491 buffer[2] = funcs.pending&1;
492 buffer[3] = speed.pending+reverse.pending*0x80;
499 unsigned ArduControl::Locomotive::create_speed_func_command(unsigned f, char *buffer) const
504 throw invalid_argument("Locomotive::create_speed_func_command");
506 buffer[0] = MOTOROLA_SPEED_FUNCTION;
508 buffer[2] = (f<<4)|(((funcs.pending>>f)&1)<<1)|(funcs.pending&1);
509 buffer[3] = speed.pending;
517 ArduControl::Accessory::Accessory(Kind k, unsigned a, unsigned b):
522 active_time(500*Time::msec)
525 unsigned ArduControl::Accessory::create_state_command(unsigned b, bool c, char *buffer) const
528 throw invalid_argument("Accessory::create_state_command");
530 unsigned a = (address+b+3)*2;
531 if(!((state.pending>>b)&1))
533 buffer[0] = MOTOROLA_SOLENOID;
535 buffer[2] = ((a&7)<<4)|c;
540 ArduControl::Sensor::Sensor(unsigned a):
546 ArduControl::ControlThread::ControlThread(ArduControl &c):
553 void ArduControl::ControlThread::exit()
559 void ArduControl::ControlThread::main()
563 unsigned repeat_count = 0;
565 Locomotive *loco = 0;
567 unsigned s88_octets_remaining = 0;
576 if(control.pop_command(qcmd))
578 length = qcmd.length;
579 copy(qcmd.command, qcmd.command+length, command);
580 if(qcmd.tag.type==Tag::LOCOMOTIVE)
584 else if(loco && phase==0)
586 length = loco->create_speed_func_command(control.refresh_counter%4+1, command);
590 else if(!s88_octets_remaining && control.n_s88_octets)
592 command[0] = S88_READ;
593 command[1] = control.n_s88_octets;
595 s88_octets_remaining = control.n_s88_octets;
597 else if((loco = control.get_loco_to_refresh()))
599 length = loco->create_speed_dir_command(command);
605 // Send an idle packet for the MM protocol
606 command[0] = MOTOROLA_SPEED;
617 for(unsigned i=0; i<length; ++i)
618 cmd_hex += format(" %02X", static_cast<unsigned char>(command[i]));
619 IO::print("< %02X%s\n", length^0xFF, cmd_hex);
622 control.serial.put(length^0xFF);
623 control.serial.write(command, length);
626 bool got_reply = false;
627 bool got_data = false;
628 while(!got_reply || got_data)
631 got_data = IO::poll(control.serial, IO::P_INPUT, Time::zero);
633 got_data = IO::poll(control.serial, IO::P_INPUT);
637 unsigned rlength = control.serial.get()^0xFF;
640 IO::print("Invalid length %02X\n", rlength);
647 pos += control.serial.read(reply+pos, rlength-pos);
652 for(unsigned i=0; i<rlength; ++i)
653 reply_hex += format(" %02X", static_cast<unsigned char>(reply[i]));
654 IO::print("> %02X%s\n", rlength^0xFF, reply_hex);
657 unsigned char type = reply[0];
658 if((type&0xE0)==0x80)
662 IO::print("Error %02X\n", type);
663 else if(tag && !repeat_count)
664 control.push_completed_tag(tag);
666 else if(type==POWER_STATE && rlength==2)
668 control.power.set(reply[1]);
671 ptag.type = Tag::GENERAL;
672 ptag.command = POWER;
673 ptag.serial = control.power.serial;
674 control.push_completed_tag(ptag);
676 else if(type==S88_DATA && rlength>2)
678 unsigned offset = static_cast<unsigned char>(reply[1]);
679 unsigned count = rlength-2;
681 SensorMap::iterator begin = control.sensors.lower_bound(offset*8+1);
682 SensorMap::iterator end = control.sensors.upper_bound((offset+count)*8);
683 for(SensorMap::iterator i=begin; i!=end; ++i)
685 unsigned bit_index = i->first-1-offset*8;
686 bool state = (reply[2+bit_index/8]>>(7-bit_index%8))&1;
687 i->second.state.set(state);
690 stag.type = Tag::SENSOR;
691 stag.command = Sensor::STATE;
692 stag.serial = i->second.state.serial;
694 control.push_completed_tag(stag);
697 if(count>s88_octets_remaining)
698 s88_octets_remaining = 0;
700 s88_octets_remaining -= count;
708 ArduControl::QueuedCommand::QueuedCommand():
712 ArduControl::QueuedCommand::QueuedCommand(GeneralCommand cmd):
715 tag.type = Tag::GENERAL;
719 ArduControl::QueuedCommand::QueuedCommand(Locomotive &loco, Locomotive::Command cmd, unsigned index)
721 tag.type = Tag::LOCOMOTIVE;
724 if(cmd==Locomotive::SPEED)
726 tag.serial = loco.speed.serial;
727 length = loco.create_speed_dir_command(command);
729 else if(cmd==Locomotive::REVERSE)
731 tag.serial = loco.reverse.serial;
732 length = loco.create_speed_dir_command(command);
734 else if(cmd==Locomotive::FUNCTIONS)
736 tag.serial = loco.funcs.serial;
737 length = loco.create_speed_func_command(index, command);
740 throw invalid_argument("QueuedCommand");
743 ArduControl::QueuedCommand::QueuedCommand(Accessory &acc, Accessory::Command cmd, unsigned index)
745 tag.type = Tag::ACCESSORY;
747 tag.id = acc.address;
748 if(cmd==Accessory::ACTIVATE || cmd==Accessory::DEACTIVATE)
750 tag.serial = acc.state.serial;
751 length = acc.create_state_command(index, (cmd==Accessory::ACTIVATE), command);
754 throw invalid_argument("QueuedCommand");