1 #include <msp/core/maputils.h>
2 #include <msp/io/print.h>
3 #include <msp/time/utils.h>
4 #include "arducontrol.h"
12 ArduControl::ArduControl(const string &dev):
16 next_refresh(refresh_cycle.end()),
24 ArduControl::~ArduControl()
29 void ArduControl::set_power(bool p)
37 QueuedCommand cmd(POWER);
38 cmd.tag.serial = power.serial;
39 cmd.command[0] = (p ? POWER_ON : POWER_OFF);
44 void ArduControl::halt(bool)
48 const char *ArduControl::enumerate_protocols(unsigned i) const
56 ArduControl::Protocol ArduControl::map_protocol(const string &proto_name)
61 throw invalid_argument("ArduControl::map_protocol");
64 unsigned ArduControl::get_protocol_speed_steps(const string &proto_name) const
66 Protocol proto = map_protocol(proto_name);
73 void ArduControl::add_loco(unsigned addr, const string &proto_name, const VehicleType &)
76 throw invalid_argument("ArduControl::add_loco");
78 Protocol proto = map_protocol(proto_name);
83 throw invalid_argument("ArduControl::add_loco");
86 insert_unique(locomotives, addr, Locomotive(proto, addr));
89 void ArduControl::remove_loco(unsigned addr)
91 Locomotive &loco = get_item(locomotives, addr);
92 remove_loco_from_refresh(loco);
93 locomotives.erase(addr);
96 void ArduControl::set_loco_speed(unsigned addr, unsigned speed)
98 Locomotive &loco = get_item(locomotives, addr);
99 if(loco.speed.set(speed))
101 QueuedCommand cmd(loco, Locomotive::SPEED);
104 add_loco_to_refresh(loco);
108 void ArduControl::set_loco_reverse(unsigned addr, bool rev)
110 Locomotive &loco = get_item(locomotives, addr);
111 if(loco.reverse.set(rev))
113 QueuedCommand cmd(loco, Locomotive::REVERSE);
116 add_loco_to_refresh(loco);
120 void ArduControl::set_loco_function(unsigned addr, unsigned func, bool state)
122 Locomotive &loco = get_item(locomotives, addr);
123 unsigned mask = 1<<func;
124 if(loco.funcs.set((loco.funcs&~mask)|(mask*state)))
128 QueuedCommand cmd(loco, Locomotive::FUNCTIONS, func);
132 add_loco_to_refresh(loco);
136 void ArduControl::add_loco_to_refresh(Locomotive &loco)
138 MutexLock lock(mutex);
139 refresh_cycle.push_back(&loco);
140 if(refresh_cycle.size()>15)
142 LocomotivePtrList::iterator oldest = refresh_cycle.begin();
143 for(LocomotivePtrList::iterator i=refresh_cycle.begin(); ++i!=refresh_cycle.end(); )
144 if((*i)->last_change_age>(*oldest)->last_change_age)
146 if(oldest==next_refresh)
147 advance_next_refresh();
148 refresh_cycle.erase(oldest);
150 if(next_refresh==refresh_cycle.end())
151 next_refresh = refresh_cycle.begin();
154 void ArduControl::remove_loco_from_refresh(Locomotive &loco)
156 MutexLock lock(mutex);
157 for(LocomotivePtrList::iterator i=refresh_cycle.begin(); i!=refresh_cycle.end(); ++i)
162 if(refresh_cycle.size()>1)
163 advance_next_refresh();
165 next_refresh = refresh_cycle.end();
167 refresh_cycle.erase(i);
172 ArduControl::Locomotive *ArduControl::get_loco_to_refresh()
174 MutexLock lock(mutex);
175 if(refresh_cycle.empty())
178 Locomotive *loco = *next_refresh;
179 advance_next_refresh();
183 void ArduControl::advance_next_refresh()
186 if(next_refresh==refresh_cycle.end())
188 next_refresh = refresh_cycle.begin();
193 void ArduControl::add_turnout(unsigned addr, const TrackType &type)
195 if(!addr || !type.is_turnout())
196 throw invalid_argument("ArduControl::add_turnout");
198 add_accessory(Accessory::TURNOUT, addr, type.get_state_bits());
201 void ArduControl::remove_turnout(unsigned addr)
203 remove_accessory(Accessory::TURNOUT, addr);
206 void ArduControl::set_turnout(unsigned addr, unsigned state)
208 set_accessory(Accessory::TURNOUT, addr, state);
211 unsigned ArduControl::get_turnout(unsigned addr) const
213 return get_accessory(Accessory::TURNOUT, addr);
216 void ArduControl::add_signal(unsigned addr, const SignalType &)
218 add_accessory(Accessory::SIGNAL, addr, 1);
221 void ArduControl::remove_signal(unsigned addr)
223 remove_accessory(Accessory::SIGNAL, addr);
226 void ArduControl::set_signal(unsigned addr, unsigned state)
228 set_accessory(Accessory::SIGNAL, addr, state);
231 unsigned ArduControl::get_signal(unsigned addr) const
233 return get_accessory(Accessory::SIGNAL, addr);
236 void ArduControl::add_accessory(Accessory::Kind kind, unsigned addr, unsigned bits)
238 AccessoryMap::iterator i = accessories.lower_bound(addr);
239 AccessoryMap::iterator j = accessories.upper_bound(addr+bits-1);
241 throw key_error(addr);
242 if(i!=accessories.begin())
245 if(i->first+i->second.bits>addr)
246 throw key_error(addr);
249 insert_unique(accessories, addr, Accessory(kind, addr, bits));
252 void ArduControl::remove_accessory(Accessory::Kind kind, unsigned addr)
254 Accessory &acc = get_item(accessories, addr);
256 throw key_error(addr);
257 accessories.erase(addr);
260 void ArduControl::set_accessory(Accessory::Kind kind, unsigned addr, unsigned state)
262 Accessory &acc = get_item(accessories, addr);
264 throw key_error(addr);
266 if(state!=acc.target)
269 accessory_queue.push_back(&acc);
273 unsigned ArduControl::get_accessory(Accessory::Kind kind, unsigned addr) const
275 const Accessory &acc = get_item(accessories, addr);
277 throw key_error(addr);
281 void ArduControl::add_sensor(unsigned addr)
284 throw invalid_argument("ArduControl::add_sensor");
286 insert_unique(sensors, addr, Sensor(addr));
287 unsigned octet_index = (addr-1)/8;
288 if(octet_index>=n_s88_octets)
289 n_s88_octets = octet_index+1;
292 void ArduControl::remove_sensor(unsigned addr)
294 remove_existing(sensors, addr);
295 // TODO update n_s88_octets
298 bool ArduControl::get_sensor(unsigned addr) const
300 return get_item(sensors, addr).state;
303 void ArduControl::tick()
305 while(Tag tag = pop_completed_tag())
307 if(tag.type==Tag::GENERAL)
309 if(tag.command==POWER)
311 if(power.commit(tag.serial))
312 signal_power.emit(power.current);
315 else if(tag.type==Tag::LOCOMOTIVE)
317 LocomotiveMap::iterator i = locomotives.find(tag.address);
318 if(i==locomotives.end())
321 Locomotive &loco = i->second;
322 if(tag.command==Locomotive::SPEED)
324 if(loco.speed.commit(tag.serial))
325 signal_loco_speed.emit(loco.address, loco.speed, loco.reverse);
327 else if(tag.command==Locomotive::REVERSE)
329 if(loco.reverse.commit(tag.serial))
330 signal_loco_speed.emit(loco.address, loco.speed, loco.reverse);
332 else if(tag.command==Locomotive::FUNCTIONS)
334 unsigned old = loco.funcs;
335 if(loco.funcs.commit(tag.serial))
337 unsigned changed = old^loco.funcs;
338 for(unsigned j=0; changed>>j; ++j)
340 signal_loco_function.emit(loco.address, j, (loco.funcs>>j)&1);
344 else if(tag.type==Tag::ACCESSORY)
346 AccessoryMap::iterator i = accessories.find(tag.address);
347 if(i==accessories.end())
350 Accessory &acc = i->second;
351 if(tag.command==Accessory::ACTIVATE)
353 off_timeout = Time::now()+acc.active_time;
355 else if(tag.command==Accessory::DEACTIVATE)
357 if(acc.state.commit(tag.serial))
359 if(acc.state==acc.target)
361 if(acc.kind==Accessory::TURNOUT)
362 signal_turnout.emit(acc.address, acc.state);
363 else if(acc.kind==Accessory::SIGNAL)
364 signal_signal.emit(acc.address, acc.state);
366 if(&acc==active_accessory)
367 active_accessory = 0;
371 else if(tag.type==Tag::SENSOR)
373 SensorMap::iterator i = sensors.find(tag.address);
377 Sensor &sensor = i->second;
378 if(tag.command==Sensor::STATE)
380 if(sensor.state.commit(tag.serial))
381 signal_sensor.emit(sensor.address, sensor.state);
386 while(!active_accessory && !accessory_queue.empty())
388 Accessory &acc = *accessory_queue.front();
390 if(acc.state!=acc.target)
392 active_accessory = &acc;
394 unsigned changes = acc.state^acc.target;
395 unsigned lowest_bit = changes&~(changes-1);
397 for(i=0; (lowest_bit>>i)>1; ++i) ;
398 acc.state.set(acc.state^lowest_bit);
399 QueuedCommand cmd(acc, Accessory::ACTIVATE, i);
403 accessory_queue.pop_front();
406 if(active_accessory && off_timeout)
408 Time::TimeStamp t = Time::now();
411 off_timeout = Time::TimeStamp();
412 QueuedCommand cmd(*active_accessory, Accessory::DEACTIVATE);
418 void ArduControl::flush()
422 void ArduControl::push_command(const QueuedCommand &cmd)
424 MutexLock lock(mutex);
425 command_queue.push_back(cmd);
428 bool ArduControl::pop_command(QueuedCommand &cmd)
430 MutexLock lock(mutex);
431 if(command_queue.empty())
433 cmd = command_queue.front();
434 command_queue.pop_front();
438 void ArduControl::push_completed_tag(const Tag &tag)
440 MutexLock lock(mutex);
441 completed_commands.push_back(tag);
444 ArduControl::Tag ArduControl::pop_completed_tag()
446 MutexLock lock(mutex);
447 if(completed_commands.empty())
449 Tag tag = completed_commands.front();
450 completed_commands.pop_front();
455 ArduControl::Tag::Tag():
463 ArduControl::Locomotive::Locomotive(Protocol p, unsigned a):
472 unsigned ArduControl::Locomotive::create_speed_dir_command(char *buffer) const
476 buffer[0] = MOTOROLA_SPEED_DIRECTION;
478 buffer[2] = funcs.pending&1;
479 buffer[3] = speed.pending+reverse.pending*0x80;
486 unsigned ArduControl::Locomotive::create_speed_func_command(unsigned f, char *buffer) const
491 throw invalid_argument("Locomotive::create_speed_func_command");
493 buffer[0] = MOTOROLA_SPEED_FUNCTION;
495 buffer[2] = (f<<4)|(((funcs.pending>>f)&1)<<1)|(funcs.pending&1);
496 buffer[3] = speed.pending;
504 ArduControl::Accessory::Accessory(Kind k, unsigned a, unsigned b):
509 active_time(500*Time::msec)
512 unsigned ArduControl::Accessory::create_state_command(unsigned b, bool c, char *buffer) const
515 throw invalid_argument("Accessory::create_state_command");
517 unsigned a = (address+b+3)*2;
518 if(!((state.pending>>b)&1))
520 buffer[0] = MOTOROLA_SOLENOID;
522 buffer[2] = ((a&7)<<4)|c;
527 ArduControl::Sensor::Sensor(unsigned a):
533 ArduControl::ControlThread::ControlThread(ArduControl &c):
540 void ArduControl::ControlThread::exit()
546 void ArduControl::ControlThread::main()
550 unsigned repeat_count = 0;
552 Locomotive *loco = 0;
554 unsigned s88_octets_remaining = 0;
563 if(control.pop_command(qcmd))
565 length = qcmd.length;
566 copy(qcmd.command, qcmd.command+length, command);
567 if(qcmd.tag.type==Tag::LOCOMOTIVE)
571 else if(loco && phase==0)
573 length = loco->create_speed_func_command(control.refresh_counter%4+1, command);
577 else if(!s88_octets_remaining && control.n_s88_octets)
579 command[0] = S88_READ;
580 command[1] = control.n_s88_octets;
582 s88_octets_remaining = control.n_s88_octets;
584 else if((loco = control.get_loco_to_refresh()))
586 length = loco->create_speed_dir_command(command);
592 // Send an idle packet for the MM protocol
593 command[0] = MOTOROLA_SPEED;
604 for(unsigned i=0; i<length; ++i)
605 cmd_hex += format(" %02X", static_cast<unsigned char>(command[i]));
606 IO::print("< %02X%s\n", length^0xFF, cmd_hex);
609 control.serial.put(length^0xFF);
610 control.serial.write(command, length);
613 bool got_reply = false;
614 bool got_data = false;
615 while(!got_reply || got_data)
618 got_data = IO::poll(control.serial, IO::P_INPUT, Time::zero);
620 got_data = IO::poll(control.serial, IO::P_INPUT);
624 unsigned rlength = control.serial.get()^0xFF;
627 IO::print("Invalid length %02X\n", rlength);
634 pos += control.serial.read(reply+pos, rlength-pos);
639 for(unsigned i=0; i<rlength; ++i)
640 reply_hex += format(" %02X", static_cast<unsigned char>(reply[i]));
641 IO::print("> %02X%s\n", rlength^0xFF, reply_hex);
644 unsigned char type = reply[0];
645 if((type&0xE0)==0x80)
649 IO::print("Error %02X\n", type);
650 else if(tag && !repeat_count)
651 control.push_completed_tag(tag);
653 else if(type==S88_DATA && rlength>2)
655 unsigned offset = static_cast<unsigned char>(reply[1]);
656 unsigned count = rlength-2;
658 SensorMap::iterator begin = control.sensors.lower_bound(offset*8+1);
659 SensorMap::iterator end = control.sensors.upper_bound((offset+count)*8);
660 for(SensorMap::iterator i=begin; i!=end; ++i)
662 unsigned bit_index = i->first-1-offset*8;
663 bool state = (reply[2+bit_index/8]>>(7-bit_index%8))&1;
664 i->second.state.set(state);
667 stag.type = Tag::SENSOR;
668 stag.command = Sensor::STATE;
669 stag.serial = i->second.state.serial;
670 stag.address = i->first;
671 control.push_completed_tag(stag);
674 if(count>s88_octets_remaining)
675 s88_octets_remaining = 0;
677 s88_octets_remaining -= count;
685 ArduControl::QueuedCommand::QueuedCommand():
689 ArduControl::QueuedCommand::QueuedCommand(GeneralCommand cmd):
692 tag.type = Tag::GENERAL;
696 ArduControl::QueuedCommand::QueuedCommand(Locomotive &loco, Locomotive::Command cmd, unsigned index)
698 tag.type = Tag::LOCOMOTIVE;
700 tag.address = loco.address;
701 if(cmd==Locomotive::SPEED)
703 tag.serial = loco.speed.serial;
704 length = loco.create_speed_dir_command(command);
706 else if(cmd==Locomotive::REVERSE)
708 tag.serial = loco.reverse.serial;
709 length = loco.create_speed_dir_command(command);
711 else if(cmd==Locomotive::FUNCTIONS)
713 tag.serial = loco.funcs.serial;
714 length = loco.create_speed_func_command(index, command);
717 throw invalid_argument("QueuedCommand");
720 ArduControl::QueuedCommand::QueuedCommand(Accessory &acc, Accessory::Command cmd, unsigned index)
722 tag.type = Tag::ACCESSORY;
724 tag.address = acc.address;
725 if(cmd==Accessory::ACTIVATE || cmd==Accessory::DEACTIVATE)
727 tag.serial = acc.state.serial;
728 length = acc.create_state_command(index, (cmd==Accessory::ACTIVATE), command);
731 throw invalid_argument("QueuedCommand");