+/* $Id$
+
+This file is part of the MSP Märklin suite
+Copyright © 2007-2009 Mikkosoft Productions, Mikko Rasa
+Distributed under the GPL
+*/
+
#include <fcntl.h>
#include <termios.h>
#include <sys/poll.h>
-#include <iostream>
-#include <msp/core/error.h>
+#include <msp/core/except.h>
+#include <msp/io/print.h>
#include <msp/time/units.h>
#include <msp/time/utils.h>
#include "command.h"
#include "control.h"
+#include "reply.h"
using namespace std;
using namespace Msp;
Control::Control():
serial_fd(-1),
- p50_enabled(false),
power(true),
poll_sensors(false),
debug(false)
{ }
-void Control::set_power(bool p)
-{
- power=p;
- if(power)
- command(string(1, CMD_POWER_ON));
- else
- command(string(1, CMD_POWER_OFF));
-}
-
-Turnout *Control::get_turnout(unsigned id) const
-{
- TurnoutMap::const_iterator i=turnouts.find(id);
- if(i!=turnouts.end())
- return i->second;
-
- return 0;
-}
-
-Locomotive *Control::get_locomotive(unsigned id) const
-{
- for(LocomotiveSeq::const_iterator i=locomotives.begin(); i!=locomotives.end(); ++i)
- if((*i)->get_address()==id)
- return *i;
- return 0;
-}
-
-Sensor *Control::get_sensor(unsigned id) const
+Control::~Control()
{
- SensorMap::const_iterator i=sensors.find(id);
- if(i!=sensors.end())
- return i->second;
-
- return 0;
+ for(map<unsigned, Sensor *>::iterator i=sensors.begin(); i!=sensors.end(); ++i)
+ delete i->second;
+ for(map<unsigned, Turnout *>::iterator i=turnouts.begin(); i!=turnouts.end(); ++i)
+ delete i->second;
+ for(map<unsigned, Locomotive *>::iterator i=locomotives.begin(); i!=locomotives.end(); ++i)
+ delete i->second;
+ if(serial_fd>=0)
+ close(serial_fd);
}
void Control::open(const string &dev)
{
- serial_fd=::open(dev.c_str(), O_RDWR);
+ serial_fd = ::open(dev.c_str(), O_RDWR);
if(serial_fd<0)
throw Exception("Couldn't open serial port\n");
termios attr;
tcgetattr(serial_fd, &attr);
cfmakeraw(&attr);
- attr.c_cflag|=CSTOPB;
+ attr.c_cflag |= CSTOPB;
- bool ok=false;
+ bool ok = false;
+ bool p50 = false;
for(unsigned i=0; baud[i]; i+=2)
{
cfsetospeed(&attr, baud[i+1]);
write(serial_fd, "\xC4", 1);
- pollfd pfd={serial_fd, POLLIN, 0};
+ pollfd pfd = { serial_fd, POLLIN, 0 };
if(poll(&pfd, 1, 500)>0)
{
- cout<<"IB detected at "<<baud[i]<<" bits/s, P50 is ";
+ IO::print("IB detected at %d bits/s\n", baud[i]);
char buf[2];
- if(read(serial_fd, buf, 2)==2)
- {
- p50_enabled=true;
- cout<<"enabled";
- }
- else
- {
- p50_enabled=false;
- cout<<"disabled";
- }
- cout<<'\n';
- ok=true;
+ 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).signal_done.connect(sigc::mem_fun(this, &Control::status_done));
+}
+
+void Control::set_debug(bool d)
+{
+ debug = d;
}
-Command &Control::command(const string &cmd)
+void Control::set_power(bool p)
{
- queue.push_back(Command(cmd));
+ power = p;
+ if(power)
+ command(CMD_POWER_ON);
+ else
+ command(CMD_POWER_OFF);
+
+ signal_power_event.emit(power);
+}
+
+Command &Control::command(Cmd cmd)
+{
+ queue.push_back(Command(cmd, 0, 0));
+ return queue.back();
+}
+
+Command &Control::command(Cmd cmd, unsigned char data)
+{
+ queue.push_back(Command(cmd, &data, 1));
return queue.back();
}
-void Control::add_turnout(Turnout *t)
+Command &Control::command(Cmd cmd, const unsigned char *data, unsigned len)
{
- if(turnouts.count(t->get_address())==0)
- turnouts.insert(TurnoutMap::value_type(t->get_address(), t));
+ queue.push_back(Command(cmd, data, len));
+ return queue.back();
}
-void Control::add_locomotive(Locomotive *l)
+void Control::flush()
{
- if(find(locomotives.begin(), locomotives.end(), l)==locomotives.end())
- locomotives.push_back(l);
+ for(list<Command>::iterator i=queue.begin(); i!=queue.end(); ++i)
+ i->send(serial_fd);
}
-void Control::add_sensor(Sensor *s)
+void Control::add_turnout(Turnout &t)
{
- if(sensors.count(s->get_address())==0)
- {
- sensors.insert(SensorMap::value_type(s->get_address(), s));
- poll_sensors=true;
- }
+ turnouts[t.get_address()] = &t;
+}
+
+Turnout &Control::get_turnout(unsigned id) const
+{
+ map<unsigned, Turnout *>::const_iterator i = turnouts.find(id);
+ if(i==turnouts.end())
+ throw KeyError("Unknown turnout");
+
+ return *i->second;
+}
+
+void Control::add_locomotive(Locomotive &l)
+{
+ locomotives[l.get_address()] = &l;
+}
+
+Locomotive &Control::get_locomotive(unsigned id) const
+{
+ map<unsigned, Locomotive *>::const_iterator i = locomotives.find(id);
+ if(i==locomotives.end())
+ throw KeyError("Unknown locomotive");
+
+ return *i->second;
+}
+
+void Control::add_sensor(Sensor &s)
+{
+ sensors[s.get_address()] = &s;
+ poll_sensors = true;
+}
+
+Sensor &Control::get_sensor(unsigned id) const
+{
+ map<unsigned, Sensor *>::const_iterator i = sensors.find(id);
+ if(i==sensors.end())
+ throw KeyError("Unknown sensor");
+
+ return *i->second;
}
void Control::tick()
{
- const Time::TimeStamp t=Time::now();
+ const Time::TimeStamp t = Time::now();
+
+ for(map<unsigned, Sensor *>::const_iterator i=sensors.begin(); i!=sensors.end(); ++i)
+ i->second->tick();
+
+ timer.tick(false);
if(t>next_event_query)
{
- next_event_query=t+300*Time::msec;
- command(string(1, CMD_EVENT)).signal_done.connect(sigc::mem_fun(this, &Control::event_query_done));
+ next_event_query = t+200*Time::msec;
+ command(CMD_EVENT).signal_done.connect(sigc::mem_fun(this, &Control::event_query_done));
}
if(poll_sensors)
{
- unsigned max_addr=(--sensors.end())->second->get_address();
- string cmd(3, 0);
- cmd[0]=CMD_SENSOR_PARAM_SET;
- cmd[1]=0;
- cmd[2]=(max_addr+7)/8;
- command(cmd);
- command(string(1, CMD_SENSOR_REPORT));
- poll_sensors=false;
+ 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);
+ poll_sensors = false;
}
- if(queue.size() && queue.front().get_sent())
+ if(!queue.empty() && queue.front().is_sent())
{
- pollfd pfd={serial_fd, POLLIN, 0};
+ pollfd pfd = { serial_fd, POLLIN, 0 };
if(poll(&pfd, 1, 0)>0)
{
- string resp=read_reply((Cmd)(unsigned char)queue.front().get_command()[0]);
+ Reply reply = Reply::read(serial_fd, queue.front().get_command());
if(debug)
- {
- printf("read: ");
- for(unsigned i=0; i<resp.size(); ++i)
- printf("%02X ", (unsigned char)resp[i]);
- printf("(%d bytes)\n", resp.size());
- }
-
- queue.front().signal_done.emit((Error)resp[0], resp.substr(1));
+ IO::print("R: %s\n", reply);
+
+ queue.front().signal_done.emit(reply);
queue.erase(queue.begin());
}
else
return;
}
- if(queue.size())
+ if(!queue.empty())
{
- string cmd=queue.front().get_command();
-
- if(p50_enabled)
- {
- if(cmd[0]&0x80)
- cmd="X"+cmd;
- else
- cmd="x"+cmd;
- }
-
if(debug)
+ IO::print("W: %s\n", queue.front());
+
+ if(serial_fd>=0)
+ queue.front().send(serial_fd);
+ else
{
- printf("write: ");
- for(unsigned i=0; i<cmd.size(); ++i)
- printf("%02X ",(unsigned char)cmd[i]);
- printf("(%d bytes)\n",cmd.size());
+ Reply reply = Reply::simulate(queue.front().get_command());
+ queue.front().signal_done.emit(reply);
+ queue.erase(queue.begin());
}
-
- write(serial_fd, cmd.data(), cmd.size());
- queue.front().set_sent(true);
}
}
-Control::~Control()
-{
- close(serial_fd);
-}
-
-/*** private ***/
-
-void Control::read_all(int fd, char *buf, int size)
+Time::Timer::Slot &Control::set_timer(const Time::TimeDelta &dt)
{
- int pos=0;
- while(pos<size)
- {
- int len=read(fd, buf+pos, size-pos);
- pos+=len;
- }
+ return timer.add(dt);
}
-string Control::read_reply(Cmd cmd)
+void Control::status_done(const Reply &reply)
{
- string result;
- if(cmd==CMD_EVENT)
- {
- result+=ERR_NO_ERROR;
- for(unsigned i=0; i<3; ++i)
- {
- char c;
- read(serial_fd, &c, 1);
- result+=c;
- if(!(c&0x80)) break;
- }
- }
- else if(cmd==CMD_EVENT_LOK)
- {
- result+=ERR_NO_ERROR;
- char c[5];
- read(serial_fd, c+4, 1);
- result+=c[4];
- while(c[4]&0x80)
- {
- read_all(serial_fd, c, 5);
- result.append(c, 5);
- }
- }
- else if(cmd==CMD_EVENT_TURNOUT)
- {
- result+=ERR_NO_ERROR;
- char c[511];
- read(serial_fd, c, 1);
- read_all(serial_fd, c+1, c[0]*2);
- result.append(c, c[0]*2+1);
- }
- else if(cmd==CMD_EVENT_SENSOR)
- {
- result+=ERR_NO_ERROR;
- char c[3];
- read(serial_fd, c+2, 1);
- result+=c[2];
- while(c[2])
- {
- read_all(serial_fd, c, 3);
- result.append(c, 3);
- }
- }
- else
- {
- unsigned expected_bytes=1;
- if(cmd==CMD_FUNC_STATUS || cmd==CMD_TURNOUT_STATUS)
- expected_bytes=2;
- if(cmd==CMD_SENSOR_STATUS || cmd==CMD_TURNOUT_GROUP_STATUS)
- expected_bytes=3;
- if(cmd==CMD_LOK_STATUS)
- expected_bytes=4;
- if(cmd==CMD_LOK_CONFIG)
- expected_bytes=5;
- char c[5];
- read_all(serial_fd, c, 1);
- result+=c[0];
- if(!c[0])
- {
- read_all(serial_fd, c+1, expected_bytes-1);
- result.append(c+1, expected_bytes-1);
- }
- }
-
- return result;
+ power = ((reply.get_data()[0]&0x08)!=0);
+ signal_power_event.emit(power);
}
-void Control::event_query_done(Error, const string &resp)
+void Control::event_query_done(const Reply &reply)
{
- if(resp[0]&0x20)
- command(string(1, CMD_EVENT_TURNOUT)).signal_done.connect(sigc::mem_fun(this, &Control::turnout_event_done));
- if(resp[0]&0x04)
- command(string(1, CMD_EVENT_SENSOR)).signal_done.connect(sigc::mem_fun(this, &Control::sensor_event_done));
+ const unsigned char *data = reply.get_data();
+ if(data[0]&0x01)
+ command(CMD_EVENT_LOK);
+ if(data[0]&0x20)
+ command(CMD_EVENT_TURNOUT).signal_done.connect(sigc::mem_fun(this, &Control::turnout_event_done));
+ if(data[0]&0x04)
+ command(CMD_EVENT_SENSOR).signal_done.connect(sigc::mem_fun(this, &Control::sensor_event_done));
+ if((data[0]&0x80) && (data[1]&0x40))
+ command(CMD_STATUS).signal_done.connect(sigc::mem_fun(this, &Control::status_done));
}
-void Control::turnout_event_done(Error, const string &resp)
+void Control::turnout_event_done(const Reply &reply)
{
- unsigned count=resp[0];
+ const unsigned char *data = reply.get_data();
+ unsigned count = data[0];
for(unsigned i=0; i<count; ++i)
{
- unsigned addr=(unsigned char)resp[i*2+1]+((resp[i*2+2]&7)<<8);
- bool status=!(resp[i*2+2]&0x80);
- cout<<"Turnout "<<addr<<", status "<<status<<'\n';
+ unsigned addr = (data[i*2+1])+((data[i*2+2]&7)<<8);
+ bool status = !(data[i*2+2]&0x80);
+ IO::print("Turnout %d, status %d\n", addr, status);
signal_turnout_event.emit(addr, status);
}
}
-void Control::sensor_event_done(Error, const string &resp)
+void Control::sensor_event_done(const Reply &reply)
{
- for(unsigned i=0; resp[i]; i+=3)
+ const unsigned char *data = reply.get_data();
+ for(unsigned i=0; data[i]; i+=3)
{
- unsigned module=(unsigned char)resp[i];
- cout<<"S88 module "<<module<<", status ";
+ unsigned module = data[i];
+
+ IO::print("S88 module %d, status %08b%08b\n", module, data[1], data[2]);
+
for(unsigned j=0; j<16; ++j)
- {
- bool status=(resp[i+1+j/8]>>(7-j%8))&1;
- cout<<status;
- signal_sensor_event.emit(module*16+j-15, status);
- }
- cout<<'\n';
+ signal_sensor_event.emit(module*16+j-15, (data[i+1+j/8]>>(7-j%8))&1);
}
}