#include <algorithm>
#include <fstream>
+#include <limits>
#include <msp/gl/rendermode.h>
#include <msp/gl/select.h>
#include <msp/gl/texture.h>
Distributed under the GPL
*/
+#include <cstring>
+#include <msp/strings/formatter.h>
#include "command.h"
using namespace std;
+using namespace Msp;
namespace Marklin {
-Command::Command(const string &c):
+Command::Command(Cmd c, const unsigned char *d, unsigned l):
cmd(c),
+ len(1),
sent(false)
-{ }
+{
+ data[0]=cmd;
+ if(d)
+ {
+ memcpy(data+1, d, min(l, 127U));
+ len+=min(l, 127U);
+ }
+}
+
+void Command::send(int fd)
+{
+ write(fd, data, len);
+ sent=true;
+}
-void Command::set_sent(bool s)
+ostream &operator<<(ostream &out, const Command &cmd)
{
- sent=s;
+ out<<cmd.cmd;
+ for(unsigned i=1; i<cmd.len; ++i)
+ out<<format(" %02X", static_cast<int>(cmd.data[i]));
+
+ return out;
}
} // namespace Marklin
#ifndef COMMAND_H_
#define COMMAND_H_
+#include <ostream>
#include <string>
#include <sigc++/sigc++.h>
#include "constants.h"
namespace Marklin {
+class Reply;
+
class Command
{
private:
- std::string cmd;
- bool sent;
+ Cmd cmd;
+ unsigned char data[128];
+ unsigned len;
+ bool sent;
public:
- sigc::signal<void, Error, const std::string &> signal_done;
+ sigc::signal<void, const Reply &> signal_done;
+
+ Command(Cmd, const unsigned char *, unsigned);
- Command(const std::string &);
+ void send(int);
+ bool get_sent() const { return sent; }
+ Cmd get_command() const { return cmd; }
- void set_sent(bool);
- const std::string &get_command() const { return cmd; }
- bool get_sent() const { return sent; }
+ friend std::ostream &operator<<(std::ostream &, const Command &);
};
} // namespace Marklin
#ifndef ERROR_H_
#define ERROR_H_
+#include <ostream>
+
namespace Marklin {
enum Error
ERR_UNKNOWN_ERROR=0xFF
};
+std::ostream &operator<<(std::ostream &, const Error &);
+
enum Cmd
{
CMD_LOK=0x80,
CMD_EVENT_SENSOR=0xCB
};
+std::ostream &operator<<(std::ostream &, const Cmd &);
+
} // namespace Marklin
#endif
#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)
{
power=p;
if(power)
- command(string(1, CMD_POWER_ON));
+ command(CMD_POWER_ON);
else
- command(string(1, CMD_POWER_OFF));
+ command(CMD_POWER_OFF);
signal_power_event.emit(power);
}
attr.c_cflag|=CSTOPB;
bool ok=false;
+ bool p50=false;
for(unsigned i=0; baud[i]; i+=2)
{
cfsetospeed(&attr, baud[i+1]);
pollfd pfd={serial_fd, POLLIN, 0};
if(poll(&pfd, 1, 500)>0)
{
- cout<<"IB detected at "<<baud[i]<<" bits/s, P50 is ";
+ cout<<"IB detected at "<<baud[i]<<" bits/s\n";
char buf[2];
- if(read(serial_fd, buf, 2)==2)
- {
- p50_enabled=true;
- cout<<"enabled";
- }
- else
- {
- p50_enabled=false;
- cout<<"disabled";
- }
- cout<<'\n';
+ 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(string(1, CMD_STATUS)).signal_done.connect(sigc::mem_fun(this, &Control::status_done));
+ command(CMD_STATUS).signal_done.connect(sigc::mem_fun(this, &Control::status_done));
}
-Command &Control::command(const string &cmd)
+Command &Control::command(Cmd cmd)
{
- queue.push_back(Command(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();
+}
+
+Command &Control::command(Cmd cmd, const unsigned char *data, unsigned len)
+{
+ queue.push_back(Command(cmd, data, len));
return queue.back();
}
if(t>next_event_query)
{
next_event_query=t+200*Time::msec;
- command(string(1, CMD_EVENT)).signal_done.connect(sigc::mem_fun(this, &Control::event_query_done));
+ 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));
+ 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;
}
pollfd pfd={serial_fd, POLLIN, 0};
if(poll(&pfd, 1, 0)>0)
{
- string resp=read_reply(static_cast<Cmd>(static_cast<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 ", static_cast<unsigned char>(resp[i]));
- printf("(%d bytes)\n", resp.size());
- }
-
- queue.front().signal_done.emit(static_cast<Error>(resp[0]), resp.substr(1));
+ cout<<"R: "<<reply<<'\n';
+
+ queue.front().signal_done.emit(reply);
queue.erase(queue.begin());
}
else
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)
- {
- printf("write: ");
- for(unsigned i=0; i<cmd.size(); ++i)
- printf("%02X ", static_cast<unsigned char>(cmd[i]));
- printf("(%d bytes)\n", cmd.size());
- }
+ cout<<"W: "<<queue.front()<<'\n';
- write(serial_fd, cmd.data(), cmd.size());
- queue.front().set_sent(true);
+ queue.front().send(serial_fd);
}
}
}
}
-string Control::read_reply(Cmd cmd)
-{
- 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
- {
- if(cmd==CMD_STATUS)
- result+=ERR_NO_ERROR;
-
- 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;
-}
-
-void Control::status_done(Error, const string &resp)
+void Control::status_done(const Reply &reply)
{
- power=((resp[0]&0x08)!=0);
+ 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));
- if(resp.size()>1 && (resp[1]&0x40))
- command(string(1, CMD_STATUS)).signal_done.connect(sigc::mem_fun(this, &Control::status_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[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=static_cast<unsigned char>(resp[i*2+1])+((resp[i*2+2]&7)<<8);
- bool status=!(resp[i*2+2]&0x80);
+ unsigned addr=(data[i*2+1])+((data[i*2+2]&7)<<8);
+ bool status=!(data[i*2+2]&0x80);
cout<<"Turnout "<<addr<<", status "<<status<<'\n';
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=static_cast<unsigned char>(resp[i]);
+ unsigned module=data[i];
cout<<"S88 module "<<module<<", status ";
for(unsigned j=0; j<16; ++j)
- cout<<((resp[i+1+j/8]>>(7-j%8))&1);
+ cout<<((data[i+1+j/8]>>(7-j%8))&1);
cout<<'\n';
for(unsigned j=0; j<16; ++j)
- signal_sensor_event.emit(module*16+j-15, (resp[i+1+j/8]>>(7-j%8))&1);
+ signal_sensor_event.emit(module*16+j-15, (data[i+1+j/8]>>(7-j%8))&1);
}
}
namespace Marklin {
class Command;
+class Reply;
class Control
{
private:
int serial_fd;
- bool p50_enabled;
bool power;
std::list<Command> queue;
std::map<unsigned, Turnout *> turnouts;
const std::map<unsigned, Sensor *> &get_sensors() const { return sensors; }
unsigned get_queue_length() const { return queue.size(); }
void open(const std::string &);
- Command &command(const std::string &);
+ Command &command(Cmd);
+ Command &command(Cmd, unsigned char);
+ Command &command(Cmd, const unsigned char *, unsigned);
void add_turnout(Turnout &);
Turnout &get_turnout(unsigned) const;
private:
void read_all(int, char *, int);
std::string read_reply(Cmd);
- void status_done(Error, const std::string &);
- void event_query_done(Error, const std::string &);
- void turnout_event_done(Error, const std::string &);
- void sensor_event_done(Error, const std::string &);
+ void status_done(const Reply &);
+ void event_query_done(const Reply &);
+ void turnout_event_done(const Reply &);
+ void sensor_event_done(const Reply &);
};
} // namespace Marklin
#include "constants.h"
#include "control.h"
#include "locomotive.h"
+#include "reply.h"
using namespace std;
using namespace Msp;
void Locomotive::refresh_status()
{
- char cmd[3];
- cmd[0]=CMD_LOK_STATUS;
- cmd[1]=addr&0xFF;
- cmd[2]=(addr>>8)&0xFF;
- control.command(string(cmd, 3)).signal_done.connect(sigc::mem_fun(this, &Locomotive::status_reply));
+ unsigned char data[2];
+ data[0]=addr&0xFF;
+ data[1]=(addr>>8)&0xFF;
+ control.command(CMD_LOK_STATUS, data, 2).signal_done.connect(sigc::mem_fun(this, &Locomotive::status_reply));
}
void Locomotive::send_command(bool setf)
{
- char cmd[16];
- cmd[0]=CMD_LOK;
- cmd[1]=addr&0xFF;
- cmd[2]=(addr>>8)&0xFF;
+ unsigned char data[4];
+ data[0]=addr&0xFF;
+ data[1]=(addr>>8)&0xFF;
if(speed==0)
- cmd[3]=0;
+ data[2]=0;
else if(speed==1)
- cmd[3]=2;
+ data[2]=2;
else
- cmd[3]=(speed*19-18)/2;
+ data[2]=(speed*19-18)/2;
- cmd[4]=(reverse?0:0x20) | ((funcs&1)?0x10:0);
+ data[3]=(reverse ? 0 : 0x20) | ((funcs&1) ? 0x10 : 0);
if(setf)
{
- cmd[4]|=0x80;
+ data[3]|=0x80;
for(unsigned i=0; i<4; ++i)
if((funcs>>i)&2)
- cmd[4]|=(1<<i);
+ data[3]|=(1<<i);
}
- control.command(string(cmd, 5));
+ control.command(CMD_LOK, data, 4);
}
-void Locomotive::status_reply(Error err, const string &reply)
+void Locomotive::status_reply(const Reply &reply)
{
- if(err==ERR_NO_ERROR)
+ if(reply.get_error()==ERR_NO_ERROR)
{
- if(static_cast<unsigned char>(reply[0])<=1)
+ const unsigned char *data=reply.get_data();
+
+ if(data[0]<=1)
speed=0;
else
- speed=static_cast<unsigned char>(reply[0])*2/19+1;
- reverse=(reply[1]&0x20)?false:true;
- funcs=(reply[1]&0xF)<<1;
- if(reply[1]&0x10)
+ speed=data[0]*2/19+1;
+
+ reverse=(data[1]&0x20) ? false : true;
+ funcs=(data[1]&0xF)<<1;
+ if(data[1]&0x10)
funcs|=1;
for(unsigned i=0; i<5; ++i)
class Control;
class LocoType;
+class Reply;
class Locomotive
{
void refresh_status();
private:
void send_command(bool);
- void status_reply(Error, const std::string &);
+ void status_reply(const Reply &);
bool reverse_timeout();
};
--- /dev/null
+/* $Id$
+
+This file is part of the MSP Märklin suite
+Copyright © 2006-2008 Mikkosoft Productions, Mikko Rasa
+Distributed under the GPL
+*/
+
+#include <cstring>
+#include <unistd.h>
+#include <msp/strings/formatter.h>
+#include "constants.h"
+#include "reply.h"
+
+using namespace std;
+using namespace Msp;
+
+namespace {
+
+unsigned read_all(int fd, char *buf, unsigned size)
+{
+ unsigned pos=0;
+ while(pos<size)
+ pos+=read(fd, buf+pos, size-pos);
+
+ return pos;
+}
+
+}
+
+namespace Marklin {
+
+Reply::Reply():
+ err(ERR_NO_ERROR),
+ len(0)
+{
+ memset(data, 0, 128);
+}
+
+Reply Reply::read(int fd, Cmd cmd)
+{
+ Reply result;
+
+ char *data=reinterpret_cast<char *>(result.data);
+
+ if(cmd==CMD_EVENT)
+ {
+ for(unsigned i=0; i<3; ++i)
+ {
+ result.len+=read_all(fd, data+i, 1);
+ if(!(result.data[i]&0x80))
+ break;
+ }
+ }
+ else if(cmd==CMD_EVENT_LOK)
+ {
+ for(unsigned i=0;; i+=5)
+ {
+ result.len+=read_all(fd, data+i, 1);
+
+ if(result.data[i]&0x80)
+ break;
+
+ result.len+=read_all(fd, data+i+1, 4);
+ }
+ }
+ else if(cmd==CMD_EVENT_TURNOUT)
+ {
+ result.len+=read_all(fd, data, 1);
+ result.len+=read_all(fd, data+1, result.data[0]*2);
+ }
+ else if(cmd==CMD_EVENT_SENSOR)
+ {
+ for(unsigned i=0;; i+=3)
+ {
+ result.len+=read_all(fd, data+i, 1);
+
+ if(result.data[i]==0)
+ break;
+
+ result.len+=read_all(fd, data+i+1, 2);
+ }
+ }
+ else
+ {
+ bool expect_errcode=(cmd!=CMD_STATUS);
+
+ unsigned expected_bytes=0;
+ if(cmd==CMD_STATUS || cmd==CMD_FUNC_STATUS || cmd==CMD_TURNOUT_STATUS)
+ expected_bytes=1;
+ if(cmd==CMD_SENSOR_STATUS || cmd==CMD_TURNOUT_GROUP_STATUS)
+ expected_bytes=2;
+ if(cmd==CMD_LOK_STATUS)
+ expected_bytes=3;
+ if(cmd==CMD_LOK_CONFIG)
+ expected_bytes=4;
+
+ if(expect_errcode)
+ {
+ char c;
+ read_all(fd, &c, 1);
+ result.err=static_cast<Error>(c);
+ }
+
+ if(result.err==ERR_NO_ERROR)
+ result.len+=read_all(fd, data, expected_bytes);
+ }
+
+ return result;
+}
+
+ostream &operator<<(ostream &out, const Reply &reply)
+{
+ out<<reply.err;
+ for(unsigned i=0; i<reply.len; ++i)
+ out<<format(" %02X", static_cast<int>(reply.data[i]));
+
+ return out;
+}
+
+} // namespace Marklin
--- /dev/null
+/* $Id$
+
+This file is part of the MSP Märklin suite
+Copyright © 2006-2008 Mikkosoft Productions, Mikko Rasa
+Distributed under the GPL
+*/
+
+#ifndef LIBMARKLIN_REPLY_H_
+#define LIBMARKLIN_REPLY_H_
+
+#include <ostream>
+
+namespace Marklin {
+
+class Reply
+{
+private:
+ Error err;
+ unsigned char data[128];
+ unsigned len;
+
+ Reply();
+public:
+ Error get_error() const { return err; }
+ const unsigned char *get_data() const { return data; }
+
+ static Reply read(int, Cmd);
+
+ friend std::ostream &operator<<(std::ostream &, const Reply &);
+};
+
+
+
+} // namespace Marklin
+
+#endif
Distributed under the GPL
*/
+#include <algorithm>
#include <msp/time/utils.h>
#include "control.h"
#include "layout.h"
}
for(list<Block *>::iterator i=blocks.begin(); i!=blocks.end(); ++i)
- {
for(list<Block *>::iterator j=i; j!=blocks.end(); ++j)
if(j!=i)
(*i)->check_link(**j);
- (*i)->print_debug();
- }
}
TrafficManager::~TrafficManager()
#include <msp/time/units.h>
#include "command.h"
#include "control.h"
+#include "reply.h"
#include "turnout.h"
using namespace std;
control.signal_turnout_event.connect(sigc::mem_fun(this, &Turnout::turnout_event));
- char cmd[3];
- cmd[0]=CMD_TURNOUT_STATUS;
- cmd[1]=addr&0xFF;
- cmd[2]=(addr>>8)&0xFF;
- control.command(string(cmd, 3)).signal_done.connect(sigc::mem_fun(this, &Turnout::status_reply));
+ unsigned char data[2];
+ data[0]=addr&0xFF;
+ data[1]=(addr>>8)&0xFF;
+ control.command(CMD_TURNOUT_STATUS, data, 2).signal_done.connect(sigc::mem_fun(this, &Turnout::status_reply));
}
void Turnout::set_route(unsigned r)
void Turnout::command(bool on)
{
- char cmd[3];
- cmd[0]=CMD_TURNOUT;
- cmd[1]=addr&0xFF;
- cmd[2]=((addr>>8)&0x7);
- if(on)
- cmd[2]|=0x40;
- if(route==0)
- cmd[2]|=0x80;
- control.command(string(cmd, 3));
+ unsigned char data[2];
+ data[0]=addr&0xFF;
+ data[1]=((addr>>8)&0x7) | (on ? 0x40 : 0) | (route==0 ? 0x80 : 0);
+ control.command(CMD_TURNOUT, data, 2);
}
-void Turnout::status_reply(Error err, const string &reply)
+void Turnout::status_reply(const Reply &reply)
{
- if(err==ERR_NO_ERROR)
+ if(reply.get_error()==ERR_NO_ERROR)
{
- route=(reply[0]&4)?0:1;
+ route=(reply.get_data()[0]&0x04) ? 0 : 1;
signal_route_changed.emit(route);
}
}
namespace Marklin {
class Control;
+class Reply;
class Turnout
{
unsigned get_route() const { return route; }
private:
void command(bool);
- void status_reply(Error, const std::string &);
+ void status_reply(const Reply &);
bool switch_timeout();
void turnout_event(unsigned, bool);
};
projects / r2c2.git / commitdiff