3 This file is part of R²C²
4 Copyright © 2010 Mikkosoft Productions, Mikko Rasa
5 Distributed under the GPL
11 #include <msp/io/print.h>
12 #include <msp/time/units.h>
13 #include <msp/time/utils.h>
14 #include "intellibox.h"
15 #include "tracktype.h"
16 #include "vehicletype.h"
23 Intellibox::Intellibox(const string &dev):
26 update_sensors(false),
29 serial_fd = ::open(dev.c_str(), O_RDWR);
31 throw Exception("Couldn't open serial port\n");
33 static unsigned baud[]=
43 tcgetattr(serial_fd, &attr);
45 attr.c_cflag |= CSTOPB;
49 for(unsigned i=0; baud[i]; i+=2)
51 cfsetospeed(&attr, baud[i+1]);
52 tcsetattr(serial_fd, TCSADRAIN, &attr);
54 write(serial_fd, "\xC4", 1);
56 pollfd pfd = { serial_fd, POLLIN, 0 };
57 if(poll(&pfd, 1, 500)>0)
59 IO::print("IB detected at %d bits/s\n", baud[i]);
61 p50 = (read(serial_fd, buf, 2)==2);
68 throw Exception("IB not detected");
71 write(serial_fd, "xZzA1\r", 6);
76 void Intellibox::set_power(bool p)
80 command(CMD_POWER_ON);
82 command(CMD_POWER_OFF);
83 signal_power.emit(power);
86 void Intellibox::halt(bool h)
91 for(map<unsigned, Locomotive>::iterator i=locos.begin(); i!=locos.end(); ++i)
93 set_loco_speed(i->first, 0);
96 signal_halt.emit(halted);
99 const char *Intellibox::enumerate_protocols(unsigned i) const
109 unsigned Intellibox::get_protocol_speed_steps(const string &proto_name) const
111 Protocol proto = map_protocol(proto_name);
114 else if(proto==MM_27)
119 void Intellibox::add_loco(unsigned addr, const string &proto_name, const VehicleType &type)
121 Protocol proto = map_protocol(proto_name);
123 if(!locos.count(addr))
125 Locomotive &loco = locos[addr];
126 loco.protocol = proto;
127 if(type.get_max_function()>4)
129 loco.ext_func = true;
130 locos[addr+1].protocol = NONE;
133 unsigned char data[2];
135 data[1] = (addr>>8)&0xFF;
136 command(CMD_LOK_STATUS, addr, data, 2);
140 void Intellibox::set_loco_speed(unsigned addr, unsigned speed)
142 Locomotive &loco = locos[addr];
143 if(loco.protocol==NONE)
146 if(speed==loco.speed)
148 if(loco.pending_half_step)
150 loco.pending_half_step = 0;
151 loco.half_step_delay = Time::TimeStamp();
152 signal_loco_speed.emit(addr, speed, loco.reverse);
159 if(loco.protocol==MM_27)
164 if(speed>loco.speed && !(speed&1))
166 loco.pending_half_step = -1;
169 else if(speed<loco.speed && (speed&1))
171 loco.pending_half_step = 1;
175 loco.pending_half_step = 0;
176 loco.half_step_delay = Time::TimeStamp();
178 loco_command(addr, (speed+1)/2, loco.reverse, loco.funcs, false);
180 else if(loco.protocol==MM)
185 loco_command(addr, speed, loco.reverse, loco.funcs, false);
190 void Intellibox::set_loco_reverse(unsigned addr, bool rev)
192 Locomotive &loco = locos[addr];
193 if(loco.protocol==NONE || rev==loco.reverse)
198 loco_command(addr, 0, rev, loco.funcs, false);
201 void Intellibox::set_loco_function(unsigned addr, unsigned func, bool state)
203 Locomotive &loco = locos[addr];
204 if(loco.protocol==NONE)
208 loco.funcs |= 1<<func;
210 loco.funcs &= ~(1<<func);
212 loco_command(addr, loco.speed, loco.reverse, loco.funcs, true);
213 else if(loco.ext_func && func<=8)
214 loco_command(addr+1, 0, false, (loco.funcs>>4)&0x1E, true);
215 signal_loco_function.emit(addr, func, state);
218 void Intellibox::add_turnout(unsigned addr, const TrackType &type)
220 if(!turnouts.count(addr))
222 Turnout &turnout = turnouts[addr];
223 turnout.bits = type.get_state_bits();
225 unsigned char data[2];
227 data[1] = (addr>>8)&0xFF;
228 command(CMD_TURNOUT_STATUS, addr, data, 2);
229 for(unsigned i=1; i<turnout.bits; ++i)
231 turnouts[addr+i].bits = 0;
236 command(CMD_TURNOUT_STATUS, addr+i, data, 2);
241 void Intellibox::set_turnout(unsigned addr, unsigned state)
243 Turnout &turnout = turnouts[addr];
244 unsigned mask = (1<<turnout.bits)-1;
245 if(((state^turnout.state)&mask)==0 || ((state^turnout.pending)&mask)==0)
247 turnout.state = state;
248 turnout.pending = state;
249 signal_turnout.emit(addr, state);
253 turnout.state = (turnout.state&mask) | (state&~mask);
254 turnout.pending = state;
255 turnout.active = true;
256 turnout.off_timeout = Time::TimeStamp();
258 for(unsigned i=0; i<turnout.bits; ++i)
259 if((state^turnout.state)&(1<<i))
260 turnout_command(addr+i, !(state&(1<<i)), true);
263 unsigned Intellibox::get_turnout(unsigned addr) const
265 map<unsigned, Turnout>::const_iterator i = turnouts.find(addr);
266 if(i!=turnouts.end())
267 return i->second.state;
271 void Intellibox::add_sensor(unsigned addr)
273 if(!sensors.count(addr))
276 update_sensors = true;
280 bool Intellibox::get_sensor(unsigned addr) const
282 map<unsigned, Sensor>::const_iterator i = sensors.find(addr);
284 return i->second.state;
288 void Intellibox::tick()
290 const Time::TimeStamp t = Time::now();
292 if(t>next_event_query)
294 next_event_query = t+200*Time::msec;
298 for(map<unsigned, Locomotive>::iterator i=locos.begin(); i!=locos.end(); ++i)
299 if(i->second.protocol==MM_27 && i->second.pending_half_step && i->second.half_step_delay && t>i->second.half_step_delay)
301 i->second.speed += i->second.pending_half_step;
302 i->second.pending_half_step = 0;
303 i->second.half_step_delay = Time::TimeStamp();
304 loco_command(i->first, (i->second.speed+1)/2, i->second.reverse, i->second.funcs, false);
307 for(map<unsigned, Turnout>::iterator i=turnouts.begin(); i!=turnouts.end(); ++i)
308 if(i->second.active && i->second.off_timeout && t>i->second.off_timeout)
310 i->second.active = false;
311 i->second.off_timeout = Time::TimeStamp();
312 for(unsigned j=0; j<i->second.bits; ++j)
313 turnout_command(i->first+j, !(i->second.state&(1<<j)), false);
316 for(map<unsigned, Sensor>::iterator i=sensors.begin(); i!=sensors.end(); ++i)
317 if(i->second.off_timeout && t>i->second.off_timeout)
319 i->second.state = false;
320 i->second.off_timeout = Time::TimeStamp();
321 signal_sensor.emit(i->first, false);
326 unsigned max_addr = (--sensors.end())->first;
327 unsigned char data[2];
329 data[1] = (max_addr+7)/8;
330 command(CMD_SENSOR_PARAM_SET, data, 2);
331 command(CMD_SENSOR_REPORT);
332 update_sensors = false;
335 if(!queue.empty() && command_sent)
337 pollfd pfd = { serial_fd, POLLIN, 0 };
338 if(poll(&pfd, 1, 0)>0)
341 queue.erase(queue.begin());
342 command_sent = false;
350 const CommandSlot &slot = queue.front();
351 write(serial_fd, slot.data, slot.length);
356 void Intellibox::flush()
358 Time::TimeStamp t = Time::now();
359 for(list<CommandSlot>::iterator i=queue.begin(); i!=queue.end(); ++i)
361 write(serial_fd, i->data, i->length);
362 pollfd pfd = { serial_fd, POLLIN, 0 };
364 while(poll(&pfd, 1, (first ? -1 : 0))>0)
367 read(serial_fd, data, 16);
373 command_sent = false;
376 Intellibox::Protocol Intellibox::map_protocol(const string &name) const
380 else if(name=="MM-27")
383 throw InvalidParameterValue("Unknown protocol");
386 void Intellibox::command(Command cmd)
391 void Intellibox::command(Command cmd, const unsigned char *data, unsigned len)
393 command(cmd, 0, data, len);
396 void Intellibox::command(Command cmd, unsigned addr, const unsigned char *data, unsigned len)
402 copy(data, data+len, slot.data+1);
404 queue.push_back(slot);
407 void Intellibox::loco_command(unsigned addr, unsigned speed, bool rev, unsigned funcs, bool setf)
409 unsigned char data[4];
411 data[1] = (addr>>8)&0xFF;
418 data[2] = (speed*19-18)/2;
420 data[3] = (rev ? 0 : 0x20) | ((funcs&1) ? 0x10 : 0);
423 data[3] |= 0x80 | ((funcs>>1)&0xF);
425 command(CMD_LOK, addr, data, 4);
428 void Intellibox::turnout_command(unsigned addr, bool state, bool active)
430 unsigned char data[2];
432 data[1] = ((addr>>8)&0x7) | (active ? 0x40 : 0) | (state ? 0x80 : 0);
433 command(CMD_TURNOUT, addr, data, 2);
436 void Intellibox::process_reply(const Time::TimeStamp &t)
438 Command cmd = queue.front().cmd;
442 unsigned char status;
443 read_all(&status, 1);
445 signal_power.emit(power);
447 else if(cmd==CMD_EVENT)
449 for(unsigned i=0;; ++i)
457 command(CMD_EVENT_LOK);
459 command(CMD_EVENT_TURNOUT);
461 command(CMD_EVENT_SENSOR);
473 else if(cmd==CMD_EVENT_LOK)
477 unsigned char data[5];
484 else if(cmd==CMD_EVENT_TURNOUT)
488 for(unsigned i=0; i<count; ++i)
490 unsigned char data[2];
493 unsigned addr = data[0]+((data[1]&7)<<8);
495 for(; !turnouts[addr].bits; --addr, mask<<=1) ;
496 Turnout &turnout = turnouts[addr];
498 unsigned bit = !(data[1]&0x80);
499 turnout.state = (turnout.state&~mask) | (bit*mask);
500 turnout.pending = turnout.state;
501 signal_turnout.emit(addr, turnout.state);
504 else if(cmd==CMD_EVENT_SENSOR)
513 unsigned char data[2];
515 for(unsigned i=0; i<16; ++i)
517 unsigned addr = mod*16+i-15;
518 bool state = (data[i/8]>>(7-i%8))&1;
520 Sensor &sensor = sensors[addr];
523 sensor.off_timeout = Time::TimeStamp();
526 sensor.state = state;
527 signal_sensor(addr, state);
530 else if(sensor.state)
531 sensor.off_timeout = t+700*Time::msec;
535 else if(cmd==CMD_LOK)
540 if(err==ERR_NO_ERROR)
542 unsigned addr = queue.front().addr;
543 Locomotive &loco = locos[addr];
546 signal_loco_speed.emit(addr, loco.speed+loco.pending_half_step, loco.reverse);
547 if(loco.pending_half_step)
548 loco.half_step_delay = Time::now()+500*Time::msec;
554 else if(cmd==CMD_TURNOUT)
559 unsigned addr = queue.front().addr;
561 for(; !turnouts[addr].bits; --addr, mask<<=1) ;
562 Turnout &turnout = turnouts[addr];
564 if(err==ERR_NO_ERROR)
566 turnout.state = (turnout.state&~mask) | (turnout.pending&mask);
569 if(turnout.state==turnout.pending)
570 signal_turnout.emit(addr, turnout.state);
571 turnout.off_timeout = t+500*Time::msec;
574 else if(err==ERR_NO_I2C_SPACE)
575 queue.push_back(queue.front());
578 turnout.pending = (turnout.pending&~mask) | (turnout.state&mask);
582 else if(cmd==CMD_TURNOUT_STATUS)
587 if(err==ERR_NO_ERROR)
592 unsigned addr = queue.front().addr;
594 for(; !turnouts[addr].bits; --addr, mask<<=1) ;
595 Turnout &turnout = turnouts[addr];
597 bool bit = !(data&0x04);
598 if(bit!=((turnout.state&mask)!=0))
600 turnout.state = (turnout.state&~mask) | (bit*mask);
601 turnout.pending = turnout.state;
602 signal_turnout.emit(addr, turnout.state);
608 else if(cmd==CMD_LOK_STATUS)
613 if(err==ERR_NO_ERROR)
615 unsigned char data[3];
618 unsigned addr = queue.front().addr;
619 Locomotive &loco = locos[addr];
621 unsigned speed = (data[0]<=1 ? 0 : data[0]*2/19+1);
622 bool reverse = !(data[1]&0x20);
623 if(speed!=loco.speed || reverse!=loco.reverse)
626 loco.reverse = reverse;
627 signal_loco_speed.emit(addr, loco.speed, loco.reverse);
630 unsigned funcs = (data[1]&0xF)<<1;
633 if(funcs!=loco.funcs)
635 unsigned changed = loco.funcs^funcs;
637 for(unsigned i=0; i<5; ++i)
639 signal_loco_function.emit(addr, i, loco.funcs&(1<<i));
647 unsigned expected_bytes = 0;
648 if(cmd==CMD_FUNC_STATUS)
650 if(cmd==CMD_TURNOUT_GROUP_STATUS)
652 if(cmd==CMD_LOK_CONFIG)
658 if(err==ERR_NO_ERROR)
660 unsigned char data[8];
661 read_all(data, expected_bytes);
668 unsigned Intellibox::read_all(unsigned char *buf, unsigned len)
672 pos += read(serial_fd, buf+pos, len-pos);
677 unsigned Intellibox::read_status(Error *err)
680 unsigned ret = read_all(&c, 1);
681 *err = static_cast<Error>(c);
685 void Intellibox::error(Command cmd, Error err)
687 const char *cmd_str = 0;
690 case CMD_LOK: cmd_str = "CMD_LOK"; break;
691 case CMD_LOK_STATUS: cmd_str = "CMD_LOK_STATUS"; break;
692 case CMD_LOK_CONFIG: cmd_str = "CMD_LOK_CONFIG"; break;
693 case CMD_FUNC: cmd_str = "CMD_FUNC"; break;
694 case CMD_FUNC_STATUS: cmd_str = "CMD_FUNC_STATUS"; break;
695 case CMD_TURNOUT: cmd_str = "CMD_TURNOUT"; break;
696 case CMD_TURNOUT_FREE: cmd_str = "CMD_TURNOUT_FREE"; break;
697 case CMD_TURNOUT_STATUS: cmd_str = "CMD_TURNOUT_STATUS"; break;
698 case CMD_TURNOUT_GROUP_STATUS: cmd_str = "CMD_TURNOUT_GROUP_STATUS"; break;
699 case CMD_SENSOR_STATUS: cmd_str = "CMD_SENSOR_STATUS"; break;
700 case CMD_SENSOR_REPORT: cmd_str = "CMD_SENSOR_REPORT"; break;
701 case CMD_SENSOR_PARAM_SET: cmd_str = "CMD_SENSOR_PARAM_SET"; break;
702 case CMD_STATUS: cmd_str = "CMD_STATUS"; break;
703 case CMD_POWER_OFF: cmd_str = "CMD_POWER_OFF"; break;
704 case CMD_POWER_ON: cmd_str = "CMD_POWER_ON"; break;
705 case CMD_NOP: cmd_str = "CMD_NOP"; break;
706 case CMD_EVENT: cmd_str = "CMD_EVENT"; break;
707 case CMD_EVENT_LOK: cmd_str = "CMD_EVENT_LOK"; break;
708 case CMD_EVENT_TURNOUT: cmd_str = "CMD_EVENT_TURNOUT"; break;
709 case CMD_EVENT_SENSOR: cmd_str = "CMD_EVENT_SENSOR"; break;
710 default: cmd_str = "(unknown command)";
713 const char *err_str = 0;
716 case ERR_NO_ERROR: err_str = "ERR_NO_ERROR"; break;
717 case ERR_SYS_ERROR: err_str = "ERR_SYS_ERROR"; break;
718 case ERR_BAD_PARAM: err_str = "ERR_BAD_PARAM"; break;
719 case ERR_POWER_OFF: err_str = "ERR_POWER_OFF"; break;
720 case ERR_NO_LOK_SPACE: err_str = "ERR_NO_LOK_SPACE"; break;
721 case ERR_NO_TURNOUT_SPACE: err_str = "ERR_NO_TURNOUT_SPACE"; break;
722 case ERR_NO_DATA: err_str = "ERR_NO_DATA"; break;
723 case ERR_NO_SLOT: err_str = "ERR_NO_SLOT"; break;
724 case ERR_BAD_LOK_ADDR: err_str = "ERR_BAD_LOK_ADDR"; break;
725 case ERR_LOK_BUSY: err_str = "ERR_LOK_BUSY"; break;
726 case ERR_BAD_TURNOUT_ADDR: err_str = "ERR_BAD_TURNOUT_ADDR"; break;
727 case ERR_BAD_SO_VALUE: err_str = "ERR_BAD_SO_VALUE"; break;
728 case ERR_NO_I2C_SPACE: err_str = "ERR_NO_I2C_SPACE"; break;
729 case ERR_LOW_TURNOUT_SPACE: err_str = "ERR_LOW_TURNOUT_SPACE"; break;
730 case ERR_LOK_HALTED: err_str = "ERR_LOK_HALTED"; break;
731 case ERR_LOK_POWER_OFF: err_str = "ERR_LOK_POWER_OFF"; break;
732 default: cmd_str = "(unknown error)";
735 IO::print("Error: %s: %s\n", cmd_str, err_str);
739 Intellibox::Locomotive::Locomotive():
747 Intellibox::Turnout::Turnout():
755 Intellibox::Sensor::Sensor():