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()),
29 ArduControl::~ArduControl()
34 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);
49 void ArduControl::halt(bool)
53 const char *ArduControl::enumerate_protocols(unsigned i) const
61 ArduControl::Protocol ArduControl::map_protocol(const string &proto_name)
65 else if(proto_name=="MFX")
68 throw invalid_argument("ArduControl::map_protocol");
71 unsigned ArduControl::get_protocol_speed_steps(const string &proto_name) const
73 return protocol_info[map_protocol(proto_name)].max_speed;
76 unsigned ArduControl::add_loco(unsigned addr, const string &proto_name, const VehicleType &)
79 throw invalid_argument("ArduControl::add_loco");
81 Protocol proto = map_protocol(proto_name);
82 if(addr>protocol_info[proto].max_address)
83 throw invalid_argument("ArduControl::add_loco");
85 Locomotive loco(proto, addr);
86 insert_unique(locomotives, loco.id, loco);
91 void ArduControl::remove_loco(unsigned id)
93 Locomotive &loco = get_item(locomotives, id);
94 remove_loco_from_refresh(loco);
95 locomotives.erase(id);
98 void ArduControl::set_loco_speed(unsigned id, unsigned speed)
100 Locomotive &loco = get_item(locomotives, id);
101 if(speed>protocol_info[loco.proto].max_speed)
102 throw invalid_argument("ArduControl::set_loco_speed");
104 if(loco.speed.set(speed))
106 QueuedCommand cmd(loco, Locomotive::SPEED);
109 add_loco_to_refresh(loco);
113 void ArduControl::set_loco_reverse(unsigned id, bool rev)
115 Locomotive &loco = get_item(locomotives, id);
116 if(loco.reverse.set(rev))
118 QueuedCommand cmd(loco, Locomotive::REVERSE);
121 add_loco_to_refresh(loco);
125 void ArduControl::set_loco_function(unsigned id, unsigned func, bool state)
127 Locomotive &loco = get_item(locomotives, id);
128 if(func>protocol_info[loco.proto].max_func)
129 throw invalid_argument("ArduControl::set_loco_function");
131 unsigned mask = 1<<func;
132 if(loco.funcs.set((loco.funcs&~mask)|(mask*state)))
136 QueuedCommand cmd(loco, Locomotive::FUNCTIONS, func);
140 add_loco_to_refresh(loco);
144 void ArduControl::add_loco_to_refresh(Locomotive &loco)
146 MutexLock lock(mutex);
147 refresh_cycle.push_back(&loco);
148 if(refresh_cycle.size()>15)
150 LocomotivePtrList::iterator oldest = refresh_cycle.begin();
151 for(LocomotivePtrList::iterator i=refresh_cycle.begin(); ++i!=refresh_cycle.end(); )
152 if((*i)->last_change_age>(*oldest)->last_change_age)
154 if(oldest==next_refresh)
155 advance_next_refresh();
156 refresh_cycle.erase(oldest);
158 if(next_refresh==refresh_cycle.end())
159 next_refresh = refresh_cycle.begin();
162 void ArduControl::remove_loco_from_refresh(Locomotive &loco)
164 MutexLock lock(mutex);
165 for(LocomotivePtrList::iterator i=refresh_cycle.begin(); i!=refresh_cycle.end(); ++i)
170 if(refresh_cycle.size()>1)
171 advance_next_refresh();
173 next_refresh = refresh_cycle.end();
175 refresh_cycle.erase(i);
180 ArduControl::Locomotive *ArduControl::get_loco_to_refresh()
182 MutexLock lock(mutex);
183 if(refresh_cycle.empty())
186 Locomotive *loco = *next_refresh;
187 advance_next_refresh();
191 void ArduControl::advance_next_refresh()
194 if(next_refresh==refresh_cycle.end())
196 next_refresh = refresh_cycle.begin();
201 unsigned ArduControl::add_turnout(unsigned addr, const TrackType &type)
203 if(!addr || !type.is_turnout())
204 throw invalid_argument("ArduControl::add_turnout");
206 return add_accessory(Accessory::TURNOUT, addr, type.get_state_bits());
209 void ArduControl::remove_turnout(unsigned addr)
211 remove_accessory(Accessory::TURNOUT, addr);
214 void ArduControl::set_turnout(unsigned addr, unsigned state)
216 set_accessory(Accessory::TURNOUT, addr, state);
219 unsigned ArduControl::get_turnout(unsigned addr) const
221 return get_accessory(Accessory::TURNOUT, addr);
224 unsigned ArduControl::add_signal(unsigned addr, const SignalType &)
226 return add_accessory(Accessory::SIGNAL, addr, 1);
229 void ArduControl::remove_signal(unsigned addr)
231 remove_accessory(Accessory::SIGNAL, addr);
234 void ArduControl::set_signal(unsigned addr, unsigned state)
236 set_accessory(Accessory::SIGNAL, addr, state);
239 unsigned ArduControl::get_signal(unsigned addr) const
241 return get_accessory(Accessory::SIGNAL, addr);
244 unsigned ArduControl::add_accessory(Accessory::Kind kind, unsigned addr, unsigned bits)
246 AccessoryMap::iterator i = accessories.lower_bound(addr);
247 AccessoryMap::iterator j = accessories.upper_bound(addr+bits-1);
249 throw key_error(addr);
250 if(i!=accessories.begin())
253 if(i->first+i->second.bits>addr)
254 throw key_error(addr);
257 insert_unique(accessories, addr, Accessory(kind, addr, bits));
261 void ArduControl::remove_accessory(Accessory::Kind kind, unsigned addr)
263 Accessory &acc = get_item(accessories, addr);
265 throw key_error(addr);
266 accessories.erase(addr);
269 void ArduControl::set_accessory(Accessory::Kind kind, unsigned addr, unsigned state)
271 Accessory &acc = get_item(accessories, addr);
273 throw key_error(addr);
275 if(state!=acc.target)
278 accessory_queue.push_back(&acc);
282 unsigned ArduControl::get_accessory(Accessory::Kind kind, unsigned addr) const
284 const Accessory &acc = get_item(accessories, addr);
286 throw key_error(addr);
290 unsigned ArduControl::add_sensor(unsigned addr)
293 throw invalid_argument("ArduControl::add_sensor");
295 insert_unique(sensors, addr, Sensor(addr));
296 unsigned octet_index = (addr-1)/8;
297 if(octet_index>=n_s88_octets)
298 n_s88_octets = octet_index+1;
303 void ArduControl::remove_sensor(unsigned addr)
305 remove_existing(sensors, addr);
306 // TODO update n_s88_octets
309 bool ArduControl::get_sensor(unsigned addr) const
311 return get_item(sensors, addr).state;
314 void ArduControl::tick()
316 while(Tag tag = pop_completed_tag())
318 if(tag.type==Tag::GENERAL)
320 if(tag.command==POWER)
322 if(power.commit(tag.serial))
323 signal_power.emit(power.current);
326 else if(tag.type==Tag::LOCOMOTIVE)
328 LocomotiveMap::iterator i = locomotives.find(tag.id);
329 if(i==locomotives.end())
332 Locomotive &loco = i->second;
333 if(tag.command==Locomotive::SPEED)
335 if(loco.speed.commit(tag.serial))
336 signal_loco_speed.emit(loco.id, loco.speed, loco.reverse);
338 else if(tag.command==Locomotive::REVERSE)
340 if(loco.reverse.commit(tag.serial))
341 signal_loco_speed.emit(loco.id, loco.speed, loco.reverse);
343 else if(tag.command==Locomotive::FUNCTIONS)
345 unsigned old = loco.funcs;
346 if(loco.funcs.commit(tag.serial))
348 unsigned changed = old^loco.funcs;
349 for(unsigned j=0; changed>>j; ++j)
351 signal_loco_function.emit(loco.id, j, (loco.funcs>>j)&1);
355 else if(tag.type==Tag::ACCESSORY)
357 AccessoryMap::iterator i = accessories.find(tag.id);
358 if(i==accessories.end())
361 Accessory &acc = i->second;
362 if(tag.command==Accessory::ACTIVATE)
364 off_timeout = Time::now()+acc.active_time;
366 else if(tag.command==Accessory::DEACTIVATE)
368 if(acc.state.commit(tag.serial))
370 if(acc.state==acc.target)
372 if(acc.kind==Accessory::TURNOUT)
373 signal_turnout.emit(acc.address, acc.state);
374 else if(acc.kind==Accessory::SIGNAL)
375 signal_signal.emit(acc.address, acc.state);
377 if(&acc==active_accessory)
378 active_accessory = 0;
382 else if(tag.type==Tag::SENSOR)
384 SensorMap::iterator i = sensors.find(tag.id);
388 Sensor &sensor = i->second;
389 if(tag.command==Sensor::STATE)
391 if(sensor.state.commit(tag.serial))
392 signal_sensor.emit(sensor.address, sensor.state);
397 while(!active_accessory && !accessory_queue.empty())
399 Accessory &acc = *accessory_queue.front();
401 if(acc.state!=acc.target)
403 active_accessory = &acc;
405 unsigned changes = acc.state^acc.target;
406 unsigned lowest_bit = changes&~(changes-1);
408 for(i=0; (lowest_bit>>i)>1; ++i) ;
409 acc.state.set(acc.state^lowest_bit);
410 QueuedCommand cmd(acc, Accessory::ACTIVATE, i);
414 accessory_queue.pop_front();
417 if(active_accessory && off_timeout)
419 Time::TimeStamp t = Time::now();
422 off_timeout = Time::TimeStamp();
423 QueuedCommand cmd(*active_accessory, Accessory::DEACTIVATE);
429 void ArduControl::flush()
433 void ArduControl::push_command(const QueuedCommand &cmd)
435 MutexLock lock(mutex);
436 command_queue.push_back(cmd);
439 bool ArduControl::pop_command(QueuedCommand &cmd)
441 MutexLock lock(mutex);
442 if(command_queue.empty())
444 cmd = command_queue.front();
445 command_queue.pop_front();
449 void ArduControl::push_completed_tag(const Tag &tag)
451 MutexLock lock(mutex);
452 completed_commands.push_back(tag);
455 ArduControl::Tag ArduControl::pop_completed_tag()
457 MutexLock lock(mutex);
458 if(completed_commands.empty())
460 Tag tag = completed_commands.front();
461 completed_commands.pop_front();
466 ArduControl::Tag::Tag():
474 ArduControl::Locomotive::Locomotive(Protocol p, unsigned a):
484 unsigned ArduControl::Locomotive::create_speed_dir_command(char *buffer) const
488 buffer[0] = MOTOROLA_SPEED_DIRECTION;
490 buffer[2] = funcs.pending&1;
491 buffer[3] = speed.pending+reverse.pending*0x80;
498 unsigned ArduControl::Locomotive::create_speed_func_command(unsigned f, char *buffer) const
503 throw invalid_argument("Locomotive::create_speed_func_command");
505 buffer[0] = MOTOROLA_SPEED_FUNCTION;
507 buffer[2] = (f<<4)|(((funcs.pending>>f)&1)<<1)|(funcs.pending&1);
508 buffer[3] = speed.pending;
516 ArduControl::Accessory::Accessory(Kind k, unsigned a, unsigned b):
521 active_time(500*Time::msec)
524 unsigned ArduControl::Accessory::create_state_command(unsigned b, bool c, char *buffer) const
527 throw invalid_argument("Accessory::create_state_command");
529 unsigned a = (address+b+3)*2;
530 if(!((state.pending>>b)&1))
532 buffer[0] = MOTOROLA_SOLENOID;
534 buffer[2] = ((a&7)<<4)|c;
539 ArduControl::Sensor::Sensor(unsigned a):
545 ArduControl::ControlThread::ControlThread(ArduControl &c):
552 void ArduControl::ControlThread::exit()
558 void ArduControl::ControlThread::main()
562 unsigned repeat_count = 0;
564 Locomotive *loco = 0;
566 unsigned s88_octets_remaining = 0;
575 if(control.pop_command(qcmd))
577 length = qcmd.length;
578 copy(qcmd.command, qcmd.command+length, command);
579 if(qcmd.tag.type==Tag::LOCOMOTIVE)
583 else if(loco && phase==0)
585 length = loco->create_speed_func_command(control.refresh_counter%4+1, command);
589 else if(!s88_octets_remaining && control.n_s88_octets)
591 command[0] = S88_READ;
592 command[1] = control.n_s88_octets;
594 s88_octets_remaining = control.n_s88_octets;
596 else if((loco = control.get_loco_to_refresh()))
598 length = loco->create_speed_dir_command(command);
604 // Send an idle packet for the MM protocol
605 command[0] = MOTOROLA_SPEED;
616 for(unsigned i=0; i<length; ++i)
617 cmd_hex += format(" %02X", static_cast<unsigned char>(command[i]));
618 IO::print("< %02X%s\n", length^0xFF, cmd_hex);
621 control.serial.put(length^0xFF);
622 control.serial.write(command, length);
625 bool got_reply = false;
626 bool got_data = false;
627 while(!got_reply || got_data)
630 got_data = IO::poll(control.serial, IO::P_INPUT, Time::zero);
632 got_data = IO::poll(control.serial, IO::P_INPUT);
636 unsigned rlength = control.serial.get()^0xFF;
639 IO::print("Invalid length %02X\n", rlength);
646 pos += control.serial.read(reply+pos, rlength-pos);
651 for(unsigned i=0; i<rlength; ++i)
652 reply_hex += format(" %02X", static_cast<unsigned char>(reply[i]));
653 IO::print("> %02X%s\n", rlength^0xFF, reply_hex);
656 unsigned char type = reply[0];
657 if((type&0xE0)==0x80)
661 IO::print("Error %02X\n", type);
662 else if(tag && !repeat_count)
663 control.push_completed_tag(tag);
665 else if(type==S88_DATA && rlength>2)
667 unsigned offset = static_cast<unsigned char>(reply[1]);
668 unsigned count = rlength-2;
670 SensorMap::iterator begin = control.sensors.lower_bound(offset*8+1);
671 SensorMap::iterator end = control.sensors.upper_bound((offset+count)*8);
672 for(SensorMap::iterator i=begin; i!=end; ++i)
674 unsigned bit_index = i->first-1-offset*8;
675 bool state = (reply[2+bit_index/8]>>(7-bit_index%8))&1;
676 i->second.state.set(state);
679 stag.type = Tag::SENSOR;
680 stag.command = Sensor::STATE;
681 stag.serial = i->second.state.serial;
683 control.push_completed_tag(stag);
686 if(count>s88_octets_remaining)
687 s88_octets_remaining = 0;
689 s88_octets_remaining -= count;
697 ArduControl::QueuedCommand::QueuedCommand():
701 ArduControl::QueuedCommand::QueuedCommand(GeneralCommand cmd):
704 tag.type = Tag::GENERAL;
708 ArduControl::QueuedCommand::QueuedCommand(Locomotive &loco, Locomotive::Command cmd, unsigned index)
710 tag.type = Tag::LOCOMOTIVE;
713 if(cmd==Locomotive::SPEED)
715 tag.serial = loco.speed.serial;
716 length = loco.create_speed_dir_command(command);
718 else if(cmd==Locomotive::REVERSE)
720 tag.serial = loco.reverse.serial;
721 length = loco.create_speed_dir_command(command);
723 else if(cmd==Locomotive::FUNCTIONS)
725 tag.serial = loco.funcs.serial;
726 length = loco.create_speed_func_command(index, command);
729 throw invalid_argument("QueuedCommand");
732 ArduControl::QueuedCommand::QueuedCommand(Accessory &acc, Accessory::Command cmd, unsigned index)
734 tag.type = Tag::ACCESSORY;
736 tag.id = acc.address;
737 if(cmd==Accessory::ACTIVATE || cmd==Accessory::DEACTIVATE)
739 tag.serial = acc.state.serial;
740 length = acc.create_state_command(index, (cmd==Accessory::ACTIVATE), command);
743 throw invalid_argument("QueuedCommand");