]> git.tdb.fi Git - r2c2.git/blob - intellibox.cpp
9ac448e466108f3e964ffc6b6f6bbd9151566faf
[r2c2.git] / intellibox.cpp
1 /* $Id$
2
3 This file is part of R²C²
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 R2C2 {
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 const char *Intellibox::enumerate_protocols(unsigned i) const
98 {
99         if(i==MM)
100                 return "MM";
101         else if(i==MM_27)
102                 return "MM-27";
103         return 0;
104 }
105
106 unsigned Intellibox::get_protocol_speed_steps(const string &proto_name) const
107 {
108         Protocol proto = map_protocol(proto_name);
109         if(proto==MM)
110                 return 14;
111         else if(proto==MM_27)
112                 return 27;
113         return 0;
114 }
115
116 void Intellibox::add_loco(unsigned addr, const string &proto_name)
117 {
118         Protocol proto = map_protocol(proto_name);
119
120         if(!locos.count(addr))
121         {
122                 locos[addr].protocol = proto;
123
124                 unsigned char data[2];
125                 data[0] = addr&0xFF;
126                 data[1] = (addr>>8)&0xFF;
127                 command(CMD_LOK_STATUS, addr, data, 2);
128         }
129 }
130
131 void Intellibox::set_loco_speed(unsigned addr, unsigned speed)
132 {
133         Locomotive &loco = locos[addr];
134         if(speed==loco.speed)
135         {
136                 if(loco.pending_half_step)
137                 {
138                         loco.pending_half_step = 0;
139                         loco.half_step_delay = Time::TimeStamp();
140                         signal_loco_speed.emit(addr, speed, loco.reverse);
141                 }
142                 return;
143         }
144         if(speed && halted)
145                 return;
146
147         if(loco.protocol==MM_27)
148         {
149                 if(speed>27)
150                         speed = 27;
151
152                 if(speed>loco.speed && !(speed&1))
153                 {
154                         loco.pending_half_step = -1;
155                         speed |= 1;
156                 }
157                 else if(speed<loco.speed && (speed&1))
158                 {
159                         loco.pending_half_step = 1;
160                         speed &= ~1;
161                 }
162                 else
163                         loco.pending_half_step = 0;
164                 loco.half_step_delay = Time::TimeStamp();
165
166                 loco_command(addr, (speed+1)/2, loco.reverse, loco.funcs|0x100);
167         }
168         else
169         {
170                 if(speed>14)
171                         speed = 14;
172
173                 loco_command(addr, speed, loco.reverse, loco.funcs|0x100);
174         }
175         loco.speed = speed;
176 }
177
178 void Intellibox::set_loco_reverse(unsigned addr, bool rev)
179 {
180         Locomotive &loco = locos[addr];
181         if(rev==loco.reverse)
182                 return;
183
184         loco.reverse = rev;
185         loco_command(addr, loco.speed, rev, loco.funcs|0x100);
186 }
187
188 void Intellibox::set_loco_function(unsigned addr, unsigned func, bool state)
189 {
190         Locomotive &loco = locos[addr];
191         if(state)
192                 loco.funcs |= 1<<func;
193         else
194                 loco.funcs &= ~(1<<func);
195         loco_command(addr, loco.speed, loco.reverse, loco.funcs);
196         signal_loco_function.emit(addr, func, state);
197 }
198
199 void Intellibox::add_turnout(unsigned addr)
200 {
201         if(!turnouts.count(addr))
202         {
203                 turnouts[addr];
204
205                 unsigned char data[2];
206                 data[0] = addr&0xFF;
207                 data[1] = (addr>>8)&0xFF;
208                 command(CMD_TURNOUT_STATUS, addr, data, 2);
209         }
210 }
211
212 void Intellibox::set_turnout(unsigned addr, bool state)
213 {
214         Turnout &turnout = turnouts[addr];
215         if(state==turnout.state || state==turnout.pending)
216                 return;
217
218         turnout.pending = state;
219         turnout.active = true;
220         turnout.off_timeout = Time::TimeStamp();
221
222         turnout_command(addr, state, true);
223 }
224
225 bool Intellibox::get_turnout(unsigned addr) const
226 {
227         map<unsigned, Turnout>::const_iterator i = turnouts.find(addr);
228         if(i!=turnouts.end())
229                 return i->second.state;
230         return false;
231 }
232
233 void Intellibox::add_sensor(unsigned addr)
234 {
235         if(!sensors.count(addr))
236         {
237                 sensors[addr];
238                 update_sensors = true;
239         }
240 }
241
242 bool Intellibox::get_sensor(unsigned addr) const
243 {
244         map<unsigned, Sensor>::const_iterator i = sensors.find(addr);
245         if(i!=sensors.end())
246                 return i->second.state;
247         return false;
248 }
249
250 void Intellibox::tick()
251 {
252         const Time::TimeStamp t = Time::now();
253
254         if(t>next_event_query)
255         {
256                 next_event_query = t+200*Time::msec;
257                 command(CMD_EVENT);
258         }
259
260         for(map<unsigned, Locomotive>::iterator i=locos.begin(); i!=locos.end(); ++i)
261                 if(i->second.protocol==MM_27 && i->second.pending_half_step && i->second.half_step_delay && t>i->second.half_step_delay)
262                 {
263                         i->second.speed += i->second.pending_half_step;
264                         i->second.pending_half_step = 0;
265                         i->second.half_step_delay = Time::TimeStamp();
266                         loco_command(i->first, (i->second.speed+1)/2, i->second.reverse, i->second.funcs|0x100);
267                 }
268
269         for(map<unsigned, Turnout>::iterator i=turnouts.begin(); i!=turnouts.end(); ++i)
270                 if(i->second.active && i->second.off_timeout && t>i->second.off_timeout)
271                 {
272                         i->second.active = false;
273                         i->second.off_timeout = Time::TimeStamp();
274                         turnout_command(i->first, i->second.state, false);
275                 }
276
277         for(map<unsigned, Sensor>::iterator i=sensors.begin(); i!=sensors.end(); ++i)
278                 if(i->second.off_timeout && t>i->second.off_timeout)
279                 {
280                         i->second.state = false;
281                         i->second.off_timeout = Time::TimeStamp();
282                         signal_sensor.emit(i->first, false);
283                 }
284
285         if(update_sensors)
286         {
287                 unsigned max_addr = (--sensors.end())->first;
288                 unsigned char data[2];
289                 data[0] = 0;
290                 data[1] = (max_addr+7)/8;
291                 command(CMD_SENSOR_PARAM_SET, data, 2);
292                 command(CMD_SENSOR_REPORT);
293                 update_sensors = false;
294         }
295
296         if(!queue.empty() && command_sent)
297         {
298                 pollfd pfd = { serial_fd, POLLIN, 0 };
299                 if(poll(&pfd, 1, 0)>0)
300                 {
301                         process_reply(t);
302                         queue.erase(queue.begin());
303                         command_sent = false;
304                 }
305                 else
306                         return;
307         }
308
309         if(!queue.empty())
310         {
311                 const CommandSlot &slot = queue.front();
312                 write(serial_fd, slot.data, slot.length);
313                 command_sent = true;
314         }
315 }
316
317 void Intellibox::flush()
318 {
319         Time::TimeStamp t = Time::now();
320         for(list<CommandSlot>::iterator i=queue.begin(); i!=queue.end(); ++i)
321         {
322                 write(serial_fd, i->data, i->length);
323                 pollfd pfd = { serial_fd, POLLIN, 0 };
324                 bool first = true;
325                 while(poll(&pfd, 1, (first ? -1 : 0))>0)
326                 {
327                         char data[16];
328                         read(serial_fd, data, 16);
329                         first = false;
330                 }
331         }
332
333         queue.clear();
334         command_sent = false;
335 }
336
337 Intellibox::Protocol Intellibox::map_protocol(const string &name) const
338 {
339         if(name=="MM")
340                 return MM;
341         else if(name=="MM-27")
342                 return MM_27;
343         else
344                 throw InvalidParameterValue("Unknown protocol");
345 }
346
347 void Intellibox::command(Command cmd)
348 {
349         command(cmd, 0, 0);
350 }
351
352 void Intellibox::command(Command cmd, const unsigned char *data, unsigned len)
353 {
354         command(cmd, 0, data, len);
355 }
356
357 void Intellibox::command(Command cmd, unsigned addr, const unsigned char *data, unsigned len)
358 {
359         CommandSlot slot;
360         slot.cmd = cmd;
361         slot.addr = addr;
362         slot.data[0] = cmd;
363         copy(data, data+len, slot.data+1);
364         slot.length = 1+len;
365         queue.push_back(slot);
366 }
367
368 void Intellibox::loco_command(unsigned addr, unsigned speed, bool rev, unsigned funcs)
369 {
370         unsigned char data[4];
371         data[0] = addr&0xFF;
372         data[1] = (addr>>8)&0xFF;
373
374         if(speed==0)
375                 data[2] = 0;
376         else if(speed==1)
377                 data[2] = 2;
378         else
379                 data[2] = (speed*19-18)/2;
380         
381         data[3] = (rev ? 0 : 0x20) | ((funcs&1) ? 0x10 : 0);
382
383         if(!(funcs&0x100))
384                 data[3] |= 0x80 | ((funcs>>1)&0xF);
385
386         command(CMD_LOK, addr, data, 4);
387 }
388
389 void Intellibox::turnout_command(unsigned addr, bool state, bool active)
390 {
391         unsigned char data[2];
392         data[0] = addr&0xFF;
393         data[1] = ((addr>>8)&0x7) | (active ? 0x40 : 0) | (state ? 0x80 : 0);
394         command(CMD_TURNOUT, addr, data, 2);
395 }
396
397 void Intellibox::process_reply(const Time::TimeStamp &t)
398 {
399         Command cmd = queue.front().cmd;
400
401         if(cmd==CMD_STATUS)
402         {
403                 unsigned char status;
404                 read_all(&status, 1);
405                 power = status&0x08;
406                 signal_power.emit(power);
407         }
408         else if(cmd==CMD_EVENT)
409         {
410                 for(unsigned i=0;; ++i)
411                 {
412                         unsigned char byte;
413                         read_all(&byte, 1);
414
415                         if(i==0)
416                         {
417                                 if(byte&0x01)
418                                         command(CMD_EVENT_LOK);
419                                 if(byte&0x20)
420                                         command(CMD_EVENT_TURNOUT);
421                                 if(byte&0x04)
422                                         command(CMD_EVENT_SENSOR);
423                         }
424                         else if(i==1)
425                         {
426                                 if(byte&0x40)
427                                         command(CMD_STATUS);
428                         }
429
430                         if(!(byte&0x80))
431                                 break;
432                 }
433         }
434         else if(cmd==CMD_EVENT_LOK)
435         {
436                 while(1)
437                 {
438                         unsigned char data[5];
439                         read_all(data, 1);
440                         if(data[0]==0x80)
441                                 break;
442                         read_all(data+1, 4);
443                 }
444         }
445         else if(cmd==CMD_EVENT_TURNOUT)
446         {
447                 unsigned char count;
448                 read_all(&count, 1);
449                 for(unsigned i=0; i<count; ++i)
450                 {
451                         unsigned char data[2];
452                         read_all(data, 2);
453
454                         unsigned addr = data[0]+((data[1]&7)<<8);
455                         Turnout &turnout = turnouts[addr];
456                         turnout.state = (data[1]&0x80)!=0;
457                         turnout.pending = turnout.state;
458                         signal_turnout.emit(addr, turnout.state);
459                 }
460         }
461         else if(cmd==CMD_EVENT_SENSOR)
462         {
463                 while(1)
464                 {
465                         unsigned char mod;
466                         read_all(&mod, 1);
467                         if(!mod)
468                                 break;
469
470                         unsigned char data[2];
471                         read_all(data, 2);
472                         for(unsigned i=0; i<16; ++i)
473                         {
474                                 unsigned addr = mod*16+i-15;
475                                 bool state = (data[i/8]>>(7-i%8))&1;
476
477                                 Sensor &sensor = sensors[addr];
478                                 if(state)
479                                 {
480                                         sensor.off_timeout = Time::TimeStamp();
481                                         if(!sensor.state)
482                                         {
483                                                 sensor.state = state;
484                                                 signal_sensor(addr, state);
485                                         }
486                                 }
487                                 else if(sensor.state)
488                                         sensor.off_timeout = t+700*Time::msec;
489                         }
490                 }
491         }
492         else if(cmd==CMD_LOK)
493         {
494                 Error err;
495                 read_status(&err);
496
497                 if(err==ERR_NO_ERROR)
498                 {
499                         unsigned addr = queue.front().addr;
500                         Locomotive &loco = locos[addr];
501                         signal_loco_speed.emit(addr, loco.speed+loco.pending_half_step, loco.reverse);
502                         if(loco.pending_half_step)
503                                 loco.half_step_delay = Time::now()+500*Time::msec;
504                 }
505                 else
506                         error(cmd, err);
507         }
508         else if(cmd==CMD_TURNOUT)
509         {
510                 Error err;
511                 read_status(&err);
512
513                 unsigned addr = queue.front().addr;
514                 Turnout &turnout = turnouts[addr];
515
516                 if(err==ERR_NO_ERROR)
517                 {
518                         turnout.state = turnout.pending;
519                         if(turnout.active)
520                         {
521                                 signal_turnout.emit(addr, turnout.state);
522                                 turnout.off_timeout = t+500*Time::msec;
523                         }
524                 }
525                 else if(err==ERR_NO_I2C_SPACE)
526                         queue.push_back(queue.front());
527                 else
528                 {
529                         turnout.pending = turnout.state;
530                         error(cmd, err);
531                 }
532         }
533         else if(cmd==CMD_TURNOUT_STATUS)
534         {
535                 Error err;
536                 read_status(&err);
537
538                 if(err==ERR_NO_ERROR)
539                 {
540                         unsigned char data;
541                         read_all(&data, 1);
542
543                         unsigned addr = queue.front().addr;
544                         bool state = data&0x04;
545
546                         Turnout &turnout = turnouts[addr];
547                         if(state!=turnout.state)
548                         {
549                                 turnout.state = state;
550                                 turnout.pending = state;
551                                 signal_turnout.emit(addr, turnout.state);
552                         }
553                 }
554                 else
555                         error(cmd, err);
556         }
557         else if(cmd==CMD_LOK_STATUS)
558         {
559                 Error err;
560                 read_status(&err);
561
562                 if(err==ERR_NO_ERROR)
563                 {
564                         unsigned char data[3];
565                         read_all(data, 3);
566
567                         unsigned addr = queue.front().addr;
568                         Locomotive &loco = locos[addr];
569
570                         unsigned speed = (data[0]<=1 ? 0 : data[0]*2/19+1);
571                         bool reverse = !(data[1]&0x20);
572                         if(speed!=loco.speed || reverse!=loco.reverse)
573                         {
574                                 loco.speed = speed;
575                                 loco.reverse = reverse;
576                                 signal_loco_speed.emit(addr, loco.speed, loco.reverse);
577                         }
578
579                         unsigned funcs = (data[1]&0xF)<<1;
580                         if(data[1]&0x10)
581                                 funcs |= 1;
582                         if(funcs!=loco.funcs)
583                         {
584                                 unsigned changed = loco.funcs^funcs;
585                                 loco.funcs = funcs;
586                                 for(unsigned i=0; i<5; ++i)
587                                         if(changed&(1<<i))
588                                                 signal_loco_function.emit(addr, i, loco.funcs&(1<<i));
589                         }
590                 }
591                 else
592                         error(cmd, err);
593         }
594         else
595         {
596                 unsigned expected_bytes = 0;
597                 if(cmd==CMD_FUNC_STATUS)
598                         expected_bytes = 1;
599                 if(cmd==CMD_TURNOUT_GROUP_STATUS)
600                         expected_bytes = 2;
601                 if(cmd==CMD_LOK_CONFIG)
602                         expected_bytes = 4;
603
604                 Error err;
605                 read_status(&err);
606
607                 if(err==ERR_NO_ERROR)
608                 {
609                         unsigned char data[8];
610                         read_all(data, expected_bytes);
611                 }
612                 else
613                         error(cmd, err);
614         }
615 }
616
617 unsigned Intellibox::read_all(unsigned char *buf, unsigned len)
618 {
619         unsigned pos = 0;
620         while(pos<len)
621                 pos += read(serial_fd, buf+pos, len-pos);
622
623         return pos;
624 }
625
626 unsigned Intellibox::read_status(Error *err)
627 {
628         unsigned char c;
629         unsigned ret = read_all(&c, 1);
630         *err = static_cast<Error>(c);
631         return ret;
632 }
633
634 void Intellibox::error(Command cmd, Error err)
635 {
636         const char *cmd_str = 0;
637         switch(cmd)
638         {
639         case CMD_LOK: cmd_str = "CMD_LOK"; break;
640         case CMD_LOK_STATUS: cmd_str = "CMD_LOK_STATUS"; break;
641         case CMD_LOK_CONFIG: cmd_str = "CMD_LOK_CONFIG"; break;
642         case CMD_FUNC: cmd_str = "CMD_FUNC"; break;
643         case CMD_FUNC_STATUS: cmd_str = "CMD_FUNC_STATUS"; break;
644         case CMD_TURNOUT: cmd_str = "CMD_TURNOUT"; break;
645         case CMD_TURNOUT_FREE: cmd_str = "CMD_TURNOUT_FREE"; break;
646         case CMD_TURNOUT_STATUS: cmd_str = "CMD_TURNOUT_STATUS"; break;
647         case CMD_TURNOUT_GROUP_STATUS: cmd_str = "CMD_TURNOUT_GROUP_STATUS"; break;
648         case CMD_SENSOR_STATUS: cmd_str = "CMD_SENSOR_STATUS"; break;
649         case CMD_SENSOR_REPORT: cmd_str = "CMD_SENSOR_REPORT"; break;
650         case CMD_SENSOR_PARAM_SET: cmd_str = "CMD_SENSOR_PARAM_SET"; break;
651         case CMD_STATUS: cmd_str = "CMD_STATUS"; break;
652         case CMD_POWER_OFF: cmd_str = "CMD_POWER_OFF"; break;
653         case CMD_POWER_ON: cmd_str = "CMD_POWER_ON"; break;
654         case CMD_NOP: cmd_str = "CMD_NOP"; break;
655         case CMD_EVENT: cmd_str = "CMD_EVENT"; break;
656         case CMD_EVENT_LOK: cmd_str = "CMD_EVENT_LOK"; break;
657         case CMD_EVENT_TURNOUT: cmd_str = "CMD_EVENT_TURNOUT"; break;
658         case CMD_EVENT_SENSOR: cmd_str = "CMD_EVENT_SENSOR"; break;
659         default: cmd_str = "(unknown command)";
660         }
661
662         const char *err_str = 0;
663         switch(err)
664         {
665         case ERR_NO_ERROR: err_str = "ERR_NO_ERROR"; break;
666         case ERR_SYS_ERROR: err_str = "ERR_SYS_ERROR"; break;
667         case ERR_BAD_PARAM: err_str = "ERR_BAD_PARAM"; break;
668         case ERR_POWER_OFF: err_str = "ERR_POWER_OFF"; break;
669         case ERR_NO_LOK_SPACE: err_str = "ERR_NO_LOK_SPACE"; break;
670         case ERR_NO_TURNOUT_SPACE: err_str = "ERR_NO_TURNOUT_SPACE"; break;
671         case ERR_NO_DATA: err_str = "ERR_NO_DATA"; break;
672         case ERR_NO_SLOT: err_str = "ERR_NO_SLOT"; break;
673         case ERR_BAD_LOK_ADDR: err_str = "ERR_BAD_LOK_ADDR"; break;
674         case ERR_LOK_BUSY: err_str = "ERR_LOK_BUSY"; break;
675         case ERR_BAD_TURNOUT_ADDR: err_str = "ERR_BAD_TURNOUT_ADDR"; break;
676         case ERR_BAD_SO_VALUE: err_str = "ERR_BAD_SO_VALUE"; break;
677         case ERR_NO_I2C_SPACE: err_str = "ERR_NO_I2C_SPACE"; break;
678         case ERR_LOW_TURNOUT_SPACE: err_str = "ERR_LOW_TURNOUT_SPACE"; break;
679         case ERR_LOK_HALTED: err_str = "ERR_LOK_HALTED"; break;
680         case ERR_LOK_POWER_OFF: err_str = "ERR_LOK_POWER_OFF"; break;
681         default: cmd_str = "(unknown error)";
682         }
683
684         IO::print("Error: %s: %s\n", cmd_str, err_str);
685 }
686
687
688 Intellibox::Locomotive::Locomotive():
689         speed(0),
690         reverse(false),
691         funcs(0)
692 { }
693
694
695 Intellibox::Turnout::Turnout():
696         state(false),
697         active(false),
698         pending(false)
699 { }
700
701
702 Intellibox::Sensor::Sensor():
703         state(false)
704 { }
705
706 } // namespace R2C2