--- /dev/null
+/* $Id$
+
+This file is part of R²C²
+Copyright © 2010 Mikkosoft Productions, Mikko Rasa
+Distributed under the GPL
+*/
+
+#include <fcntl.h>
+#include <termios.h>
+#include <sys/poll.h>
+#include <msp/io/print.h>
+#include <msp/time/units.h>
+#include <msp/time/utils.h>
+#include "intellibox.h"
+
+using namespace std;
+using namespace Msp;
+
+namespace R2C2 {
+
+Intellibox::Intellibox(const string &dev):
+ power(false),
+ halted(false),
+ update_sensors(false),
+ command_sent(false)
+{
+ serial_fd = ::open(dev.c_str(), O_RDWR);
+ if(serial_fd<0)
+ throw Exception("Couldn't open serial port\n");
+
+ static unsigned baud[]=
+ {
+ 2400, B2400,
+ 4800, B4800,
+ 9600, B9600,
+ 19200, B19200,
+ 0
+ };
+
+ termios attr;
+ tcgetattr(serial_fd, &attr);
+ cfmakeraw(&attr);
+ attr.c_cflag |= CSTOPB;
+
+ bool ok = false;
+ bool p50 = false;
+ for(unsigned i=0; baud[i]; i+=2)
+ {
+ cfsetospeed(&attr, baud[i+1]);
+ tcsetattr(serial_fd, TCSADRAIN, &attr);
+
+ write(serial_fd, "\xC4", 1);
+
+ pollfd pfd = { serial_fd, POLLIN, 0 };
+ if(poll(&pfd, 1, 500)>0)
+ {
+ IO::print("IB detected at %d bits/s\n", baud[i]);
+ char buf[2];
+ p50 = (read(serial_fd, buf, 2)==2);
+ ok = true;
+ break;
+ }
+ }
+
+ if(!ok)
+ throw Exception("IB not detected");
+
+ if(p50)
+ write(serial_fd, "xZzA1\r", 6);
+
+ command(CMD_STATUS);
+}
+
+void Intellibox::set_power(bool p)
+{
+ power = p;
+ if(power)
+ command(CMD_POWER_ON);
+ else
+ command(CMD_POWER_OFF);
+ signal_power.emit(power);
+}
+
+void Intellibox::halt(bool h)
+{
+ halted = h;
+ if(halted)
+ {
+ for(map<unsigned, Locomotive>::iterator i=locos.begin(); i!=locos.end(); ++i)
+ if(i->second.speed)
+ set_loco_speed(i->first, 0);
+ }
+
+ signal_halt.emit(halted);
+}
+
+const char *Intellibox::enumerate_protocols(unsigned i) const
+{
+ if(i==MM)
+ return "MM";
+ else if(i==MM_27)
+ return "MM-27";
+ return 0;
+}
+
+unsigned Intellibox::get_protocol_speed_steps(const string &proto_name) const
+{
+ Protocol proto = map_protocol(proto_name);
+ if(proto==MM)
+ return 14;
+ else if(proto==MM_27)
+ return 27;
+ return 0;
+}
+
+void Intellibox::add_loco(unsigned addr, const string &proto_name)
+{
+ Protocol proto = map_protocol(proto_name);
+
+ if(!locos.count(addr))
+ {
+ locos[addr].protocol = proto;
+
+ unsigned char data[2];
+ data[0] = addr&0xFF;
+ data[1] = (addr>>8)&0xFF;
+ command(CMD_LOK_STATUS, addr, data, 2);
+ }
+}
+
+void Intellibox::set_loco_speed(unsigned addr, unsigned speed)
+{
+ Locomotive &loco = locos[addr];
+ if(speed==loco.speed)
+ {
+ if(loco.pending_half_step)
+ {
+ loco.pending_half_step = 0;
+ loco.half_step_delay = Time::TimeStamp();
+ signal_loco_speed.emit(addr, speed, loco.reverse);
+ }
+ return;
+ }
+ if(speed && halted)
+ return;
+
+ if(loco.protocol==MM_27)
+ {
+ if(speed>27)
+ speed = 27;
+
+ if(speed>loco.speed && !(speed&1))
+ {
+ loco.pending_half_step = -1;
+ speed |= 1;
+ }
+ else if(speed<loco.speed && (speed&1))
+ {
+ loco.pending_half_step = 1;
+ speed &= ~1;
+ }
+ else
+ loco.pending_half_step = 0;
+ loco.half_step_delay = Time::TimeStamp();
+
+ loco_command(addr, (speed+1)/2, loco.reverse, loco.funcs|0x100);
+ }
+ else
+ {
+ if(speed>14)
+ speed = 14;
+
+ loco_command(addr, speed, loco.reverse, loco.funcs|0x100);
+ }
+ loco.speed = speed;
+}
+
+void Intellibox::set_loco_reverse(unsigned addr, bool rev)
+{
+ Locomotive &loco = locos[addr];
+ if(rev==loco.reverse)
+ return;
+
+ loco.reverse = rev;
+ loco_command(addr, loco.speed, rev, loco.funcs|0x100);
+}
+
+void Intellibox::set_loco_function(unsigned addr, unsigned func, bool state)
+{
+ Locomotive &loco = locos[addr];
+ if(state)
+ loco.funcs |= 1<<func;
+ else
+ loco.funcs &= ~(1<<func);
+ loco_command(addr, loco.speed, loco.reverse, loco.funcs);
+ signal_loco_function.emit(addr, func, state);
+}
+
+void Intellibox::add_turnout(unsigned addr)
+{
+ if(!turnouts.count(addr))
+ {
+ turnouts[addr];
+
+ unsigned char data[2];
+ data[0] = addr&0xFF;
+ data[1] = (addr>>8)&0xFF;
+ command(CMD_TURNOUT_STATUS, addr, data, 2);
+ }
+}
+
+void Intellibox::set_turnout(unsigned addr, bool state)
+{
+ Turnout &turnout = turnouts[addr];
+ if(state==turnout.state || state==turnout.pending)
+ return;
+
+ turnout.pending = state;
+ turnout.active = true;
+ turnout.off_timeout = Time::TimeStamp();
+
+ turnout_command(addr, state, true);
+}
+
+bool Intellibox::get_turnout(unsigned addr) const
+{
+ map<unsigned, Turnout>::const_iterator i = turnouts.find(addr);
+ if(i!=turnouts.end())
+ return i->second.state;
+ return false;
+}
+
+void Intellibox::add_sensor(unsigned addr)
+{
+ if(!sensors.count(addr))
+ {
+ sensors[addr];
+ update_sensors = true;
+ }
+}
+
+bool Intellibox::get_sensor(unsigned addr) const
+{
+ map<unsigned, Sensor>::const_iterator i = sensors.find(addr);
+ if(i!=sensors.end())
+ return i->second.state;
+ return false;
+}
+
+void Intellibox::tick()
+{
+ const Time::TimeStamp t = Time::now();
+
+ if(t>next_event_query)
+ {
+ next_event_query = t+200*Time::msec;
+ command(CMD_EVENT);
+ }
+
+ for(map<unsigned, Locomotive>::iterator i=locos.begin(); i!=locos.end(); ++i)
+ if(i->second.protocol==MM_27 && i->second.pending_half_step && i->second.half_step_delay && t>i->second.half_step_delay)
+ {
+ i->second.speed += i->second.pending_half_step;
+ i->second.pending_half_step = 0;
+ i->second.half_step_delay = Time::TimeStamp();
+ loco_command(i->first, (i->second.speed+1)/2, i->second.reverse, i->second.funcs|0x100);
+ }
+
+ for(map<unsigned, Turnout>::iterator i=turnouts.begin(); i!=turnouts.end(); ++i)
+ if(i->second.active && i->second.off_timeout && t>i->second.off_timeout)
+ {
+ i->second.active = false;
+ i->second.off_timeout = Time::TimeStamp();
+ turnout_command(i->first, i->second.state, false);
+ }
+
+ for(map<unsigned, Sensor>::iterator i=sensors.begin(); i!=sensors.end(); ++i)
+ if(i->second.off_timeout && t>i->second.off_timeout)
+ {
+ i->second.state = false;
+ i->second.off_timeout = Time::TimeStamp();
+ signal_sensor.emit(i->first, false);
+ }
+
+ if(update_sensors)
+ {
+ unsigned max_addr = (--sensors.end())->first;
+ unsigned char data[2];
+ data[0] = 0;
+ data[1] = (max_addr+7)/8;
+ command(CMD_SENSOR_PARAM_SET, data, 2);
+ command(CMD_SENSOR_REPORT);
+ update_sensors = false;
+ }
+
+ if(!queue.empty() && command_sent)
+ {
+ pollfd pfd = { serial_fd, POLLIN, 0 };
+ if(poll(&pfd, 1, 0)>0)
+ {
+ process_reply(t);
+ queue.erase(queue.begin());
+ command_sent = false;
+ }
+ else
+ return;
+ }
+
+ if(!queue.empty())
+ {
+ const CommandSlot &slot = queue.front();
+ write(serial_fd, slot.data, slot.length);
+ command_sent = true;
+ }
+}
+
+void Intellibox::flush()
+{
+ Time::TimeStamp t = Time::now();
+ for(list<CommandSlot>::iterator i=queue.begin(); i!=queue.end(); ++i)
+ {
+ write(serial_fd, i->data, i->length);
+ pollfd pfd = { serial_fd, POLLIN, 0 };
+ bool first = true;
+ while(poll(&pfd, 1, (first ? -1 : 0))>0)
+ {
+ char data[16];
+ read(serial_fd, data, 16);
+ first = false;
+ }
+ }
+
+ queue.clear();
+ command_sent = false;
+}
+
+Intellibox::Protocol Intellibox::map_protocol(const string &name) const
+{
+ if(name=="MM")
+ return MM;
+ else if(name=="MM-27")
+ return MM_27;
+ else
+ throw InvalidParameterValue("Unknown protocol");
+}
+
+void Intellibox::command(Command cmd)
+{
+ command(cmd, 0, 0);
+}
+
+void Intellibox::command(Command cmd, const unsigned char *data, unsigned len)
+{
+ command(cmd, 0, data, len);
+}
+
+void Intellibox::command(Command cmd, unsigned addr, const unsigned char *data, unsigned len)
+{
+ CommandSlot slot;
+ slot.cmd = cmd;
+ slot.addr = addr;
+ slot.data[0] = cmd;
+ copy(data, data+len, slot.data+1);
+ slot.length = 1+len;
+ queue.push_back(slot);
+}
+
+void Intellibox::loco_command(unsigned addr, unsigned speed, bool rev, unsigned funcs)
+{
+ unsigned char data[4];
+ data[0] = addr&0xFF;
+ data[1] = (addr>>8)&0xFF;
+
+ if(speed==0)
+ data[2] = 0;
+ else if(speed==1)
+ data[2] = 2;
+ else
+ data[2] = (speed*19-18)/2;
+
+ data[3] = (rev ? 0 : 0x20) | ((funcs&1) ? 0x10 : 0);
+
+ if(!(funcs&0x100))
+ data[3] |= 0x80 | ((funcs>>1)&0xF);
+
+ command(CMD_LOK, addr, data, 4);
+}
+
+void Intellibox::turnout_command(unsigned addr, bool state, bool active)
+{
+ unsigned char data[2];
+ data[0] = addr&0xFF;
+ data[1] = ((addr>>8)&0x7) | (active ? 0x40 : 0) | (state ? 0x80 : 0);
+ command(CMD_TURNOUT, addr, data, 2);
+}
+
+void Intellibox::process_reply(const Time::TimeStamp &t)
+{
+ Command cmd = queue.front().cmd;
+
+ if(cmd==CMD_STATUS)
+ {
+ unsigned char status;
+ read_all(&status, 1);
+ power = status&0x08;
+ signal_power.emit(power);
+ }
+ else if(cmd==CMD_EVENT)
+ {
+ for(unsigned i=0;; ++i)
+ {
+ unsigned char byte;
+ read_all(&byte, 1);
+
+ if(i==0)
+ {
+ if(byte&0x01)
+ command(CMD_EVENT_LOK);
+ if(byte&0x20)
+ command(CMD_EVENT_TURNOUT);
+ if(byte&0x04)
+ command(CMD_EVENT_SENSOR);
+ }
+ else if(i==1)
+ {
+ if(byte&0x40)
+ command(CMD_STATUS);
+ }
+
+ if(!(byte&0x80))
+ break;
+ }
+ }
+ else if(cmd==CMD_EVENT_LOK)
+ {
+ while(1)
+ {
+ unsigned char data[5];
+ read_all(data, 1);
+ if(data[0]==0x80)
+ break;
+ read_all(data+1, 4);
+ }
+ }
+ else if(cmd==CMD_EVENT_TURNOUT)
+ {
+ unsigned char count;
+ read_all(&count, 1);
+ for(unsigned i=0; i<count; ++i)
+ {
+ unsigned char data[2];
+ read_all(data, 2);
+
+ unsigned addr = data[0]+((data[1]&7)<<8);
+ Turnout &turnout = turnouts[addr];
+ turnout.state = (data[1]&0x80)!=0;
+ turnout.pending = turnout.state;
+ signal_turnout.emit(addr, turnout.state);
+ }
+ }
+ else if(cmd==CMD_EVENT_SENSOR)
+ {
+ while(1)
+ {
+ unsigned char mod;
+ read_all(&mod, 1);
+ if(!mod)
+ break;
+
+ unsigned char data[2];
+ read_all(data, 2);
+ for(unsigned i=0; i<16; ++i)
+ {
+ unsigned addr = mod*16+i-15;
+ bool state = (data[i/8]>>(7-i%8))&1;
+
+ Sensor &sensor = sensors[addr];
+ if(state)
+ {
+ sensor.off_timeout = Time::TimeStamp();
+ if(!sensor.state)
+ {
+ sensor.state = state;
+ signal_sensor(addr, state);
+ }
+ }
+ else if(sensor.state)
+ sensor.off_timeout = t+700*Time::msec;
+ }
+ }
+ }
+ else if(cmd==CMD_LOK)
+ {
+ Error err;
+ read_status(&err);
+
+ if(err==ERR_NO_ERROR)
+ {
+ unsigned addr = queue.front().addr;
+ Locomotive &loco = locos[addr];
+ signal_loco_speed.emit(addr, loco.speed+loco.pending_half_step, loco.reverse);
+ if(loco.pending_half_step)
+ loco.half_step_delay = Time::now()+500*Time::msec;
+ }
+ else
+ error(cmd, err);
+ }
+ else if(cmd==CMD_TURNOUT)
+ {
+ Error err;
+ read_status(&err);
+
+ unsigned addr = queue.front().addr;
+ Turnout &turnout = turnouts[addr];
+
+ if(err==ERR_NO_ERROR)
+ {
+ turnout.state = turnout.pending;
+ if(turnout.active)
+ {
+ signal_turnout.emit(addr, turnout.state);
+ turnout.off_timeout = t+500*Time::msec;
+ }
+ }
+ else if(err==ERR_NO_I2C_SPACE)
+ queue.push_back(queue.front());
+ else
+ {
+ turnout.pending = turnout.state;
+ error(cmd, err);
+ }
+ }
+ else if(cmd==CMD_TURNOUT_STATUS)
+ {
+ Error err;
+ read_status(&err);
+
+ if(err==ERR_NO_ERROR)
+ {
+ unsigned char data;
+ read_all(&data, 1);
+
+ unsigned addr = queue.front().addr;
+ bool state = data&0x04;
+
+ Turnout &turnout = turnouts[addr];
+ if(state!=turnout.state)
+ {
+ turnout.state = state;
+ turnout.pending = state;
+ signal_turnout.emit(addr, turnout.state);
+ }
+ }
+ else
+ error(cmd, err);
+ }
+ else if(cmd==CMD_LOK_STATUS)
+ {
+ Error err;
+ read_status(&err);
+
+ if(err==ERR_NO_ERROR)
+ {
+ unsigned char data[3];
+ read_all(data, 3);
+
+ unsigned addr = queue.front().addr;
+ Locomotive &loco = locos[addr];
+
+ unsigned speed = (data[0]<=1 ? 0 : data[0]*2/19+1);
+ bool reverse = !(data[1]&0x20);
+ if(speed!=loco.speed || reverse!=loco.reverse)
+ {
+ loco.speed = speed;
+ loco.reverse = reverse;
+ signal_loco_speed.emit(addr, loco.speed, loco.reverse);
+ }
+
+ unsigned funcs = (data[1]&0xF)<<1;
+ if(data[1]&0x10)
+ funcs |= 1;
+ if(funcs!=loco.funcs)
+ {
+ unsigned changed = loco.funcs^funcs;
+ loco.funcs = funcs;
+ for(unsigned i=0; i<5; ++i)
+ if(changed&(1<<i))
+ signal_loco_function.emit(addr, i, loco.funcs&(1<<i));
+ }
+ }
+ else
+ error(cmd, err);
+ }
+ else
+ {
+ unsigned expected_bytes = 0;
+ if(cmd==CMD_FUNC_STATUS)
+ expected_bytes = 1;
+ if(cmd==CMD_TURNOUT_GROUP_STATUS)
+ expected_bytes = 2;
+ if(cmd==CMD_LOK_CONFIG)
+ expected_bytes = 4;
+
+ Error err;
+ read_status(&err);
+
+ if(err==ERR_NO_ERROR)
+ {
+ unsigned char data[8];
+ read_all(data, expected_bytes);
+ }
+ else
+ error(cmd, err);
+ }
+}
+
+unsigned Intellibox::read_all(unsigned char *buf, unsigned len)
+{
+ unsigned pos = 0;
+ while(pos<len)
+ pos += read(serial_fd, buf+pos, len-pos);
+
+ return pos;
+}
+
+unsigned Intellibox::read_status(Error *err)
+{
+ unsigned char c;
+ unsigned ret = read_all(&c, 1);
+ *err = static_cast<Error>(c);
+ return ret;
+}
+
+void Intellibox::error(Command cmd, Error err)
+{
+ const char *cmd_str = 0;
+ switch(cmd)
+ {
+ case CMD_LOK: cmd_str = "CMD_LOK"; break;
+ case CMD_LOK_STATUS: cmd_str = "CMD_LOK_STATUS"; break;
+ case CMD_LOK_CONFIG: cmd_str = "CMD_LOK_CONFIG"; break;
+ case CMD_FUNC: cmd_str = "CMD_FUNC"; break;
+ case CMD_FUNC_STATUS: cmd_str = "CMD_FUNC_STATUS"; break;
+ case CMD_TURNOUT: cmd_str = "CMD_TURNOUT"; break;
+ case CMD_TURNOUT_FREE: cmd_str = "CMD_TURNOUT_FREE"; break;
+ case CMD_TURNOUT_STATUS: cmd_str = "CMD_TURNOUT_STATUS"; break;
+ case CMD_TURNOUT_GROUP_STATUS: cmd_str = "CMD_TURNOUT_GROUP_STATUS"; break;
+ case CMD_SENSOR_STATUS: cmd_str = "CMD_SENSOR_STATUS"; break;
+ case CMD_SENSOR_REPORT: cmd_str = "CMD_SENSOR_REPORT"; break;
+ case CMD_SENSOR_PARAM_SET: cmd_str = "CMD_SENSOR_PARAM_SET"; break;
+ case CMD_STATUS: cmd_str = "CMD_STATUS"; break;
+ case CMD_POWER_OFF: cmd_str = "CMD_POWER_OFF"; break;
+ case CMD_POWER_ON: cmd_str = "CMD_POWER_ON"; break;
+ case CMD_NOP: cmd_str = "CMD_NOP"; break;
+ case CMD_EVENT: cmd_str = "CMD_EVENT"; break;
+ case CMD_EVENT_LOK: cmd_str = "CMD_EVENT_LOK"; break;
+ case CMD_EVENT_TURNOUT: cmd_str = "CMD_EVENT_TURNOUT"; break;
+ case CMD_EVENT_SENSOR: cmd_str = "CMD_EVENT_SENSOR"; break;
+ default: cmd_str = "(unknown command)";
+ }
+
+ const char *err_str = 0;
+ switch(err)
+ {
+ case ERR_NO_ERROR: err_str = "ERR_NO_ERROR"; break;
+ case ERR_SYS_ERROR: err_str = "ERR_SYS_ERROR"; break;
+ case ERR_BAD_PARAM: err_str = "ERR_BAD_PARAM"; break;
+ case ERR_POWER_OFF: err_str = "ERR_POWER_OFF"; break;
+ case ERR_NO_LOK_SPACE: err_str = "ERR_NO_LOK_SPACE"; break;
+ case ERR_NO_TURNOUT_SPACE: err_str = "ERR_NO_TURNOUT_SPACE"; break;
+ case ERR_NO_DATA: err_str = "ERR_NO_DATA"; break;
+ case ERR_NO_SLOT: err_str = "ERR_NO_SLOT"; break;
+ case ERR_BAD_LOK_ADDR: err_str = "ERR_BAD_LOK_ADDR"; break;
+ case ERR_LOK_BUSY: err_str = "ERR_LOK_BUSY"; break;
+ case ERR_BAD_TURNOUT_ADDR: err_str = "ERR_BAD_TURNOUT_ADDR"; break;
+ case ERR_BAD_SO_VALUE: err_str = "ERR_BAD_SO_VALUE"; break;
+ case ERR_NO_I2C_SPACE: err_str = "ERR_NO_I2C_SPACE"; break;
+ case ERR_LOW_TURNOUT_SPACE: err_str = "ERR_LOW_TURNOUT_SPACE"; break;
+ case ERR_LOK_HALTED: err_str = "ERR_LOK_HALTED"; break;
+ case ERR_LOK_POWER_OFF: err_str = "ERR_LOK_POWER_OFF"; break;
+ default: cmd_str = "(unknown error)";
+ }
+
+ IO::print("Error: %s: %s\n", cmd_str, err_str);
+}
+
+
+Intellibox::Locomotive::Locomotive():
+ speed(0),
+ reverse(false),
+ funcs(0)
+{ }
+
+
+Intellibox::Turnout::Turnout():
+ state(false),
+ active(false),
+ pending(false)
+{ }
+
+
+Intellibox::Sensor::Sensor():
+ state(false)
+{ }
+
+} // namespace R2C2
projects / r2c2.git / blobdiff