Convert engineer to use mspgbase instead of SDL

[r2c2.git] / source / libmarklin / control.cpp

diff --git a/source/libmarklin/control.cpp b/source/libmarklin/control.cpp
index d371777fc5ca15d58478c001649732d038d33737..6e82614c21ebf904a65fd3d4d29b359bfe77c03e 100644 (file)
--- a/source/libmarklin/control.cpp
+++ b/source/libmarklin/control.cpp
@@ -2,7 +2,7 @@
 #include <termios.h>
 #include <sys/poll.h>
 #include <iostream>
-#include <msp/core/error.h>
+#include <msp/core/except.h>
 #include <msp/time/units.h>
 #include <msp/time/utils.h>
 #include "command.h"
@@ -30,32 +30,6 @@ void Control::set_power(bool p)
                command(string(1, CMD_POWER_OFF));
 }
 
-Turnout *Control::get_turnout(unsigned id) const
-{
-       TurnoutMap::const_iterator i=turnouts.find(id);
-       if(i!=turnouts.end())
-               return i->second;
-       
-       return 0;
-}
-
-Locomotive *Control::get_locomotive(unsigned id) const
-{
-       for(LocomotiveSeq::const_iterator i=locomotives.begin(); i!=locomotives.end(); ++i)
-               if((*i)->get_address()==id)
-                       return *i;
-       return 0;
-}
-
-Sensor *Control::get_sensor(unsigned id) const
-{
-       SensorMap::const_iterator i=sensors.find(id);
-       if(i!=sensors.end())
-               return i->second;
-       
-       return 0;
-}
-
 void Control::open(const string &dev)
 {
        serial_fd=::open(dev.c_str(), O_RDWR);
@@ -114,31 +88,58 @@ Command &Control::command(const string &cmd)
        return queue.back();
 }
 
-void Control::add_turnout(Turnout *t)
+void Control::add_turnout(Turnout &t)
 {
-       if(turnouts.count(t->get_address())==0)
-               turnouts.insert(TurnoutMap::value_type(t->get_address(), t));
+       turnouts[t.get_address()]=&t;
 }
 
-void Control::add_locomotive(Locomotive *l)
+Turnout &Control::get_turnout(unsigned id) const
 {
-       if(find(locomotives.begin(), locomotives.end(), l)==locomotives.end())
-               locomotives.push_back(l);
+       map<unsigned, Turnout *>::const_iterator i=turnouts.find(id);
+       if(i==turnouts.end())
+               throw KeyError("Unknown turnout");
+
+       return *i->second;
 }
 
-void Control::add_sensor(Sensor *s)
+void Control::add_locomotive(Locomotive &l)
 {
-       if(sensors.count(s->get_address())==0)
-       {
-               sensors.insert(SensorMap::value_type(s->get_address(), s));
-               poll_sensors=true;
-       }
+       locomotives[l.get_address()]=&l;
+}
+
+Locomotive &Control::get_locomotive(unsigned id) const
+{
+       map<unsigned, Locomotive *>::const_iterator i=locomotives.find(id);
+       if(i==locomotives.end())
+               throw KeyError("Unknown locomotive");
+
+       return *i->second;
+}
+
+void Control::add_sensor(Sensor &s)
+{
+       sensors[s.get_address()]=&s;
+       poll_sensors=true;
+}
+
+Sensor &Control::get_sensor(unsigned id) const
+{
+       map<unsigned, Sensor *>::const_iterator i=sensors.find(id);
+       if(i==sensors.end())
+               throw KeyError("Unknown sensor");
+
+       return *i->second;
 }
 
 void Control::tick()
 {
        const Time::TimeStamp t=Time::now();
 
+       for(map<unsigned, Sensor *>::const_iterator i=sensors.begin(); i!=sensors.end(); ++i)
+               i->second->tick();
+
+       timer.tick(false);
+
        if(t>next_event_query)
        {
                next_event_query=t+300*Time::msec;
@@ -203,8 +204,19 @@ void Control::tick()
        }
 }
 
+Time::Timer::Slot &Control::set_timer(const Time::TimeDelta &dt)
+{
+       return timer.add(dt);
+}
+
 Control::~Control()
 {
+       for(map<unsigned, Sensor *>::iterator i=sensors.begin(); i!=sensors.end(); ++i)
+               delete i->second;
+       for(map<unsigned, Turnout *>::iterator i=turnouts.begin(); i!=turnouts.end(); ++i)
+               delete i->second;
+       for(map<unsigned, Locomotive *>::iterator i=locomotives.begin(); i!=locomotives.end(); ++i)
+               delete i->second;
        close(serial_fd);
 }
 
@@ -315,14 +327,14 @@ void Control::sensor_event_done(Error, const string &resp)
        for(unsigned i=0; resp[i]; i+=3)
        {
                unsigned module=(unsigned char)resp[i];
+
                cout<<"S88 module "<<module<<", status ";
                for(unsigned j=0; j<16; ++j)
-               {
-                       bool status=(resp[i+1+j/8]>>(7-j%8))&1;
-                       cout<<status;
-                       signal_sensor_event.emit(module*16+j-15, status);
-               }
+                       cout<<((resp[i+1+j/8]>>(7-j%8))&1);
                cout<<'\n';
+
+               for(unsigned j=0; j<16; ++j)
+                       signal_sensor_event.emit(module*16+j-15, (resp[i+1+j/8]>>(7-j%8))&1);
        }
 }