5 #include <msp/core/except.h>
6 #include <msp/time/units.h>
7 #include <msp/time/utils.h>
24 void Control::set_power(bool p)
28 command(string(1, CMD_POWER_ON));
30 command(string(1, CMD_POWER_OFF));
33 void Control::open(const string &dev)
35 serial_fd=::open(dev.c_str(), O_RDWR);
37 throw Exception("Couldn't open serial port\n");
39 static unsigned baud[]=
49 tcgetattr(serial_fd, &attr);
54 for(unsigned i=0; baud[i]; i+=2)
56 cfsetospeed(&attr, baud[i+1]);
57 tcsetattr(serial_fd, TCSADRAIN, &attr);
59 write(serial_fd, "\xC4", 1);
61 pollfd pfd={serial_fd, POLLIN, 0};
62 if(poll(&pfd, 1, 500)>0)
64 cout<<"IB detected at "<<baud[i]<<" bits/s, P50 is ";
66 if(read(serial_fd, buf, 2)==2)
82 throw Exception("IB not detected");
85 Command &Control::command(const string &cmd)
87 queue.push_back(Command(cmd));
91 void Control::add_turnout(Turnout &t)
93 turnouts[t.get_address()]=&t;
96 Turnout &Control::get_turnout(unsigned id) const
98 map<unsigned, Turnout *>::const_iterator i=turnouts.find(id);
100 throw KeyError("Unknown turnout");
105 void Control::add_locomotive(Locomotive &l)
107 locomotives[l.get_address()]=&l;
110 Locomotive &Control::get_locomotive(unsigned id) const
112 map<unsigned, Locomotive *>::const_iterator i=locomotives.find(id);
113 if(i==locomotives.end())
114 throw KeyError("Unknown locomotive");
119 void Control::add_sensor(Sensor &s)
121 sensors[s.get_address()]=&s;
125 Sensor &Control::get_sensor(unsigned id) const
127 map<unsigned, Sensor *>::const_iterator i=sensors.find(id);
129 throw KeyError("Unknown sensor");
136 const Time::TimeStamp t=Time::now();
138 for(map<unsigned, Sensor *>::const_iterator i=sensors.begin(); i!=sensors.end(); ++i)
143 if(t>next_event_query)
145 next_event_query=t+300*Time::msec;
146 command(string(1, CMD_EVENT)).signal_done.connect(sigc::mem_fun(this, &Control::event_query_done));
151 unsigned max_addr=(--sensors.end())->second->get_address();
153 cmd[0]=CMD_SENSOR_PARAM_SET;
155 cmd[2]=(max_addr+7)/8;
157 command(string(1, CMD_SENSOR_REPORT));
161 if(queue.size() && queue.front().get_sent())
163 pollfd pfd={serial_fd, POLLIN, 0};
164 if(poll(&pfd, 1, 0)>0)
166 string resp=read_reply((Cmd)(unsigned char)queue.front().get_command()[0]);
170 for(unsigned i=0; i<resp.size(); ++i)
171 printf("%02X ", (unsigned char)resp[i]);
172 printf("(%d bytes)\n", resp.size());
175 queue.front().signal_done.emit((Error)resp[0], resp.substr(1));
176 queue.erase(queue.begin());
184 string cmd=queue.front().get_command();
197 for(unsigned i=0; i<cmd.size(); ++i)
198 printf("%02X ",(unsigned char)cmd[i]);
199 printf("(%d bytes)\n",cmd.size());
202 write(serial_fd, cmd.data(), cmd.size());
203 queue.front().set_sent(true);
207 Time::Timer::Slot &Control::set_timer(const Time::TimeDelta &dt)
209 return timer.add(dt);
214 for(map<unsigned, Sensor *>::iterator i=sensors.begin(); i!=sensors.end(); ++i)
216 for(map<unsigned, Turnout *>::iterator i=turnouts.begin(); i!=turnouts.end(); ++i)
218 for(map<unsigned, Locomotive *>::iterator i=locomotives.begin(); i!=locomotives.end(); ++i)
225 void Control::read_all(int fd, char *buf, int size)
230 int len=read(fd, buf+pos, size-pos);
235 string Control::read_reply(Cmd cmd)
240 result+=ERR_NO_ERROR;
241 for(unsigned i=0; i<3; ++i)
244 read(serial_fd, &c, 1);
249 else if(cmd==CMD_EVENT_LOK)
251 result+=ERR_NO_ERROR;
253 read(serial_fd, c+4, 1);
257 read_all(serial_fd, c, 5);
261 else if(cmd==CMD_EVENT_TURNOUT)
263 result+=ERR_NO_ERROR;
265 read(serial_fd, c, 1);
266 read_all(serial_fd, c+1, c[0]*2);
267 result.append(c, c[0]*2+1);
269 else if(cmd==CMD_EVENT_SENSOR)
271 result+=ERR_NO_ERROR;
273 read(serial_fd, c+2, 1);
277 read_all(serial_fd, c, 3);
283 unsigned expected_bytes=1;
284 if(cmd==CMD_FUNC_STATUS || cmd==CMD_TURNOUT_STATUS)
286 if(cmd==CMD_SENSOR_STATUS || cmd==CMD_TURNOUT_GROUP_STATUS)
288 if(cmd==CMD_LOK_STATUS)
290 if(cmd==CMD_LOK_CONFIG)
293 read_all(serial_fd, c, 1);
297 read_all(serial_fd, c+1, expected_bytes-1);
298 result.append(c+1, expected_bytes-1);
305 void Control::event_query_done(Error, const string &resp)
308 command(string(1, CMD_EVENT_TURNOUT)).signal_done.connect(sigc::mem_fun(this, &Control::turnout_event_done));
310 command(string(1, CMD_EVENT_SENSOR)).signal_done.connect(sigc::mem_fun(this, &Control::sensor_event_done));
313 void Control::turnout_event_done(Error, const string &resp)
315 unsigned count=resp[0];
316 for(unsigned i=0; i<count; ++i)
318 unsigned addr=(unsigned char)resp[i*2+1]+((resp[i*2+2]&7)<<8);
319 bool status=!(resp[i*2+2]&0x80);
320 cout<<"Turnout "<<addr<<", status "<<status<<'\n';
321 signal_turnout_event.emit(addr, status);
325 void Control::sensor_event_done(Error, const string &resp)
327 for(unsigned i=0; resp[i]; i+=3)
329 unsigned module=(unsigned char)resp[i];
331 cout<<"S88 module "<<module<<", status ";
332 for(unsigned j=0; j<16; ++j)
333 cout<<((resp[i+1+j/8]>>(7-j%8))&1);
336 for(unsigned j=0; j<16; ++j)
337 signal_sensor_event.emit(module*16+j-15, (resp[i+1+j/8]>>(7-j%8))&1);
341 } // namespace Marklin