5 #include <msp/core/error.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 Turnout *Control::get_turnout(unsigned id) const
35 TurnoutMap::const_iterator i=turnouts.find(id);
42 Locomotive *Control::get_locomotive(unsigned id) const
44 for(LocomotiveSeq::const_iterator i=locomotives.begin(); i!=locomotives.end(); ++i)
45 if((*i)->get_address()==id)
50 Sensor *Control::get_sensor(unsigned id) const
52 SensorMap::const_iterator i=sensors.find(id);
59 void Control::open(const string &dev)
61 serial_fd=::open(dev.c_str(), O_RDWR);
63 throw Exception("Couldn't open serial port\n");
65 static unsigned baud[]=
75 tcgetattr(serial_fd, &attr);
80 for(unsigned i=0; baud[i]; i+=2)
82 cfsetospeed(&attr, baud[i+1]);
83 tcsetattr(serial_fd, TCSADRAIN, &attr);
85 write(serial_fd, "\xC4", 1);
87 pollfd pfd={serial_fd, POLLIN, 0};
88 if(poll(&pfd, 1, 500)>0)
90 cout<<"IB detected at "<<baud[i]<<" bits/s, P50 is ";
92 if(read(serial_fd, buf, 2)==2)
108 throw Exception("IB not detected");
111 Command &Control::command(const string &cmd)
113 queue.push_back(Command(cmd));
117 void Control::add_turnout(Turnout *t)
119 if(turnouts.count(t->get_address())==0)
120 turnouts.insert(TurnoutMap::value_type(t->get_address(), t));
123 void Control::add_locomotive(Locomotive *l)
125 if(find(locomotives.begin(), locomotives.end(), l)==locomotives.end())
126 locomotives.push_back(l);
129 void Control::add_sensor(Sensor *s)
131 if(sensors.count(s->get_address())==0)
133 sensors.insert(SensorMap::value_type(s->get_address(), s));
140 const Time::TimeStamp t=Time::now();
142 if(t>next_event_query)
144 next_event_query=t+300*Time::msec;
145 command(string(1, CMD_EVENT)).signal_done.connect(sigc::mem_fun(this, &Control::event_query_done));
150 unsigned max_addr=(--sensors.end())->second->get_address();
152 cmd[0]=CMD_SENSOR_PARAM_SET;
154 cmd[2]=(max_addr+7)/8;
156 command(string(1, CMD_SENSOR_REPORT));
160 if(queue.size() && queue.front().get_sent())
162 pollfd pfd={serial_fd, POLLIN, 0};
163 if(poll(&pfd, 1, 0)>0)
165 string resp=read_reply((Cmd)(unsigned char)queue.front().get_command()[0]);
169 for(unsigned i=0; i<resp.size(); ++i)
170 printf("%02X ", (unsigned char)resp[i]);
171 printf("(%d bytes)\n", resp.size());
174 queue.front().signal_done.emit((Error)resp[0], resp.substr(1));
175 queue.erase(queue.begin());
183 string cmd=queue.front().get_command();
196 for(unsigned i=0; i<cmd.size(); ++i)
197 printf("%02X ",(unsigned char)cmd[i]);
198 printf("(%d bytes)\n",cmd.size());
201 write(serial_fd, cmd.data(), cmd.size());
202 queue.front().set_sent(true);
208 for(SensorMap::iterator i=sensors.begin(); i!=sensors.end(); ++i)
210 for(TurnoutMap::iterator i=turnouts.begin(); i!=turnouts.end(); ++i)
212 for(LocomotiveSeq::iterator i=locomotives.begin(); i!=locomotives.end(); ++i)
219 void Control::read_all(int fd, char *buf, int size)
224 int len=read(fd, buf+pos, size-pos);
229 string Control::read_reply(Cmd cmd)
234 result+=ERR_NO_ERROR;
235 for(unsigned i=0; i<3; ++i)
238 read(serial_fd, &c, 1);
243 else if(cmd==CMD_EVENT_LOK)
245 result+=ERR_NO_ERROR;
247 read(serial_fd, c+4, 1);
251 read_all(serial_fd, c, 5);
255 else if(cmd==CMD_EVENT_TURNOUT)
257 result+=ERR_NO_ERROR;
259 read(serial_fd, c, 1);
260 read_all(serial_fd, c+1, c[0]*2);
261 result.append(c, c[0]*2+1);
263 else if(cmd==CMD_EVENT_SENSOR)
265 result+=ERR_NO_ERROR;
267 read(serial_fd, c+2, 1);
271 read_all(serial_fd, c, 3);
277 unsigned expected_bytes=1;
278 if(cmd==CMD_FUNC_STATUS || cmd==CMD_TURNOUT_STATUS)
280 if(cmd==CMD_SENSOR_STATUS || cmd==CMD_TURNOUT_GROUP_STATUS)
282 if(cmd==CMD_LOK_STATUS)
284 if(cmd==CMD_LOK_CONFIG)
287 read_all(serial_fd, c, 1);
291 read_all(serial_fd, c+1, expected_bytes-1);
292 result.append(c+1, expected_bytes-1);
299 void Control::event_query_done(Error, const string &resp)
302 command(string(1, CMD_EVENT_TURNOUT)).signal_done.connect(sigc::mem_fun(this, &Control::turnout_event_done));
304 command(string(1, CMD_EVENT_SENSOR)).signal_done.connect(sigc::mem_fun(this, &Control::sensor_event_done));
307 void Control::turnout_event_done(Error, const string &resp)
309 unsigned count=resp[0];
310 for(unsigned i=0; i<count; ++i)
312 unsigned addr=(unsigned char)resp[i*2+1]+((resp[i*2+2]&7)<<8);
313 bool status=!(resp[i*2+2]&0x80);
314 cout<<"Turnout "<<addr<<", status "<<status<<'\n';
315 signal_turnout_event.emit(addr, status);
319 void Control::sensor_event_done(Error, const string &resp)
321 for(unsigned i=0; resp[i]; i+=3)
323 unsigned module=(unsigned char)resp[i];
324 cout<<"S88 module "<<module<<", status ";
325 for(unsigned j=0; j<16; ++j)
327 bool status=(resp[i+1+j/8]>>(7-j%8))&1;
329 signal_sensor_event.emit(module*16+j-15, status);
335 } // namespace Marklin