From e61a3ee0847b957152d98ba5945e42325821c357 Mon Sep 17 00:00:00 2001 From: Mikko Rasa Date: Mon, 29 Nov 2010 17:04:20 +0000 Subject: [PATCH] =?utf8?q?Add=20driver=20for=20M=C3=A4rklin=20Central=20St?= =?utf8?q?ation?= MIME-Version: 1.0 Content-Type: text/plain; charset=utf8 Content-Transfer-Encoding: 8bit --- source/libr2c2/centralstation.cpp | 770 ++++++++++++++++++++++++++++++ source/libr2c2/centralstation.h | 170 +++++++ source/libr2c2/driver.cpp | 3 + 3 files changed, 943 insertions(+) create mode 100644 source/libr2c2/centralstation.cpp create mode 100644 source/libr2c2/centralstation.h diff --git a/source/libr2c2/centralstation.cpp b/source/libr2c2/centralstation.cpp new file mode 100644 index 0000000..69bb8a9 --- /dev/null +++ b/source/libr2c2/centralstation.cpp @@ -0,0 +1,770 @@ +/* $Id$ + +This file is part of R²C² +Copyright © 2010 Mikkosoft Productions, Mikko Rasa +Distributed under the GPL +*/ + +#include +#include +#include +#include +#include +#include +#include +#include "centralstation.h" +#include "tracktype.h" +#include "vehicletype.h" + +using namespace std; +using namespace Msp; + +namespace R2C2 { + +CentralStation::CentralStation(const string &host): + socket(Net::INET), + pending_commands(0), + power(false), + halted(false), + locos_synced(false), + turnouts_synced(false), + sensors_synced(false) +{ + RefPtr addr = Net::resolve(host+":15471"); + socket.connect(*addr); + + IO::print("Connected to central station at %s\n", addr->str()); + + command("get(1, status)"); + command("request(1, view)"); + command("queryObjects(10, addr, name)"); + command("queryObjects(11, addr)"); + command("queryObjects(26)"); +} + +CentralStation::~CentralStation() +{ + command("release(1, view)", true); + for(LocoMap::iterator i=locos.begin(); (i!=locos.end() && !(i->first&0x10000)); ++i) + command(format("release(%d, view, control)", i->first)); + for(TurnoutMap::iterator i=turnouts.begin(); (i!=turnouts.end() && !(i->first&0x10000)); ++i) + command(format("release(%d, view, control)", i->first)); + while(IO::poll(socket, IO::P_INPUT, 100*Time::msec)) + while(receive()) ; +} + +void CentralStation::set_power(bool p) +{ + power = p; + command(format("set(1, %s)", (power ? "go" : "stop"))); +} + +void CentralStation::halt(bool h) +{ + halted = h; + if(halted) + { + for(LocoMap::iterator i=locos.begin(); i!=locos.end(); ++i) + if(i->second.speed) + set_loco_speed(i->first, 0); + } + + signal_halt.emit(halted); +} + +const char *CentralStation::enumerate_protocols(unsigned index) const +{ + if(index==MM) + return "MM"; + else if(index==MM_27) + return "MM-27"; + else if(index==MFX) + return "MFX"; + else + return 0; +} + +unsigned CentralStation::get_protocol_speed_steps(const string &name) const +{ + switch(map_protocol(name)) + { + case MM: return 14; + case MM_27: return 27; + case MFX: return 127; + default: return 0; + } +} + +void CentralStation::add_loco(unsigned addr, const string &proto_name, const VehicleType &type) +{ + Protocol proto = map_protocol(proto_name); + + unsigned id = map_address(locos, loco_addr, addr); + if(!id) + { + Locomotive &loco = locos[addr|0x10000]; + loco.name = type.get_name(); + loco.protocol = proto; + loco.address = addr; + + if(locos_synced && proto!=MFX) + command("create(10)"); + } + else + command(format("request(%d, view, control, force)", id)); +} + +void CentralStation::set_loco_speed(unsigned addr, unsigned speed) +{ + if(speed && halted) + return; + + unsigned id = map_address(locos, loco_addr, addr); + if(id) + command(format("set(%d, speedstep[%d])", id, speed)); +} + +void CentralStation::set_loco_reverse(unsigned addr, bool rev) +{ + unsigned id = map_address(locos, loco_addr, addr); + if(id) + command(format("set(%d, dir[%d])", id, rev)); +} + +void CentralStation::set_loco_function(unsigned addr, unsigned func, bool state) +{ + unsigned id = map_address(locos, loco_addr, addr); + if(id) + command(format("set(%d, func[%d, %d])", id, func, state)); +} + +void CentralStation::add_turnout(unsigned addr, const TrackType &type) +{ + bool left = false; + bool right = false; + bool cross = false; + + const vector &parts = type.get_parts(); + for(vector::const_iterator i=parts.begin(); i!=parts.end(); ++i) + { + TrackPoint start = i->get_point(0); + TrackPoint end = i->get_point(i->get_length()); + if(end.dir>start.dir+0.01) + left = true; + else if(end.dir0.01) + cross = true; + } + + unsigned symbol; + if(cross) + symbol = Turnout::DOUBLESLIP; + else if(left && right) + symbol = Turnout::THREEWAY; + else if(left) + symbol = Turnout::LEFT; + else if(right) + symbol = Turnout::RIGHT; + + unsigned id = map_address(turnouts, turnout_addr, addr); + if(!id) + { + id = addr|0x10000; + + Turnout &turnout = turnouts[id]; + turnout.address = addr; + turnout.bits = type.get_state_bits(); + turnout.symbol = symbol; + + turnout_addr[addr] = id; + + if(turnouts_synced) + command("create(11, append)"); + } + else + { + Turnout &turnout = turnouts[id]; + command(format("request(%d, view, control)", id)); + if(turnout.symbol!=symbol) + command(format("set(%d, symbol[%d])", symbol)); + } +} + +void CentralStation::set_turnout(unsigned addr, unsigned state) +{ + unsigned id = map_address(turnouts, turnout_addr, addr); + if(id) + { + Turnout &turnout = turnouts[id]; + unsigned mask = (1<second.state; + } + return 0; +} + +void CentralStation::add_sensor(unsigned addr) +{ + sensors.insert(SensorMap::value_type(addr, Sensor())); + + if(sensors_synced) + { + if(addr>s88.size()*16) + command("create(26, add[0])"); + } +} + +bool CentralStation::get_sensor(unsigned addr) const +{ + SensorMap::const_iterator i = sensors.find(addr); + if(i!=sensors.end()) + return i->second.state; + return false; +} + +void CentralStation::tick() +{ + Time::TimeStamp t = Time::now(); + for(SensorMap::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, i->second.state); + } + + while(Message msg = receive()) + { + if(msg.footer.code) + IO::print("\033[31m*** ERROR: %s: %d %s ***\033[0m\n", msg.header.value, msg.footer.code, msg.footer.value); + + if(msg.header.type=="REPLY") + process_reply(msg); + else if(msg.header.type=="EVENT") + process_event(msg); + } +} + +void CentralStation::flush() +{ +} + +void CentralStation::command(const string &cmd, bool force) +{ + if(pending_commands<10 || force) + { + socket.write(cmd+"\r\n"); + ++pending_commands; + } + else + cmd_queue.push_back(cmd); +} + +CentralStation::Message CentralStation::receive() +{ + while(IO::poll(socket, IO::P_INPUT, Time::zero)) + { + char rbuf[1024]; + unsigned len = socket.read(rbuf, sizeof(rbuf)); + if(!len) + return Message(); + + in_buffer.append(rbuf, len); + } + + if(!in_buffer.empty()) + { + string::iterator iter = in_buffer.begin(); + if(Message msg = parse_message(iter, in_buffer.end())) + { + skip(iter, in_buffer.end(), "\r\n"); + in_buffer.erase(in_buffer.begin(), iter); + + if(msg.header.type=="REPLY" && pending_commands>0) + { + --pending_commands; + if(!cmd_queue.empty()) + { + command(cmd_queue.front()); + cmd_queue.pop_front(); + } + } + + return msg; + } + } + + return Message(); +} + +void CentralStation::process_reply(const Message &msg) +{ + if(!msg.header.value.compare(0, 4, "get(")) + { + for(Message::ObjectMap::const_iterator i=msg.content.begin(); i!=msg.content.end(); ++i) + { + if(turnouts.count(i->first)) + turnouts[i->first].synced = true; + + process_object(i->first, i->second); + } + } + else if(!msg.header.value.compare(0, 16, "queryObjects(10,")) + { + for(Message::ObjectMap::const_iterator i=msg.content.begin(); i!=msg.content.end(); ++i) + { + LocoMap::iterator j = locos.find(i->first); + if(j==locos.end()) + { + bool found = false; + Message::AttribMap::const_iterator k = i->second.find("addr"); + if(k!=i->second.end()) + { + unsigned addr = lexical_cast(k->second); + + j = locos.find(addr|0x10000); + if(j!=locos.end()) + { + command(format("request(%d, view, control, force)", i->first)); + command(format("get(%d, dir, func[0])", i->first)); + + locos.insert(LocoMap::value_type(i->first, j->second)); + locos.erase(j); + + found = true; + } + } + + if(!found) + locos.insert(LocoMap::value_type(i->first, Locomotive())); + } + + process_object(i->first, i->second); + } + + locos_synced = true; + + if(locos.lower_bound(0x10000)!=locos.end()) + command("create(10)"); + } + else if(!msg.header.value.compare(0, 16, "queryObjects(11,")) + { + for(Message::ObjectMap::const_iterator i=msg.content.begin(); i!=msg.content.end(); ++i) + { + TurnoutMap::iterator j = turnouts.find(i->first); + if(j==turnouts.end()) + { + bool found = false; + Message::AttribMap::const_iterator k = i->second.find("addr"); + if(k!=i->second.end()) + { + unsigned addr = lexical_cast(k->second); + + j = turnouts.find(addr|0x10000); + if(j!=turnouts.end()) + { + command(format("request(%d, view, control)", i->first)); + command(format("set(%d, symbol[%d])", i->first, j->second.symbol)); + command(format("get(%d, state)", i->first)); + + turnouts.insert(TurnoutMap::value_type(i->first, j->second)); + turnouts.erase(j); + + found = true; + } + } + + if(!found) + turnouts.insert(TurnoutMap::value_type(i->first, Turnout())); + } + + process_object(i->first, i->second); + } + + turnouts_synced = true; + + for(TurnoutMap::const_iterator i=turnouts.lower_bound(0x10000); i!=turnouts.end(); ++i) + command("create(11, append)"); + } + else if(msg.header.value=="queryObjects(26)") + { + s88.clear(); + for(Message::ObjectMap::const_iterator i=msg.content.begin(); i!=msg.content.end(); ++i) + { + s88.push_back(i->first); + command(format("request(%d, view)", i->first)); + } + + sensors_synced = true; + + if(!sensors.empty()) + { + unsigned high_addr = (--sensors.end())->first; + if(high_addr>16*s88.size()) + command("create(26, add[0])"); + } + } + else if(msg.header.value=="create(10)") + { + Message::ObjectMap::const_iterator i = msg.content.find(10); + if(i!=msg.content.end()) + { + Message::AttribMap::const_iterator j = i->second.find("id"); + if(j!=i->second.end()) + { + unsigned id = lexical_cast(j->second); + LocoMap::iterator k = locos.lower_bound(0x10000); + if(k!=locos.end()) + { + command(format("request(%d, view, control)", id)); + command(format("set(%d, addr[%d], protocol[%s], name[\"%s\"])", + id, k->second.address, (k->second.protocol==MM_27 ? "MM27" : "MM14"), k->second.name)); + command("create(10, append)"); + + locos.insert(LocoMap::value_type(id, k->second)); + locos.erase(k); + } + } + } + + if(locos.lower_bound(0x10000)!=locos.end()) + command("create(10)"); + } + else if(!msg.header.value.compare(0, 10, "create(11,")) + { + Message::ObjectMap::const_iterator i = msg.content.find(11); + if(i!=msg.content.end()) + { + Message::AttribMap::const_iterator j = i->second.find("id"); + if(j!=i->second.end()) + { + unsigned id = lexical_cast(j->second); + TurnoutMap::iterator k = turnouts.lower_bound(0x10000); + if(k!=turnouts.end()) + { + command(format("request(%d, view, control)", id)); + command(format("set(%d, addr[%d], symbol[%d], name1[\"Switch\"], name2[\"%d\"], name3[\"\"])", + id, k->second.address, k->second.symbol, k->second.address)); + command(format("set(%d, state[%d])", id, k->second.state&((1<second.bits)-1))); + + k->second.synced = true; + turnouts.insert(TurnoutMap::value_type(id, k->second)); + turnouts.erase(k); + } + } + } + } + else if(!msg.header.value.compare(0, 10, "create(26,")) + command("queryObjects(26)"); +} + +void CentralStation::process_event(const Message &msg) +{ + for(Message::ObjectMap::const_iterator i=msg.content.begin(); i!=msg.content.end(); ++i) + process_object(i->first, i->second); +} + +void CentralStation::process_object(unsigned id, const Message::AttribMap &attribs) +{ + if(id==1) + { + for(Message::AttribMap::const_iterator i=attribs.begin(); i!=attribs.end(); ++i) + if(i->first=="status") + { + power = (i->second=="GO"); + signal_power.emit(power); + } + } + else if(locos.count(id)) + { + Locomotive &loco = locos[id]; + bool speed_changed = false; + unsigned funcs_changed = 0; + for(Message::AttribMap::const_iterator i=attribs.begin(); i!=attribs.end(); ++i) + { + if(i->first=="name") + loco.name = i->second.substr(1, i->second.size()-2); + else if(i->first=="addr") + { + loco_addr.erase(loco.address); + loco.address = lexical_cast(i->second); + loco_addr[loco.address] = id; + } + else if(i->first=="protocol") + { + if(i->second=="MM") + loco.protocol = MM; + else if(i->second=="MM27") + loco.protocol = MM_27; + else if(i->second=="MFX") + loco.protocol = MFX; + } + else if(i->first=="speedstep") + { + loco.speed = lexical_cast(i->second); + speed_changed = true; + } + else if(i->first=="dir") + { + loco.reverse = i->second[0]!='0'; + speed_changed = true; + } + else if(i->first=="func") + { + vector parts = split(i->second, ", "); + unsigned func = lexical_cast(parts[0]); + bool value = lexical_cast(parts[1]); + loco.funcs &= ~(1<first=="msg") + { + if(i->second=="CONTROL_LOST") + command(format("request(%d, control, force)", id)); + } + } + + if(speed_changed) + signal_loco_speed.emit(loco.address, loco.speed, loco.reverse); + for(unsigned i=0; funcs_changed>>i; ++i) + if(funcs_changed&(1<first=="addr") + { + turnout_addr.erase(turnout.address); + turnout.address = lexical_cast(i->second); + turnout_addr[turnout.address] = id; + } + else if(i->first=="state") + { + unsigned state = lexical_cast(i->second); + unsigned mask = (1<first=="state") + { + unsigned state = lexical_cast(i->second, "%i"); + for(unsigned j=0; j<16; ++j) + { + unsigned addr = base*16+j+1; + Sensor &sensor = sensors[addr]; + bool s = state&(1< +unsigned CentralStation::map_address(const map &omap, const AddressMap &amap, unsigned addr) const +{ + if(omap.count(addr)) + return addr; + else + { + AddressMap::const_iterator i = amap.find(addr); + if(i!=amap.end()) + return i->second; + else + return 0; + } +} + +void CentralStation::skip(string::iterator &iter, const string::iterator &end, const string &what) const +{ + for(; (iter!=end && what.find(*iter)!=string::npos); ++iter) ; +} + +string CentralStation::parse_token(string::iterator &iter, const string::iterator &end, const string &stop) const +{ + vector parens; + bool quote = false; + string token; + + skip(iter, end, stop); + + for(; iter!=end; ++iter) + { + if(stop.find(*iter)!=string::npos && parens.empty() && !quote) + break; + else if(*iter=='(' || *iter=='[') + parens.push_back(*iter); + else if((*iter==')' || *iter==']') && !parens.empty()) + { + if((*iter==')' && parens.back()!='(') || (*iter==']' && parens.back()!='[')) + IO::print("Mismatched parentheses\n"); + parens.pop_back(); + } + else if(*iter=='"') + quote = !quote; + + token += *iter; + } + + return token; +} + +CentralStation::Tag CentralStation::parse_tag(string::iterator &iter, const string::iterator &end) const +{ + Tag tag; + + for(; (iter!=end && *iter!='<'); ++iter) ; + if(iter==end) + return Tag(); + + tag.type = parse_token(++iter, end, " >"); + if(tag.type=="END") + { + string code = parse_token(iter, end, " >"); + tag.code = lexical_cast(code); + } + skip(iter, end, " "); + tag.value = parse_token(iter, end, ">"); + if(iter==end) + return Tag(); + ++iter; + + return tag; +} + +CentralStation::Message CentralStation::parse_message(string::iterator &iter, const string::iterator &end) const +{ + Message msg; + + msg.header = parse_tag(iter, end); + + while(iter!=end) + { + skip(iter, end, "\r\n"); + if(*iter=='<') + break; + + string id = parse_token(iter, end, " \r\n<"); + Message::AttribMap &attribs = msg.content[lexical_cast(id)]; + while(iter!=end && *iter!='\n' && *iter!='\r') + { + string attr = parse_token(iter, end, " \r\n<"); + string::size_type open_bracket = attr.find('['); + if(open_bracket!=string::npos) + { + string::size_type close_bracket = attr.rfind(']'); + attribs[attr.substr(0, open_bracket)] = attr.substr(open_bracket+1, close_bracket-open_bracket-1); + } + else + attribs[attr]; + } + } + + msg.footer = parse_tag(iter, end); + if(msg.footer.type.empty()) + return Message(); + + return msg; +} + + +CentralStation::Tag::Tag(): + code(0) +{ } + +CentralStation::Tag::operator bool() const +{ + return !type.empty(); +} + + +CentralStation::Message::operator bool() const +{ + return header && footer; +} + + +CentralStation::Locomotive::Locomotive(): + address(0), + speed(0), + reverse(false), + funcs(0), + control(false) +{ } + + +CentralStation::Turnout::Turnout(): + address(0), + symbol(0), + state(0), + bits(0), + synced(false) +{ } + + +CentralStation::Sensor::Sensor(): + state(false) +{ } + +} // namespace R2C2 diff --git a/source/libr2c2/centralstation.h b/source/libr2c2/centralstation.h new file mode 100644 index 0000000..ce5253b --- /dev/null +++ b/source/libr2c2/centralstation.h @@ -0,0 +1,170 @@ +/* $Id$ + +This file is part of R²C² +Copyright © 2010 Mikkosoft Productions, Mikko Rasa +Distributed under the GPL +*/ + +#ifndef LIBR2C2_CENTRALSTATION_H_ +#define LIBR2C2_CENTRALSTATION_H_ + +#include +#include +#include "driver.h" + +namespace R2C2 { + +class CentralStation: public Driver +{ +private: + struct Tag + { + std::string type; + unsigned code; + std::string value; + + Tag(); + + operator bool() const; + }; + + struct Message + { + typedef std::map AttribMap; + typedef std::map ObjectMap; + + Tag header; + ObjectMap content; + Tag footer; + + operator bool() const; + }; + + enum Error + { + OK, + NERROR_UNKNOWNCOMMAND = 10, + NERROR_UNKNOWNARG = 11, + NERROR_UNKNOWNPARAM = 12, + NERROR_UNKNOWNID = 15, + NERROR_SIZE = 20, + NERROR_NOTALLOWED = 22, + NERROR_NOCONTROL = 25, + NERROR_NOCREATE = 28, + NERROR_NOARG = 29 + }; + + enum Protocol + { + MM, + MM_27, + MFX + }; + + struct Locomotive + { + std::string name; + Protocol protocol; + unsigned address; + unsigned speed; + bool reverse; + unsigned funcs; + bool control; + + Locomotive(); + }; + + struct Turnout + { + enum Symbol + { + LEFT, + RIGHT, + THREEWAY, + DOUBLESLIP + }; + + unsigned address; + unsigned symbol; + unsigned state; + unsigned bits; + bool synced; + + Turnout(); + }; + + struct Sensor + { + bool state; + Msp::Time::TimeStamp off_timeout; + + Sensor(); + }; + + typedef std::map AddressMap; + typedef std::map LocoMap; + typedef std::map TurnoutMap; + typedef std::map SensorMap; + + Msp::Net::StreamSocket socket; + std::list cmd_queue; + unsigned pending_commands; + bool power; + bool halted; + std::string in_buffer; + LocoMap locos; + AddressMap loco_addr; + bool locos_synced; + TurnoutMap turnouts; + AddressMap turnout_addr; + bool turnouts_synced; + SensorMap sensors; + std::vector s88; + bool sensors_synced; + +public: + CentralStation(const std::string &); + ~CentralStation(); + + virtual void set_power(bool); + virtual bool get_power() const { return power; } + virtual void halt(bool); + virtual bool is_halted() const { return halted; } + + virtual const char *enumerate_protocols(unsigned) const; + virtual unsigned get_protocol_speed_steps(const std::string &) const; + + virtual void add_loco(unsigned, const std::string &, const VehicleType &); + virtual void set_loco_speed(unsigned, unsigned); + virtual void set_loco_reverse(unsigned, bool); + virtual void set_loco_function(unsigned, unsigned, bool); + + virtual void add_turnout(unsigned, const TrackType &); + virtual void set_turnout(unsigned, unsigned); + virtual unsigned get_turnout(unsigned) const; + + virtual void add_sensor(unsigned); + virtual void set_sensor(unsigned, bool) { } + virtual bool get_sensor(unsigned) const; + + virtual void tick(); + virtual void flush(); + +private: + void command(const std::string &, bool = false); + Message receive(); + void process_reply(const Message &); + void process_event(const Message &); + void process_object(unsigned, const Message::AttribMap &); + Protocol map_protocol(const std::string &) const; + template + unsigned map_address(const std::map &, const AddressMap &, unsigned) const; + void skip(std::string::iterator &, const std::string::iterator &, const std::string &) const; + std::string parse_token(std::string::iterator &, const std::string::iterator &, const std::string &) const; + Tag parse_tag(std::string::iterator &, const std::string::iterator &) const; + Message parse_message(std::string::iterator &, const std::string::iterator &) const; +}; + +} // namespace R2C2 + +#endif diff --git a/source/libr2c2/driver.cpp b/source/libr2c2/driver.cpp index ea903af..a587b96 100644 --- a/source/libr2c2/driver.cpp +++ b/source/libr2c2/driver.cpp @@ -6,6 +6,7 @@ Distributed under the GPL */ #include +#include "centralstation.h" #include "driver.h" #include "dummy.h" #include "intellibox.h" @@ -25,6 +26,8 @@ Driver *Driver::create(const string &str) if(type=="ib" || type=="intellibox") return new Intellibox(params); + else if(type=="cs" || type=="centralstation") + return new CentralStation(params); else if(type=="dummy") return new Dummy; -- 2.45.2