source/libmarklin/control.cpp

   1 /* $Id$
   2
   3 This file is part of the MSP Märklin suite
   4 Copyright © 2006-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::set_power(bool p)
  43 {
  44         power=p;
  45         if(power)
  46                 command(CMD_POWER_ON);
  47         else
  48                 command(CMD_POWER_OFF);
  49
  50         signal_power_event.emit(power);
  51 }
  52
  53 void Control::set_debug(bool d)
  54 {
  55         debug=d;
  56 }
  57
  58 void Control::open(const string &dev)
  59 {
  60         serial_fd=::open(dev.c_str(), O_RDWR);
  61         if(serial_fd<0)
  62                 throw Exception("Couldn't open serial port\n");
  63
  64         static unsigned baud[]=
  65         {
  66                  2400, B2400,
  67                  4800, B4800,
  68                  9600, B9600,
  69                 19200, B19200,
  70                 0
  71         };
  72
  73         termios attr;
  74         tcgetattr(serial_fd, &attr);
  75         cfmakeraw(&attr);
  76         attr.c_cflag|=CSTOPB;
  77
  78         bool ok=false;
  79         bool p50=false;
  80         for(unsigned i=0; baud[i]; i+=2)
  81         {
  82                 cfsetospeed(&attr, baud[i+1]);
  83                 tcsetattr(serial_fd, TCSADRAIN, &attr);
  84
  85                 write(serial_fd, "\xC4", 1);
  86
  87                 pollfd pfd={serial_fd, POLLIN, 0};
  88                 if(poll(&pfd, 1, 500)>0)
  89                 {
  90                         cout<<"IB detected at "<<baud[i]<<" bits/s\n";
  91                         char buf[2];
  92                         p50=(read(serial_fd, buf, 2)==2);
  93                         ok=true;
  94                         break;
  95                 }
  96         }
  97
  98         if(!ok)
  99                 throw Exception("IB not detected");
 100
 101         if(p50)
 102                 write(serial_fd, "xZzA1\r", 6);
 103
 104         command(CMD_STATUS).signal_done.connect(sigc::mem_fun(this, &Control::status_done));
 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().get_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::read_all(int fd, char *buf, int size)
 225 {
 226         int pos=0;
 227         while(pos<size)
 228         {
 229                 int len=read(fd, buf+pos, size-pos);
 230                 pos+=len;
 231         }
 232 }
 233
 234 void Control::status_done(const Reply &reply)
 235 {
 236         power=((reply.get_data()[0]&0x08)!=0);
 237         signal_power_event.emit(power);
 238 }
 239
 240 void Control::event_query_done(const Reply &reply)
 241 {
 242         const unsigned char *data=reply.get_data();
 243         if(data[0]&0x01)
 244                 command(CMD_EVENT_LOK);
 245         if(data[0]&0x20)
 246                 command(CMD_EVENT_TURNOUT).signal_done.connect(sigc::mem_fun(this, &Control::turnout_event_done));
 247         if(data[0]&0x04)
 248                 command(CMD_EVENT_SENSOR).signal_done.connect(sigc::mem_fun(this, &Control::sensor_event_done));
 249         if(data[1]&0x40)
 250                 command(CMD_STATUS).signal_done.connect(sigc::mem_fun(this, &Control::status_done));
 251 }
 252
 253 void Control::turnout_event_done(const Reply &reply)
 254 {
 255         const unsigned char *data=reply.get_data();
 256         unsigned count=data[0];
 257         for(unsigned i=0; i<count; ++i)
 258         {
 259                 unsigned addr=(data[i*2+1])+((data[i*2+2]&7)<<8);
 260                 bool status=!(data[i*2+2]&0x80);
 261                 cout<<"Turnout "<<addr<<", status "<<status<<'\n';
 262                 signal_turnout_event.emit(addr, status);
 263         }
 264 }
 265
 266 void Control::sensor_event_done(const Reply &reply)
 267 {
 268         const unsigned char *data=reply.get_data();
 269         for(unsigned i=0; data[i]; i+=3)
 270         {
 271                 unsigned module=data[i];
 272
 273                 cout<<"S88 module "<<module<<", status ";
 274                 for(unsigned j=0; j<16; ++j)
 275                         cout<<((data[i+1+j/8]>>(7-j%8))&1);
 276                 cout<<'\n';
 277
 278                 for(unsigned j=0; j<16; ++j)
 279                         signal_sensor_event.emit(module*16+j-15, (data[i+1+j/8]>>(7-j%8))&1);
 280         }
 281 }
 282
 283 } // namespace Marklin