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"
21 Intellibox::Intellibox(const string &dev):
24 update_sensors(false),
27 serial_fd = ::open(dev.c_str(), O_RDWR);
29 throw Exception("Couldn't open serial port\n");
31 static unsigned baud[]=
41 tcgetattr(serial_fd, &attr);
43 attr.c_cflag |= CSTOPB;
47 for(unsigned i=0; baud[i]; i+=2)
49 cfsetospeed(&attr, baud[i+1]);
50 tcsetattr(serial_fd, TCSADRAIN, &attr);
52 write(serial_fd, "\xC4", 1);
54 pollfd pfd = { serial_fd, POLLIN, 0 };
55 if(poll(&pfd, 1, 500)>0)
57 IO::print("IB detected at %d bits/s\n", baud[i]);
59 p50 = (read(serial_fd, buf, 2)==2);
66 throw Exception("IB not detected");
69 write(serial_fd, "xZzA1\r", 6);
74 void Intellibox::set_power(bool p)
78 command(CMD_POWER_ON);
80 command(CMD_POWER_OFF);
81 signal_power.emit(power);
84 void Intellibox::halt(bool h)
89 for(map<unsigned, Locomotive>::iterator i=locos.begin(); i!=locos.end(); ++i)
91 set_loco_speed(i->first, 0);
94 signal_halt.emit(halted);
97 const char *Intellibox::enumerate_protocols(unsigned i) const
106 unsigned Intellibox::get_protocol_speed_steps(const string &proto_name) const
108 Protocol proto = map_protocol(proto_name);
111 else if(proto==MM_27)
116 void Intellibox::add_loco(unsigned addr, const string &proto_name)
118 Protocol proto = map_protocol(proto_name);
120 if(!locos.count(addr))
122 locos[addr].protocol = proto;
124 unsigned char data[2];
126 data[1] = (addr>>8)&0xFF;
127 command(CMD_LOK_STATUS, addr, data, 2);
131 void Intellibox::set_loco_speed(unsigned addr, unsigned speed)
133 Locomotive &loco = locos[addr];
134 if(speed==loco.speed)
136 if(loco.pending_half_step)
138 loco.pending_half_step = 0;
139 loco.half_step_delay = Time::TimeStamp();
140 signal_loco_speed.emit(addr, speed, loco.reverse);
147 if(loco.protocol==MM_27)
152 if(speed>loco.speed && !(speed&1))
154 loco.pending_half_step = -1;
157 else if(speed<loco.speed && (speed&1))
159 loco.pending_half_step = 1;
163 loco.pending_half_step = 0;
164 loco.half_step_delay = Time::TimeStamp();
166 loco_command(addr, (speed+1)/2, loco.reverse, loco.funcs|0x100);
173 loco_command(addr, speed, loco.reverse, loco.funcs|0x100);
178 void Intellibox::set_loco_reverse(unsigned addr, bool rev)
180 Locomotive &loco = locos[addr];
181 if(rev==loco.reverse)
185 loco_command(addr, loco.speed, rev, loco.funcs|0x100);
188 void Intellibox::set_loco_function(unsigned addr, unsigned func, bool state)
190 Locomotive &loco = locos[addr];
192 loco.funcs |= 1<<func;
194 loco.funcs &= ~(1<<func);
195 loco_command(addr, loco.speed, loco.reverse, loco.funcs);
196 signal_loco_function.emit(addr, func, state);
199 void Intellibox::add_turnout(unsigned addr)
201 if(!turnouts.count(addr))
205 unsigned char data[2];
207 data[1] = (addr>>8)&0xFF;
208 command(CMD_TURNOUT_STATUS, addr, data, 2);
212 void Intellibox::set_turnout(unsigned addr, bool state)
214 Turnout &turnout = turnouts[addr];
215 if(state==turnout.state || state==turnout.pending)
218 turnout.pending = state;
219 turnout.active = true;
220 turnout.off_timeout = Time::TimeStamp();
222 turnout_command(addr, state, true);
225 bool Intellibox::get_turnout(unsigned addr) const
227 map<unsigned, Turnout>::const_iterator i = turnouts.find(addr);
228 if(i!=turnouts.end())
229 return i->second.state;
233 void Intellibox::add_sensor(unsigned addr)
235 if(!sensors.count(addr))
238 update_sensors = true;
242 bool Intellibox::get_sensor(unsigned addr) const
244 map<unsigned, Sensor>::const_iterator i = sensors.find(addr);
246 return i->second.state;
250 void Intellibox::tick()
252 const Time::TimeStamp t = Time::now();
254 if(t>next_event_query)
256 next_event_query = t+200*Time::msec;
260 for(map<unsigned, Locomotive>::iterator i=locos.begin(); i!=locos.end(); ++i)
261 if(i->second.protocol==MM_27 && i->second.pending_half_step && i->second.half_step_delay && t>i->second.half_step_delay)
263 i->second.speed += i->second.pending_half_step;
264 i->second.pending_half_step = 0;
265 i->second.half_step_delay = Time::TimeStamp();
266 loco_command(i->first, (i->second.speed+1)/2, i->second.reverse, i->second.funcs|0x100);
269 for(map<unsigned, Turnout>::iterator i=turnouts.begin(); i!=turnouts.end(); ++i)
270 if(i->second.active && i->second.off_timeout && t>i->second.off_timeout)
272 i->second.active = false;
273 i->second.off_timeout = Time::TimeStamp();
274 turnout_command(i->first, i->second.state, false);
277 for(map<unsigned, Sensor>::iterator i=sensors.begin(); i!=sensors.end(); ++i)
278 if(i->second.off_timeout && t>i->second.off_timeout)
280 i->second.state = false;
281 i->second.off_timeout = Time::TimeStamp();
282 signal_sensor.emit(i->first, false);
287 unsigned max_addr = (--sensors.end())->first;
288 unsigned char data[2];
290 data[1] = (max_addr+7)/8;
291 command(CMD_SENSOR_PARAM_SET, data, 2);
292 command(CMD_SENSOR_REPORT);
293 update_sensors = false;
296 if(!queue.empty() && command_sent)
298 pollfd pfd = { serial_fd, POLLIN, 0 };
299 if(poll(&pfd, 1, 0)>0)
302 queue.erase(queue.begin());
303 command_sent = false;
311 const CommandSlot &slot = queue.front();
312 write(serial_fd, slot.data, slot.length);
317 void Intellibox::flush()
319 Time::TimeStamp t = Time::now();
320 for(list<CommandSlot>::iterator i=queue.begin(); i!=queue.end(); ++i)
322 write(serial_fd, i->data, i->length);
323 pollfd pfd = { serial_fd, POLLIN, 0 };
325 while(poll(&pfd, 1, (first ? -1 : 0))>0)
328 read(serial_fd, data, 16);
334 command_sent = false;
337 Intellibox::Protocol Intellibox::map_protocol(const string &name) const
341 else if(name=="MM-27")
344 throw InvalidParameterValue("Unknown protocol");
347 void Intellibox::command(Command cmd)
352 void Intellibox::command(Command cmd, const unsigned char *data, unsigned len)
354 command(cmd, 0, data, len);
357 void Intellibox::command(Command cmd, unsigned addr, const unsigned char *data, unsigned len)
363 copy(data, data+len, slot.data+1);
365 queue.push_back(slot);
368 void Intellibox::loco_command(unsigned addr, unsigned speed, bool rev, unsigned funcs)
370 unsigned char data[4];
372 data[1] = (addr>>8)&0xFF;
379 data[2] = (speed*19-18)/2;
381 data[3] = (rev ? 0 : 0x20) | ((funcs&1) ? 0x10 : 0);
384 data[3] |= 0x80 | ((funcs>>1)&0xF);
386 command(CMD_LOK, addr, data, 4);
389 void Intellibox::turnout_command(unsigned addr, bool state, bool active)
391 unsigned char data[2];
393 data[1] = ((addr>>8)&0x7) | (active ? 0x40 : 0) | (state ? 0x80 : 0);
394 command(CMD_TURNOUT, addr, data, 2);
397 void Intellibox::process_reply(const Time::TimeStamp &t)
399 Command cmd = queue.front().cmd;
403 unsigned char status;
404 read_all(&status, 1);
406 signal_power.emit(power);
408 else if(cmd==CMD_EVENT)
410 for(unsigned i=0;; ++i)
418 command(CMD_EVENT_LOK);
420 command(CMD_EVENT_TURNOUT);
422 command(CMD_EVENT_SENSOR);
434 else if(cmd==CMD_EVENT_LOK)
438 unsigned char data[5];
445 else if(cmd==CMD_EVENT_TURNOUT)
449 for(unsigned i=0; i<count; ++i)
451 unsigned char data[2];
454 unsigned addr = data[0]+((data[1]&7)<<8);
455 Turnout &turnout = turnouts[addr];
456 turnout.state = (data[1]&0x80)!=0;
457 turnout.pending = turnout.state;
458 signal_turnout.emit(addr, turnout.state);
461 else if(cmd==CMD_EVENT_SENSOR)
470 unsigned char data[2];
472 for(unsigned i=0; i<16; ++i)
474 unsigned addr = mod*16+i-15;
475 bool state = (data[i/8]>>(7-i%8))&1;
477 Sensor &sensor = sensors[addr];
480 sensor.off_timeout = Time::TimeStamp();
483 sensor.state = state;
484 signal_sensor(addr, state);
487 else if(sensor.state)
488 sensor.off_timeout = t+700*Time::msec;
492 else if(cmd==CMD_LOK)
497 if(err==ERR_NO_ERROR)
499 unsigned addr = queue.front().addr;
500 Locomotive &loco = locos[addr];
501 signal_loco_speed.emit(addr, loco.speed+loco.pending_half_step, loco.reverse);
502 if(loco.pending_half_step)
503 loco.half_step_delay = Time::now()+500*Time::msec;
508 else if(cmd==CMD_TURNOUT)
513 unsigned addr = queue.front().addr;
514 Turnout &turnout = turnouts[addr];
516 if(err==ERR_NO_ERROR)
518 turnout.state = turnout.pending;
521 signal_turnout.emit(addr, turnout.state);
522 turnout.off_timeout = t+500*Time::msec;
525 else if(err==ERR_NO_I2C_SPACE)
526 queue.push_back(queue.front());
529 turnout.pending = turnout.state;
533 else if(cmd==CMD_TURNOUT_STATUS)
538 if(err==ERR_NO_ERROR)
543 unsigned addr = queue.front().addr;
544 bool state = data&0x04;
546 Turnout &turnout = turnouts[addr];
547 if(state!=turnout.state)
549 turnout.state = state;
550 turnout.pending = state;
551 signal_turnout.emit(addr, turnout.state);
557 else if(cmd==CMD_LOK_STATUS)
562 if(err==ERR_NO_ERROR)
564 unsigned char data[3];
567 unsigned addr = queue.front().addr;
568 Locomotive &loco = locos[addr];
570 unsigned speed = (data[0]<=1 ? 0 : data[0]*2/19+1);
571 bool reverse = !(data[1]&0x20);
572 if(speed!=loco.speed || reverse!=loco.reverse)
575 loco.reverse = reverse;
576 signal_loco_speed.emit(addr, loco.speed, loco.reverse);
579 unsigned funcs = (data[1]&0xF)<<1;
582 if(funcs!=loco.funcs)
584 unsigned changed = loco.funcs^funcs;
586 for(unsigned i=0; i<5; ++i)
588 signal_loco_function.emit(addr, i, loco.funcs&(1<<i));
596 unsigned expected_bytes = 0;
597 if(cmd==CMD_FUNC_STATUS)
599 if(cmd==CMD_TURNOUT_GROUP_STATUS)
601 if(cmd==CMD_LOK_CONFIG)
607 if(err==ERR_NO_ERROR)
609 unsigned char data[8];
610 read_all(data, expected_bytes);
617 unsigned Intellibox::read_all(unsigned char *buf, unsigned len)
621 pos += read(serial_fd, buf+pos, len-pos);
626 unsigned Intellibox::read_status(Error *err)
629 unsigned ret = read_all(&c, 1);
630 *err = static_cast<Error>(c);
634 void Intellibox::error(Command cmd, Error err)
636 const char *cmd_str = 0;
639 case CMD_LOK: cmd_str = "CMD_LOK"; break;
640 case CMD_LOK_STATUS: cmd_str = "CMD_LOK_STATUS"; break;
641 case CMD_LOK_CONFIG: cmd_str = "CMD_LOK_CONFIG"; break;
642 case CMD_FUNC: cmd_str = "CMD_FUNC"; break;
643 case CMD_FUNC_STATUS: cmd_str = "CMD_FUNC_STATUS"; break;
644 case CMD_TURNOUT: cmd_str = "CMD_TURNOUT"; break;
645 case CMD_TURNOUT_FREE: cmd_str = "CMD_TURNOUT_FREE"; break;
646 case CMD_TURNOUT_STATUS: cmd_str = "CMD_TURNOUT_STATUS"; break;
647 case CMD_TURNOUT_GROUP_STATUS: cmd_str = "CMD_TURNOUT_GROUP_STATUS"; break;
648 case CMD_SENSOR_STATUS: cmd_str = "CMD_SENSOR_STATUS"; break;
649 case CMD_SENSOR_REPORT: cmd_str = "CMD_SENSOR_REPORT"; break;
650 case CMD_SENSOR_PARAM_SET: cmd_str = "CMD_SENSOR_PARAM_SET"; break;
651 case CMD_STATUS: cmd_str = "CMD_STATUS"; break;
652 case CMD_POWER_OFF: cmd_str = "CMD_POWER_OFF"; break;
653 case CMD_POWER_ON: cmd_str = "CMD_POWER_ON"; break;
654 case CMD_NOP: cmd_str = "CMD_NOP"; break;
655 case CMD_EVENT: cmd_str = "CMD_EVENT"; break;
656 case CMD_EVENT_LOK: cmd_str = "CMD_EVENT_LOK"; break;
657 case CMD_EVENT_TURNOUT: cmd_str = "CMD_EVENT_TURNOUT"; break;
658 case CMD_EVENT_SENSOR: cmd_str = "CMD_EVENT_SENSOR"; break;
659 default: cmd_str = "(unknown command)";
662 const char *err_str = 0;
665 case ERR_NO_ERROR: err_str = "ERR_NO_ERROR"; break;
666 case ERR_SYS_ERROR: err_str = "ERR_SYS_ERROR"; break;
667 case ERR_BAD_PARAM: err_str = "ERR_BAD_PARAM"; break;
668 case ERR_POWER_OFF: err_str = "ERR_POWER_OFF"; break;
669 case ERR_NO_LOK_SPACE: err_str = "ERR_NO_LOK_SPACE"; break;
670 case ERR_NO_TURNOUT_SPACE: err_str = "ERR_NO_TURNOUT_SPACE"; break;
671 case ERR_NO_DATA: err_str = "ERR_NO_DATA"; break;
672 case ERR_NO_SLOT: err_str = "ERR_NO_SLOT"; break;
673 case ERR_BAD_LOK_ADDR: err_str = "ERR_BAD_LOK_ADDR"; break;
674 case ERR_LOK_BUSY: err_str = "ERR_LOK_BUSY"; break;
675 case ERR_BAD_TURNOUT_ADDR: err_str = "ERR_BAD_TURNOUT_ADDR"; break;
676 case ERR_BAD_SO_VALUE: err_str = "ERR_BAD_SO_VALUE"; break;
677 case ERR_NO_I2C_SPACE: err_str = "ERR_NO_I2C_SPACE"; break;
678 case ERR_LOW_TURNOUT_SPACE: err_str = "ERR_LOW_TURNOUT_SPACE"; break;
679 case ERR_LOK_HALTED: err_str = "ERR_LOK_HALTED"; break;
680 case ERR_LOK_POWER_OFF: err_str = "ERR_LOK_POWER_OFF"; break;
681 default: cmd_str = "(unknown error)";
684 IO::print("Error: %s: %s\n", cmd_str, err_str);
688 Intellibox::Locomotive::Locomotive():
695 Intellibox::Turnout::Turnout():
702 Intellibox::Sensor::Sensor():