]> git.tdb.fi Git - r2c2.git/blob - source/libmarklin/intellibox.cpp
Add framework for generating simple meshes for vehicles
[r2c2.git] / source / libmarklin / intellibox.cpp
1 /* $Id$
2
3 This file is part of the MSP Märklin suite
4 Copyright © 2010  Mikkosoft Productions, Mikko Rasa
5 Distributed under the GPL
6 */
7
8 #include <fcntl.h>
9 #include <termios.h>
10 #include <sys/poll.h>
11 #include <msp/io/print.h>
12 #include <msp/time/units.h>
13 #include <msp/time/utils.h>
14 #include "intellibox.h"
15
16 using namespace std;
17 using namespace Msp;
18
19 namespace Marklin {
20
21 Intellibox::Intellibox(const string &dev):
22         power(false),
23         halted(false),
24         update_sensors(false),
25         command_sent(false)
26 {
27         serial_fd = ::open(dev.c_str(), O_RDWR);
28         if(serial_fd<0)
29                 throw Exception("Couldn't open serial port\n");
30
31         static unsigned baud[]=
32         {
33                  2400, B2400,
34                  4800, B4800,
35                  9600, B9600,
36                 19200, B19200,
37                 0
38         };
39
40         termios attr;
41         tcgetattr(serial_fd, &attr);
42         cfmakeraw(&attr);
43         attr.c_cflag |= CSTOPB;
44
45         bool ok = false;
46         bool p50 = false;
47         for(unsigned i=0; baud[i]; i+=2)
48         {
49                 cfsetospeed(&attr, baud[i+1]);
50                 tcsetattr(serial_fd, TCSADRAIN, &attr);
51
52                 write(serial_fd, "\xC4", 1);
53
54                 pollfd pfd = { serial_fd, POLLIN, 0 };
55                 if(poll(&pfd, 1, 500)>0)
56                 {
57                         IO::print("IB detected at %d bits/s\n", baud[i]);
58                         char buf[2];
59                         p50 = (read(serial_fd, buf, 2)==2);
60                         ok = true;
61                         break;
62                 }
63         }
64
65         if(!ok)
66                 throw Exception("IB not detected");
67
68         if(p50)
69                 write(serial_fd, "xZzA1\r", 6);
70
71         command(CMD_STATUS);
72 }
73
74 void Intellibox::set_power(bool p)
75 {
76         power = p;
77         if(power)
78                 command(CMD_POWER_ON);
79         else
80                 command(CMD_POWER_OFF);
81         signal_power.emit(power);
82 }
83
84 void Intellibox::halt(bool h)
85 {
86         halted = h;
87         if(halted)
88         {
89                 for(map<unsigned, Locomotive>::iterator i=locos.begin(); i!=locos.end(); ++i)
90                         if(i->second.speed)
91                                 set_loco_speed(i->first, 0);
92         }
93
94         signal_halt.emit(halted);
95 }
96
97 void Intellibox::add_loco(unsigned addr)
98 {
99         if(!locos.count(addr))
100         {
101                 locos[addr];
102
103                 unsigned char data[2];
104                 data[0] = addr&0xFF;
105                 data[1] = (addr>>8)&0xFF;
106                 command(CMD_LOK_STATUS, addr, data, 2);
107         }
108 }
109
110 void Intellibox::set_loco_speed(unsigned addr, unsigned speed)
111 {
112         Locomotive &loco = locos[addr];
113         if(speed==loco.speed)
114                 return;
115         if(speed && halted)
116                 return;
117
118         loco.speed = speed;
119         loco_command(addr, speed, loco.reverse, loco.funcs|0x100);
120 }
121
122 void Intellibox::set_loco_reverse(unsigned addr, bool rev)
123 {
124         Locomotive &loco = locos[addr];
125         if(rev==loco.reverse)
126                 return;
127
128         loco.reverse = rev;
129         loco_command(addr, loco.speed, rev, loco.funcs|0x100);
130 }
131
132 void Intellibox::set_loco_function(unsigned addr, unsigned func, bool state)
133 {
134         Locomotive &loco = locos[addr];
135         if(state)
136                 loco.funcs |= 1<<func;
137         else
138                 loco.funcs &= ~(1<<func);
139         loco_command(addr, loco.speed, loco.reverse, loco.funcs);
140         signal_loco_function.emit(addr, func, state);
141 }
142
143 void Intellibox::add_turnout(unsigned addr)
144 {
145         if(!turnouts.count(addr))
146         {
147                 turnouts[addr];
148
149                 unsigned char data[2];
150                 data[0] = addr&0xFF;
151                 data[1] = (addr>>8)&0xFF;
152                 command(CMD_TURNOUT_STATUS, addr, data, 2);
153         }
154 }
155
156 void Intellibox::set_turnout(unsigned addr, bool state)
157 {
158         Turnout &turnout = turnouts[addr];
159         if(state==turnout.state || state==turnout.pending)
160                 return;
161
162         turnout.pending = state;
163         turnout.active = true;
164         turnout.off_timeout = Time::TimeStamp();
165
166         turnout_command(addr, state, true);
167 }
168
169 bool Intellibox::get_turnout(unsigned addr) const
170 {
171         map<unsigned, Turnout>::const_iterator i = turnouts.find(addr);
172         if(i!=turnouts.end())
173                 return i->second.state;
174         return false;
175 }
176
177 void Intellibox::add_sensor(unsigned addr)
178 {
179         if(!sensors.count(addr))
180         {
181                 sensors[addr];
182                 update_sensors = true;
183         }
184 }
185
186 bool Intellibox::get_sensor(unsigned addr) const
187 {
188         map<unsigned, Sensor>::const_iterator i = sensors.find(addr);
189         if(i!=sensors.end())
190                 return i->second.state;
191         return false;
192 }
193
194 void Intellibox::tick()
195 {
196         const Time::TimeStamp t = Time::now();
197
198         if(t>next_event_query)
199         {
200                 next_event_query = t+200*Time::msec;
201                 command(CMD_EVENT);
202         }
203
204         for(map<unsigned, Turnout>::iterator i=turnouts.begin(); i!=turnouts.end(); ++i)
205                 if(i->second.active && i->second.off_timeout && t>i->second.off_timeout)
206                 {
207                         i->second.active = false;
208                         i->second.off_timeout = Time::TimeStamp();
209                         turnout_command(i->first, i->second.state, false);
210                 }
211
212         for(map<unsigned, Sensor>::iterator i=sensors.begin(); i!=sensors.end(); ++i)
213                 if(i->second.off_timeout && t>i->second.off_timeout)
214                 {
215                         i->second.state = false;
216                         i->second.off_timeout = Time::TimeStamp();
217                         signal_sensor.emit(i->first, false);
218                 }
219
220         if(update_sensors)
221         {
222                 unsigned max_addr = (--sensors.end())->first;
223                 unsigned char data[2];
224                 data[0] = 0;
225                 data[1] = (max_addr+7)/8;
226                 command(CMD_SENSOR_PARAM_SET, data, 2);
227                 command(CMD_SENSOR_REPORT);
228                 update_sensors = false;
229         }
230
231         if(!queue.empty() && command_sent)
232         {
233                 pollfd pfd = { serial_fd, POLLIN, 0 };
234                 if(poll(&pfd, 1, 0)>0)
235                 {
236                         process_reply(t);
237                         queue.erase(queue.begin());
238                         command_sent = false;
239                 }
240                 else
241                         return;
242         }
243
244         if(!queue.empty())
245         {
246                 const CommandSlot &slot = queue.front();
247                 write(serial_fd, slot.data, slot.length);
248                 command_sent = true;
249         }
250 }
251
252 void Intellibox::flush()
253 {
254         Time::TimeStamp t = Time::now();
255         for(list<CommandSlot>::iterator i=queue.begin(); i!=queue.end(); ++i)
256         {
257                 write(serial_fd, i->data, i->length);
258                 pollfd pfd = { serial_fd, POLLIN, 0 };
259                 bool first = true;
260                 while(poll(&pfd, 1, (first ? -1 : 0))>0)
261                 {
262                         char data[16];
263                         read(serial_fd, data, 16);
264                         first = false;
265                 }
266         }
267
268         queue.clear();
269         command_sent = false;
270 }
271
272 void Intellibox::command(Command cmd)
273 {
274         command(cmd, 0, 0);
275 }
276
277 void Intellibox::command(Command cmd, const unsigned char *data, unsigned len)
278 {
279         command(cmd, 0, data, len);
280 }
281
282 void Intellibox::command(Command cmd, unsigned addr, const unsigned char *data, unsigned len)
283 {
284         CommandSlot slot;
285         slot.cmd = cmd;
286         slot.addr = addr;
287         slot.data[0] = cmd;
288         copy(data, data+len, slot.data+1);
289         slot.length = 1+len;
290         queue.push_back(slot);
291 }
292
293 void Intellibox::loco_command(unsigned addr, unsigned speed, bool rev, unsigned funcs)
294 {
295         unsigned char data[4];
296         data[0] = addr&0xFF;
297         data[1] = (addr>>8)&0xFF;
298
299         if(speed==0)
300                 data[2] = 0;
301         else if(speed==1)
302                 data[2] = 2;
303         else
304                 data[2] = (speed*19-18)/2;
305         
306         data[3] = (rev ? 0 : 0x20) | ((funcs&1) ? 0x10 : 0);
307
308         if(!(funcs&0x100))
309                 data[3] |= 0x80 | ((funcs>>1)&0xF);
310
311         command(CMD_LOK, addr, data, 4);
312 }
313
314 void Intellibox::turnout_command(unsigned addr, bool state, bool active)
315 {
316         unsigned char data[2];
317         data[0] = addr&0xFF;
318         data[1] = ((addr>>8)&0x7) | (active ? 0x40 : 0) | (state ? 0x80 : 0);
319         command(CMD_TURNOUT, addr, data, 2);
320 }
321
322 void Intellibox::process_reply(const Time::TimeStamp &t)
323 {
324         Command cmd = queue.front().cmd;
325
326         if(cmd==CMD_STATUS)
327         {
328                 unsigned char status;
329                 read_all(&status, 1);
330                 power = status&0x08;
331                 signal_power.emit(power);
332         }
333         else if(cmd==CMD_EVENT)
334         {
335                 for(unsigned i=0;; ++i)
336                 {
337                         unsigned char byte;
338                         read_all(&byte, 1);
339
340                         if(i==0)
341                         {
342                                 if(byte&0x01)
343                                         command(CMD_EVENT_LOK);
344                                 if(byte&0x20)
345                                         command(CMD_EVENT_TURNOUT);
346                                 if(byte&0x04)
347                                         command(CMD_EVENT_SENSOR);
348                         }
349                         else if(i==1)
350                         {
351                                 if(byte&0x40)
352                                         command(CMD_STATUS);
353                         }
354
355                         if(!(byte&0x80))
356                                 break;
357                 }
358         }
359         else if(cmd==CMD_EVENT_LOK)
360         {
361                 while(1)
362                 {
363                         unsigned char data[5];
364                         read_all(data, 1);
365                         if(data[0]==0x80)
366                                 break;
367                         read_all(data+1, 4);
368                 }
369         }
370         else if(cmd==CMD_EVENT_TURNOUT)
371         {
372                 unsigned char count;
373                 read_all(&count, 1);
374                 for(unsigned i=0; i<count; ++i)
375                 {
376                         unsigned char data[2];
377                         read_all(data, 2);
378
379                         unsigned addr = data[0]+((data[1]&7)<<8);
380                         Turnout &turnout = turnouts[addr];
381                         turnout.state = (data[1]&0x80)!=0;
382                         turnout.pending = turnout.state;
383                         signal_turnout.emit(addr, turnout.state);
384                 }
385         }
386         else if(cmd==CMD_EVENT_SENSOR)
387         {
388                 while(1)
389                 {
390                         unsigned char mod;
391                         read_all(&mod, 1);
392                         if(!mod)
393                                 break;
394
395                         unsigned char data[2];
396                         read_all(data, 2);
397                         for(unsigned i=0; i<16; ++i)
398                         {
399                                 unsigned addr = mod*16+i-15;
400                                 bool state = (data[i/8]>>(7-i%8))&1;
401
402                                 Sensor &sensor = sensors[addr];
403                                 if(state)
404                                 {
405                                         sensor.off_timeout = Time::TimeStamp();
406                                         if(!sensor.state)
407                                         {
408                                                 sensor.state = state;
409                                                 signal_sensor(addr, state);
410                                         }
411                                 }
412                                 else if(sensor.state)
413                                         sensor.off_timeout = t+700*Time::msec;
414                         }
415                 }
416         }
417         else if(cmd==CMD_LOK)
418         {
419                 Error err;
420                 read_status(&err);
421
422                 if(err==ERR_NO_ERROR)
423                 {
424                         unsigned addr = queue.front().addr;
425                         Locomotive &loco = locos[addr];
426                         signal_loco_speed.emit(addr, loco.speed, loco.reverse);
427                 }
428                 else
429                         error(cmd, err);
430         }
431         else if(cmd==CMD_TURNOUT)
432         {
433                 Error err;
434                 read_status(&err);
435
436                 unsigned addr = queue.front().addr;
437                 Turnout &turnout = turnouts[addr];
438
439                 if(err==ERR_NO_ERROR)
440                 {
441                         turnout.state = turnout.pending;
442                         if(turnout.active)
443                         {
444                                 signal_turnout.emit(addr, turnout.state);
445                                 turnout.off_timeout = t+500*Time::msec;
446                         }
447                 }
448                 else if(err==ERR_NO_I2C_SPACE)
449                         queue.push_back(queue.front());
450                 else
451                 {
452                         turnout.pending = turnout.state;
453                         error(cmd, err);
454                 }
455         }
456         else if(cmd==CMD_TURNOUT_STATUS)
457         {
458                 Error err;
459                 read_status(&err);
460
461                 if(err==ERR_NO_ERROR)
462                 {
463                         unsigned char data;
464                         read_all(&data, 1);
465
466                         unsigned addr = queue.front().addr;
467                         bool state = data&0x04;
468
469                         Turnout &turnout = turnouts[addr];
470                         if(state!=turnout.state)
471                         {
472                                 turnout.state = state;
473                                 turnout.pending = state;
474                                 signal_turnout.emit(addr, turnout.state);
475                         }
476                 }
477                 else
478                         error(cmd, err);
479         }
480         else if(cmd==CMD_LOK_STATUS)
481         {
482                 Error err;
483                 read_status(&err);
484
485                 if(err==ERR_NO_ERROR)
486                 {
487                         unsigned char data[3];
488                         read_all(data, 3);
489
490                         unsigned addr = queue.front().addr;
491                         Locomotive &loco = locos[addr];
492
493                         unsigned speed = (data[0]<=1 ? 0 : data[0]*2/19+1);
494                         bool reverse = !(data[1]&0x20);
495                         if(speed!=loco.speed || reverse!=loco.reverse)
496                         {
497                                 loco.speed = speed;
498                                 loco.reverse = reverse;
499                                 signal_loco_speed.emit(addr, loco.speed, loco.reverse);
500                         }
501
502                         unsigned funcs = (data[1]&0xF)<<1;
503                         if(data[1]&0x10)
504                                 funcs |= 1;
505                         if(funcs!=loco.funcs)
506                         {
507                                 unsigned changed = loco.funcs^funcs;
508                                 loco.funcs = funcs;
509                                 for(unsigned i=0; i<5; ++i)
510                                         if(changed&(1<<i))
511                                                 signal_loco_function.emit(addr, i, loco.funcs&(1<<i));
512                         }
513                 }
514                 else
515                         error(cmd, err);
516         }
517         else
518         {
519                 unsigned expected_bytes = 0;
520                 if(cmd==CMD_FUNC_STATUS)
521                         expected_bytes = 1;
522                 if(cmd==CMD_TURNOUT_GROUP_STATUS)
523                         expected_bytes = 2;
524                 if(cmd==CMD_LOK_CONFIG)
525                         expected_bytes = 4;
526
527                 Error err;
528                 read_status(&err);
529
530                 if(err==ERR_NO_ERROR)
531                 {
532                         unsigned char data[8];
533                         read_all(data, expected_bytes);
534                 }
535                 else
536                         error(cmd, err);
537         }
538 }
539
540 unsigned Intellibox::read_all(unsigned char *buf, unsigned len)
541 {
542         unsigned pos = 0;
543         while(pos<len)
544                 pos += read(serial_fd, buf+pos, len-pos);
545
546         return pos;
547 }
548
549 unsigned Intellibox::read_status(Error *err)
550 {
551         unsigned char c;
552         unsigned ret = read_all(&c, 1);
553         *err = static_cast<Error>(c);
554         return ret;
555 }
556
557 void Intellibox::error(Command cmd, Error err)
558 {
559         const char *cmd_str = 0;
560         switch(cmd)
561         {
562         case CMD_LOK: cmd_str = "CMD_LOK"; break;
563         case CMD_LOK_STATUS: cmd_str = "CMD_LOK_STATUS"; break;
564         case CMD_LOK_CONFIG: cmd_str = "CMD_LOK_CONFIG"; break;
565         case CMD_FUNC: cmd_str = "CMD_FUNC"; break;
566         case CMD_FUNC_STATUS: cmd_str = "CMD_FUNC_STATUS"; break;
567         case CMD_TURNOUT: cmd_str = "CMD_TURNOUT"; break;
568         case CMD_TURNOUT_FREE: cmd_str = "CMD_TURNOUT_FREE"; break;
569         case CMD_TURNOUT_STATUS: cmd_str = "CMD_TURNOUT_STATUS"; break;
570         case CMD_TURNOUT_GROUP_STATUS: cmd_str = "CMD_TURNOUT_GROUP_STATUS"; break;
571         case CMD_SENSOR_STATUS: cmd_str = "CMD_SENSOR_STATUS"; break;
572         case CMD_SENSOR_REPORT: cmd_str = "CMD_SENSOR_REPORT"; break;
573         case CMD_SENSOR_PARAM_SET: cmd_str = "CMD_SENSOR_PARAM_SET"; break;
574         case CMD_STATUS: cmd_str = "CMD_STATUS"; break;
575         case CMD_POWER_OFF: cmd_str = "CMD_POWER_OFF"; break;
576         case CMD_POWER_ON: cmd_str = "CMD_POWER_ON"; break;
577         case CMD_NOP: cmd_str = "CMD_NOP"; break;
578         case CMD_EVENT: cmd_str = "CMD_EVENT"; break;
579         case CMD_EVENT_LOK: cmd_str = "CMD_EVENT_LOK"; break;
580         case CMD_EVENT_TURNOUT: cmd_str = "CMD_EVENT_TURNOUT"; break;
581         case CMD_EVENT_SENSOR: cmd_str = "CMD_EVENT_SENSOR"; break;
582         default: cmd_str = "(unknown command)";
583         }
584
585         const char *err_str = 0;
586         switch(err)
587         {
588         case ERR_NO_ERROR: err_str = "ERR_NO_ERROR"; break;
589         case ERR_SYS_ERROR: err_str = "ERR_SYS_ERROR"; break;
590         case ERR_BAD_PARAM: err_str = "ERR_BAD_PARAM"; break;
591         case ERR_POWER_OFF: err_str = "ERR_POWER_OFF"; break;
592         case ERR_NO_LOK_SPACE: err_str = "ERR_NO_LOK_SPACE"; break;
593         case ERR_NO_TURNOUT_SPACE: err_str = "ERR_NO_TURNOUT_SPACE"; break;
594         case ERR_NO_DATA: err_str = "ERR_NO_DATA"; break;
595         case ERR_NO_SLOT: err_str = "ERR_NO_SLOT"; break;
596         case ERR_BAD_LOK_ADDR: err_str = "ERR_BAD_LOK_ADDR"; break;
597         case ERR_LOK_BUSY: err_str = "ERR_LOK_BUSY"; break;
598         case ERR_BAD_TURNOUT_ADDR: err_str = "ERR_BAD_TURNOUT_ADDR"; break;
599         case ERR_BAD_SO_VALUE: err_str = "ERR_BAD_SO_VALUE"; break;
600         case ERR_NO_I2C_SPACE: err_str = "ERR_NO_I2C_SPACE"; break;
601         case ERR_LOW_TURNOUT_SPACE: err_str = "ERR_LOW_TURNOUT_SPACE"; break;
602         case ERR_LOK_HALTED: err_str = "ERR_LOK_HALTED"; break;
603         case ERR_LOK_POWER_OFF: err_str = "ERR_LOK_POWER_OFF"; break;
604         default: cmd_str = "(unknown error)";
605         }
606
607         IO::print("Error: %s: %s\n", cmd_str, err_str);
608 }
609
610
611 Intellibox::Locomotive::Locomotive():
612         speed(0),
613         reverse(false),
614         funcs(0)
615 { }
616
617
618 Intellibox::Turnout::Turnout():
619         state(false),
620         active(false),
621         pending(false)
622 { }
623
624
625 Intellibox::Sensor::Sensor():
626         state(false)
627 { }
628
629 } // namespace Marklin