source/libmarklin/control.cpp

   1 /* $Id$
   2
   3 This file is part of the MSP Märklin suite
   4 Copyright © 2007-2009  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/core/except.h>
  12 #include <msp/io/print.h>
  13 #include <msp/time/units.h>
  14 #include <msp/time/utils.h>
  15 #include "command.h"
  16 #include "control.h"
  17 #include "reply.h"
  18
  19 using namespace std;
  20 using namespace Msp;
  21
  22 namespace Marklin {
  23
  24 Control::Control():
  25         serial_fd(-1),
  26         power(true),
  27         poll_sensors(false),
  28         debug(false)
  29 { }
  30
  31 Control::~Control()
  32 {
  33         for(map<unsigned, Sensor *>::iterator i=sensors.begin(); i!=sensors.end(); ++i)
  34                 delete i->second;
  35         for(map<unsigned, Turnout *>::iterator i=turnouts.begin(); i!=turnouts.end(); ++i)
  36                 delete i->second;
  37         for(map<unsigned, Locomotive *>::iterator i=locomotives.begin(); i!=locomotives.end(); ++i)
  38                 delete i->second;
  39         if(serial_fd>=0)
  40                 close(serial_fd);
  41 }
  42
  43 void Control::open(const string &dev)
  44 {
  45         serial_fd = ::open(dev.c_str(), O_RDWR);
  46         if(serial_fd<0)
  47                 throw Exception("Couldn't open serial port\n");
  48
  49         static unsigned baud[]=
  50         {
  51                  2400, B2400,
  52                  4800, B4800,
  53                  9600, B9600,
  54                 19200, B19200,
  55                 0
  56         };
  57
  58         termios attr;
  59         tcgetattr(serial_fd, &attr);
  60         cfmakeraw(&attr);
  61         attr.c_cflag |= CSTOPB;
  62
  63         bool ok = false;
  64         bool p50 = false;
  65         for(unsigned i=0; baud[i]; i+=2)
  66         {
  67                 cfsetospeed(&attr, baud[i+1]);
  68                 tcsetattr(serial_fd, TCSADRAIN, &attr);
  69
  70                 write(serial_fd, "\xC4", 1);
  71
  72                 pollfd pfd = { serial_fd, POLLIN, 0 };
  73                 if(poll(&pfd, 1, 500)>0)
  74                 {
  75                         IO::print("IB detected at %d bits/s\n", baud[i]);
  76                         char buf[2];
  77                         p50 = (read(serial_fd, buf, 2)==2);
  78                         ok = true;
  79                         break;
  80                 }
  81         }
  82
  83         if(!ok)
  84                 throw Exception("IB not detected");
  85
  86         if(p50)
  87                 write(serial_fd, "xZzA1\r", 6);
  88
  89         command(CMD_STATUS).signal_done.connect(sigc::mem_fun(this, &Control::status_done));
  90 }
  91
  92 void Control::set_debug(bool d)
  93 {
  94         debug = d;
  95 }
  96
  97 void Control::set_power(bool p)
  98 {
  99         power = p;
 100         if(power)
 101                 command(CMD_POWER_ON);
 102         else
 103                 command(CMD_POWER_OFF);
 104
 105         signal_power_event.emit(power);
 106 }
 107
 108 Command &Control::command(Cmd cmd)
 109 {
 110         queue.push_back(Command(cmd, 0, 0));
 111         return queue.back();
 112 }
 113
 114 Command &Control::command(Cmd cmd, unsigned char data)
 115 {
 116         queue.push_back(Command(cmd, &data, 1));
 117         return queue.back();
 118 }
 119
 120 Command &Control::command(Cmd cmd, const unsigned char *data, unsigned len)
 121 {
 122         queue.push_back(Command(cmd, data, len));
 123         return queue.back();
 124 }
 125
 126 void Control::flush()
 127 {
 128         for(list<Command>::iterator i=queue.begin(); i!=queue.end(); ++i)
 129                 i->send(serial_fd);
 130 }
 131
 132 void Control::add_turnout(Turnout &t)
 133 {
 134         turnouts[t.get_address()] = &t;
 135 }
 136
 137 Turnout &Control::get_turnout(unsigned id) const
 138 {
 139         map<unsigned, Turnout *>::const_iterator i = turnouts.find(id);
 140         if(i==turnouts.end())
 141                 throw KeyError("Unknown turnout");
 142
 143         return *i->second;
 144 }
 145
 146 void Control::add_locomotive(Locomotive &l)
 147 {
 148         locomotives[l.get_address()] = &l;
 149 }
 150
 151 Locomotive &Control::get_locomotive(unsigned id) const
 152 {
 153         map<unsigned, Locomotive *>::const_iterator i = locomotives.find(id);
 154         if(i==locomotives.end())
 155                 throw KeyError("Unknown locomotive");
 156
 157         return *i->second;
 158 }
 159
 160 void Control::add_sensor(Sensor &s)
 161 {
 162         sensors[s.get_address()] = &s;
 163         poll_sensors = true;
 164 }
 165
 166 Sensor &Control::get_sensor(unsigned id) const
 167 {
 168         map<unsigned, Sensor *>::const_iterator i = sensors.find(id);
 169         if(i==sensors.end())
 170                 throw KeyError("Unknown sensor");
 171
 172         return *i->second;
 173 }
 174
 175 void Control::tick()
 176 {
 177         const Time::TimeStamp t = Time::now();
 178
 179         for(map<unsigned, Sensor *>::const_iterator i=sensors.begin(); i!=sensors.end(); ++i)
 180                 i->second->tick();
 181
 182         timer.tick(false);
 183
 184         if(t>next_event_query)
 185         {
 186                 next_event_query = t+200*Time::msec;
 187                 command(CMD_EVENT).signal_done.connect(sigc::mem_fun(this, &Control::event_query_done));
 188         }
 189
 190         if(poll_sensors)
 191         {
 192                 unsigned max_addr = (--sensors.end())->first;
 193                 unsigned char data[2];
 194                 data[0] = 0;
 195                 data[1] = (max_addr+7)/8;
 196                 command(CMD_SENSOR_PARAM_SET, data, 2);
 197                 command(CMD_SENSOR_REPORT);
 198                 poll_sensors = false;
 199         }
 200
 201         if(!queue.empty() && queue.front().is_sent())
 202         {
 203                 pollfd pfd = { serial_fd, POLLIN, 0 };
 204                 if(poll(&pfd, 1, 0)>0)
 205                 {
 206                         Reply reply = Reply::read(serial_fd, queue.front().get_command());
 207                         if(debug)
 208                                 IO::print("R: %s\n", reply);
 209
 210                         queue.front().signal_done.emit(reply);
 211                         queue.erase(queue.begin());
 212                 }
 213                 else
 214                         return;
 215         }
 216
 217         if(!queue.empty())
 218         {
 219                 if(debug)
 220                         IO::print("W: %s\n", queue.front());
 221
 222                 if(serial_fd>=0)
 223                         queue.front().send(serial_fd);
 224                 else
 225                 {
 226                         Reply reply = Reply::simulate(queue.front().get_command());
 227                         queue.front().signal_done.emit(reply);
 228                         queue.erase(queue.begin());
 229                 }
 230         }
 231 }
 232
 233 Time::Timer::Slot &Control::set_timer(const Time::TimeDelta &dt)
 234 {
 235         return timer.add(dt);
 236 }
 237
 238 void Control::status_done(const Reply &reply)
 239 {
 240         power = ((reply.get_data()[0]&0x08)!=0);
 241         signal_power_event.emit(power);
 242 }
 243
 244 void Control::event_query_done(const Reply &reply)
 245 {
 246         const unsigned char *data = reply.get_data();
 247         if(data[0]&0x01)
 248                 command(CMD_EVENT_LOK);
 249         if(data[0]&0x20)
 250                 command(CMD_EVENT_TURNOUT).signal_done.connect(sigc::mem_fun(this, &Control::turnout_event_done));
 251         if(data[0]&0x04)
 252                 command(CMD_EVENT_SENSOR).signal_done.connect(sigc::mem_fun(this, &Control::sensor_event_done));
 253         if((data[0]&0x80) && (data[1]&0x40))
 254                 command(CMD_STATUS).signal_done.connect(sigc::mem_fun(this, &Control::status_done));
 255 }
 256
 257 void Control::turnout_event_done(const Reply &reply)
 258 {
 259         const unsigned char *data = reply.get_data();
 260         unsigned count = data[0];
 261         for(unsigned i=0; i<count; ++i)
 262         {
 263                 unsigned addr = (data[i*2+1])+((data[i*2+2]&7)<<8);
 264                 bool status = !(data[i*2+2]&0x80);
 265                 IO::print("Turnout %d, status %d\n", addr, status);
 266                 signal_turnout_event.emit(addr, status);
 267         }
 268 }
 269
 270 void Control::sensor_event_done(const Reply &reply)
 271 {
 272         const unsigned char *data = reply.get_data();
 273         for(unsigned i=0; data[i]; i+=3)
 274         {
 275                 unsigned module = data[i];
 276
 277                 IO::print("S88 module %d, status %08b%08b\n", module, data[1], data[2]);
 278
 279                 for(unsigned j=0; j<16; ++j)
 280                         signal_sensor_event.emit(module*16+j-15, (data[i+1+j/8]>>(7-j%8))&1);
 281         }
 282 }
 283
 284 } // namespace Marklin