source/libmarklin/control.cpp

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