3 This file is part of the MSP Märklin suite
4 Copyright © 2006-2008 Mikkosoft Productions, Mikko Rasa
5 Distributed under the GPL
12 #include <msp/core/except.h>
13 #include <msp/time/units.h>
14 #include <msp/time/utils.h>
33 for(map<unsigned, Sensor *>::iterator i=sensors.begin(); i!=sensors.end(); ++i)
35 for(map<unsigned, Turnout *>::iterator i=turnouts.begin(); i!=turnouts.end(); ++i)
37 for(map<unsigned, Locomotive *>::iterator i=locomotives.begin(); i!=locomotives.end(); ++i)
42 void Control::set_power(bool p)
46 command(string(1, CMD_POWER_ON));
48 command(string(1, CMD_POWER_OFF));
50 signal_power_event.emit(power);
53 void Control::set_debug(bool d)
58 void Control::open(const string &dev)
60 serial_fd=::open(dev.c_str(), O_RDWR);
62 throw Exception("Couldn't open serial port\n");
64 static unsigned baud[]=
74 tcgetattr(serial_fd, &attr);
79 for(unsigned i=0; baud[i]; i+=2)
81 cfsetospeed(&attr, baud[i+1]);
82 tcsetattr(serial_fd, TCSADRAIN, &attr);
84 write(serial_fd, "\xC4", 1);
86 pollfd pfd={serial_fd, POLLIN, 0};
87 if(poll(&pfd, 1, 500)>0)
89 cout<<"IB detected at "<<baud[i]<<" bits/s, P50 is ";
91 if(read(serial_fd, buf, 2)==2)
108 throw Exception("IB not detected");
110 command(string(1, CMD_STATUS)).signal_done.connect(sigc::mem_fun(this, &Control::status_done));
113 Command &Control::command(const string &cmd)
115 queue.push_back(Command(cmd));
119 void Control::add_turnout(Turnout &t)
121 turnouts[t.get_address()]=&t;
124 Turnout &Control::get_turnout(unsigned id) const
126 map<unsigned, Turnout *>::const_iterator i=turnouts.find(id);
127 if(i==turnouts.end())
128 throw KeyError("Unknown turnout");
133 void Control::add_locomotive(Locomotive &l)
135 locomotives[l.get_address()]=&l;
138 Locomotive &Control::get_locomotive(unsigned id) const
140 map<unsigned, Locomotive *>::const_iterator i=locomotives.find(id);
141 if(i==locomotives.end())
142 throw KeyError("Unknown locomotive");
147 void Control::add_sensor(Sensor &s)
149 sensors[s.get_address()]=&s;
153 Sensor &Control::get_sensor(unsigned id) const
155 map<unsigned, Sensor *>::const_iterator i=sensors.find(id);
157 throw KeyError("Unknown sensor");
164 const Time::TimeStamp t=Time::now();
166 for(map<unsigned, Sensor *>::const_iterator i=sensors.begin(); i!=sensors.end(); ++i)
171 if(t>next_event_query)
173 next_event_query=t+200*Time::msec;
174 command(string(1, CMD_EVENT)).signal_done.connect(sigc::mem_fun(this, &Control::event_query_done));
179 unsigned max_addr=(--sensors.end())->second->get_address();
181 cmd[0]=CMD_SENSOR_PARAM_SET;
183 cmd[2]=(max_addr+7)/8;
185 command(string(1, CMD_SENSOR_REPORT));
189 if(!queue.empty() && queue.front().get_sent())
191 pollfd pfd={serial_fd, POLLIN, 0};
192 if(poll(&pfd, 1, 0)>0)
194 string resp=read_reply(static_cast<Cmd>(static_cast<unsigned char>(queue.front().get_command()[0])));
198 for(unsigned i=0; i<resp.size(); ++i)
199 printf("%02X ", static_cast<unsigned char>(resp[i]));
200 printf("(%d bytes)\n", resp.size());
203 queue.front().signal_done.emit(static_cast<Error>(resp[0]), resp.substr(1));
204 queue.erase(queue.begin());
212 string cmd=queue.front().get_command();
225 for(unsigned i=0; i<cmd.size(); ++i)
226 printf("%02X ", static_cast<unsigned char>(cmd[i]));
227 printf("(%d bytes)\n", cmd.size());
230 write(serial_fd, cmd.data(), cmd.size());
231 queue.front().set_sent(true);
235 Time::Timer::Slot &Control::set_timer(const Time::TimeDelta &dt)
237 return timer.add(dt);
240 void Control::read_all(int fd, char *buf, int size)
245 int len=read(fd, buf+pos, size-pos);
250 string Control::read_reply(Cmd cmd)
255 result+=ERR_NO_ERROR;
256 for(unsigned i=0; i<3; ++i)
259 read(serial_fd, &c, 1);
264 else if(cmd==CMD_EVENT_LOK)
266 result+=ERR_NO_ERROR;
268 read(serial_fd, c+4, 1);
272 read_all(serial_fd, c, 5);
276 else if(cmd==CMD_EVENT_TURNOUT)
278 result+=ERR_NO_ERROR;
280 read(serial_fd, c, 1);
281 read_all(serial_fd, c+1, c[0]*2);
282 result.append(c, c[0]*2+1);
284 else if(cmd==CMD_EVENT_SENSOR)
286 result+=ERR_NO_ERROR;
288 read(serial_fd, c+2, 1);
292 read_all(serial_fd, c, 3);
299 result+=ERR_NO_ERROR;
301 unsigned expected_bytes=1;
302 if(cmd==CMD_FUNC_STATUS || cmd==CMD_TURNOUT_STATUS)
304 if(cmd==CMD_SENSOR_STATUS || cmd==CMD_TURNOUT_GROUP_STATUS)
306 if(cmd==CMD_LOK_STATUS)
308 if(cmd==CMD_LOK_CONFIG)
311 read_all(serial_fd, c, 1);
315 read_all(serial_fd, c+1, expected_bytes-1);
316 result.append(c+1, expected_bytes-1);
323 void Control::status_done(Error, const string &resp)
325 power=((resp[0]&0x08)!=0);
326 signal_power_event.emit(power);
329 void Control::event_query_done(Error, const string &resp)
332 command(string(1, CMD_EVENT_TURNOUT)).signal_done.connect(sigc::mem_fun(this, &Control::turnout_event_done));
334 command(string(1, CMD_EVENT_SENSOR)).signal_done.connect(sigc::mem_fun(this, &Control::sensor_event_done));
335 if(resp.size()>1 && (resp[1]&0x40))
336 command(string(1, CMD_STATUS)).signal_done.connect(sigc::mem_fun(this, &Control::status_done));
339 void Control::turnout_event_done(Error, const string &resp)
341 unsigned count=resp[0];
342 for(unsigned i=0; i<count; ++i)
344 unsigned addr=static_cast<unsigned char>(resp[i*2+1])+((resp[i*2+2]&7)<<8);
345 bool status=!(resp[i*2+2]&0x80);
346 cout<<"Turnout "<<addr<<", status "<<status<<'\n';
347 signal_turnout_event.emit(addr, status);
351 void Control::sensor_event_done(Error, const string &resp)
353 for(unsigned i=0; resp[i]; i+=3)
355 unsigned module=static_cast<unsigned char>(resp[i]);
357 cout<<"S88 module "<<module<<", status ";
358 for(unsigned j=0; j<16; ++j)
359 cout<<((resp[i+1+j/8]>>(7-j%8))&1);
362 for(unsigned j=0; j<16; ++j)
363 signal_sensor_event.emit(module*16+j-15, (resp[i+1+j/8]>>(7-j%8))&1);
367 } // namespace Marklin