X-Git-Url: http://git.tdb.fi/?a=blobdiff_plain;f=source%2Flibmarklin%2Fcontrol.cpp;h=6c6ceb37da500e789d9a295ad68b1d0a53feb68f;hb=7839b7c3d782abb5c98a24d51cae109407068c02;hp=d371777fc5ca15d58478c001649732d038d33737;hpb=6c61179fe09af2f5366d50f10aadbf5f83438087;p=r2c2.git

diff --git a/source/libmarklin/control.cpp b/source/libmarklin/control.cpp
index d371777..6c6ceb3 100644
--- a/source/libmarklin/control.cpp
+++ b/source/libmarklin/control.cpp
@@ -1,12 +1,20 @@
+/* $Id$
+
+This file is part of the MSP Märklin suite
+Copyright © 2006-2008 Mikkosoft Productions, Mikko Rasa
+Distributed under the GPL
+*/
+
 #include <fcntl.h>
 #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"
 #include "control.h"
+#include "reply.h"
 
 using namespace std;
 using namespace Msp;
@@ -15,45 +23,36 @@ namespace Marklin {
 
 Control::Control():
 	serial_fd(-1),
-	p50_enabled(false),
 	power(true),
 	poll_sensors(false),
 	debug(false)
 { }
 
+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);
+}
+
 void Control::set_power(bool p)
 {
 	power=p;
 	if(power)
-		command(string(1, CMD_POWER_ON));
+		command(CMD_POWER_ON);
 	else
-		command(string(1, CMD_POWER_OFF));
-}
+		command(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;
+	signal_power_event.emit(power);
 }
 
-Locomotive *Control::get_locomotive(unsigned id) const
+void Control::set_debug(bool d)
 {
-	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;
+	debug=d;
 }
 
 void Control::open(const string &dev)
@@ -77,6 +76,7 @@ void Control::open(const string &dev)
 	attr.c_cflag|=CSTOPB;
 
 	bool ok=false;
+	bool p50=false;
 	for(unsigned i=0; baud[i]; i+=2)
 	{
 		cfsetospeed(&attr, baud[i+1]);
@@ -87,129 +87,140 @@ void Control::open(const string &dev)
 		pollfd pfd={serial_fd, POLLIN, 0};
 		if(poll(&pfd, 1, 500)>0)
 		{
-			cout<<"IB detected at "<<baud[i]<<" bits/s, P50 is ";
+			cout<<"IB detected at "<<baud[i]<<" bits/s\n";
 			char buf[2];
-			if(read(serial_fd, buf, 2)==2)
-			{
-				p50_enabled=true;
-				cout<<"enabled";
-			}
-			else
-			{
-				p50_enabled=false;
-				cout<<"disabled";
-			}
-			cout<<'\n';
+			p50=(read(serial_fd, buf, 2)==2);
 			ok=true;
 			break;
 		}
 	}
+
 	if(!ok)
 		throw Exception("IB not detected");
+	
+	if(p50)
+		write(serial_fd, "xZzA1\r", 6);
+
+	command(CMD_STATUS).signal_done.connect(sigc::mem_fun(this, &Control::status_done));
+}
+
+Command &Control::command(Cmd cmd)
+{
+	queue.push_back(Command(cmd, 0, 0));
+	return queue.back();
 }
 
-Command &Control::command(const string &cmd)
+Command &Control::command(Cmd cmd, unsigned char data)
 {
-	queue.push_back(Command(cmd));
+	queue.push_back(Command(cmd, &data, 1));
 	return queue.back();
 }
 
-void Control::add_turnout(Turnout *t)
+Command &Control::command(Cmd cmd, const unsigned char *data, unsigned len)
 {
-	if(turnouts.count(t->get_address())==0)
-		turnouts.insert(TurnoutMap::value_type(t->get_address(), t));
+	queue.push_back(Command(cmd, data, len));
+	return queue.back();
 }
 
-void Control::add_locomotive(Locomotive *l)
+void Control::add_turnout(Turnout &t)
 {
-	if(find(locomotives.begin(), locomotives.end(), l)==locomotives.end())
-		locomotives.push_back(l);
+	turnouts[t.get_address()]=&t;
 }
 
-void Control::add_sensor(Sensor *s)
+Turnout &Control::get_turnout(unsigned id) const
 {
-	if(sensors.count(s->get_address())==0)
-	{
-		sensors.insert(SensorMap::value_type(s->get_address(), s));
-		poll_sensors=true;
-	}
+	map<unsigned, Turnout *>::const_iterator i=turnouts.find(id);
+	if(i==turnouts.end())
+		throw KeyError("Unknown turnout");
+
+	return *i->second;
+}
+
+void Control::add_locomotive(Locomotive &l)
+{
+	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;
-		command(string(1, CMD_EVENT)).signal_done.connect(sigc::mem_fun(this, &Control::event_query_done));
+		next_event_query=t+200*Time::msec;
+		command(CMD_EVENT).signal_done.connect(sigc::mem_fun(this, &Control::event_query_done));
 	}
 
 	if(poll_sensors)
 	{
-		unsigned max_addr=(--sensors.end())->second->get_address();
-		string cmd(3, 0);
-		cmd[0]=CMD_SENSOR_PARAM_SET;
-		cmd[1]=0;
-		cmd[2]=(max_addr+7)/8;
-		command(cmd);
-		command(string(1, CMD_SENSOR_REPORT));
+		unsigned max_addr=(--sensors.end())->first;
+		unsigned char data[2];
+		data[0]=0;
+		data[1]=(max_addr+7)/8;
+		command(CMD_SENSOR_PARAM_SET, data, 2);
+		command(CMD_SENSOR_REPORT);
 		poll_sensors=false;
 	}
 
-	if(queue.size() && queue.front().get_sent())
+	if(!queue.empty() && queue.front().get_sent())
 	{
 		pollfd pfd={serial_fd, POLLIN, 0};
 		if(poll(&pfd, 1, 0)>0)
 		{
-			string resp=read_reply((Cmd)(unsigned char)queue.front().get_command()[0]);
+			Reply reply=Reply::read(serial_fd, queue.front().get_command());
 			if(debug)
-			{
-				printf("read:  ");
-				for(unsigned i=0; i<resp.size(); ++i)
-					printf("%02X ", (unsigned char)resp[i]);
-				printf("(%d bytes)\n", resp.size());
-			}
-
-			queue.front().signal_done.emit((Error)resp[0], resp.substr(1));
+				cout<<"R: "<<reply<<'\n';
+
+			queue.front().signal_done.emit(reply);
 			queue.erase(queue.begin());
 		}
 		else
 			return;
 	}
 
-	if(queue.size())
+	if(!queue.empty())
 	{
-		string cmd=queue.front().get_command();
-
-		if(p50_enabled)
-		{
-			if(cmd[0]&0x80)
-				cmd="X"+cmd;
-			else
-				cmd="x"+cmd;
-		}
-
 		if(debug)
-		{
-			printf("write: ");
-			for(unsigned i=0; i<cmd.size(); ++i)
-				printf("%02X ",(unsigned char)cmd[i]);
-			printf("(%d bytes)\n",cmd.size());
-		}
+			cout<<"W: "<<queue.front()<<'\n';
 
-		write(serial_fd, cmd.data(), cmd.size());
-		queue.front().set_sent(true);
+		queue.front().send(serial_fd);
 	}
 }
 
-Control::~Control()
+Time::Timer::Slot &Control::set_timer(const Time::TimeDelta &dt)
 {
-	close(serial_fd);
+	return timer.add(dt);
 }
 
-/*** private ***/
-
 void Control::read_all(int fd, char *buf, int size)
 {
 	int pos=0;
@@ -220,109 +231,52 @@ void Control::read_all(int fd, char *buf, int size)
 	}
 }
 
-string Control::read_reply(Cmd cmd)
+void Control::status_done(const Reply &reply)
 {
-	string result;
-	if(cmd==CMD_EVENT)
-	{
-		result+=ERR_NO_ERROR;
-		for(unsigned i=0; i<3; ++i)
-		{
-			char c;
-			read(serial_fd, &c, 1);
-			result+=c;
-			if(!(c&0x80)) break;
-		}
-	}
-	else if(cmd==CMD_EVENT_LOK)
-	{
-		result+=ERR_NO_ERROR;
-		char c[5];
-		read(serial_fd, c+4, 1);
-		result+=c[4];
-		while(c[4]&0x80)
-		{
-			read_all(serial_fd, c, 5);
-			result.append(c, 5);
-		}
-	}
-	else if(cmd==CMD_EVENT_TURNOUT)
-	{
-		result+=ERR_NO_ERROR;
-		char c[511];
-		read(serial_fd, c, 1);
-		read_all(serial_fd, c+1, c[0]*2);
-		result.append(c, c[0]*2+1);
-	}
-	else if(cmd==CMD_EVENT_SENSOR)
-	{
-		result+=ERR_NO_ERROR;
-		char c[3];
-		read(serial_fd, c+2, 1);
-		result+=c[2];
-		while(c[2])
-		{
-			read_all(serial_fd, c, 3);
-			result.append(c, 3);
-		}
-	}
-	else
-	{
-		unsigned expected_bytes=1;
-		if(cmd==CMD_FUNC_STATUS || cmd==CMD_TURNOUT_STATUS)
-			expected_bytes=2;
-		if(cmd==CMD_SENSOR_STATUS || cmd==CMD_TURNOUT_GROUP_STATUS)
-			expected_bytes=3;
-		if(cmd==CMD_LOK_STATUS)
-			expected_bytes=4;
-		if(cmd==CMD_LOK_CONFIG)
-			expected_bytes=5;
-		char c[5];
-		read_all(serial_fd, c, 1);
-		result+=c[0];
-		if(!c[0])
-		{
-			read_all(serial_fd, c+1, expected_bytes-1);
-			result.append(c+1, expected_bytes-1);
-		}
-	}
-
-	return result;
+	power=((reply.get_data()[0]&0x08)!=0);
+	signal_power_event.emit(power);
 }
 
-void Control::event_query_done(Error, const string &resp)
+void Control::event_query_done(const Reply &reply)
 {
-	if(resp[0]&0x20)
-		command(string(1, CMD_EVENT_TURNOUT)).signal_done.connect(sigc::mem_fun(this, &Control::turnout_event_done));
-	if(resp[0]&0x04)
-		command(string(1, CMD_EVENT_SENSOR)).signal_done.connect(sigc::mem_fun(this, &Control::sensor_event_done));
+	const unsigned char *data=reply.get_data();
+	if(data[0]&0x01)
+		command(CMD_EVENT_LOK);
+	if(data[0]&0x20)
+		command(CMD_EVENT_TURNOUT).signal_done.connect(sigc::mem_fun(this, &Control::turnout_event_done));
+	if(data[0]&0x04)
+		command(CMD_EVENT_SENSOR).signal_done.connect(sigc::mem_fun(this, &Control::sensor_event_done));
+	if(data[1]&0x40)
+		command(CMD_STATUS).signal_done.connect(sigc::mem_fun(this, &Control::status_done));
 }
 
-void Control::turnout_event_done(Error, const string &resp)
+void Control::turnout_event_done(const Reply &reply)
 {
-	unsigned count=resp[0];
+	const unsigned char *data=reply.get_data();
+	unsigned count=data[0];
 	for(unsigned i=0; i<count; ++i)
 	{
-		unsigned addr=(unsigned char)resp[i*2+1]+((resp[i*2+2]&7)<<8);
-		bool status=!(resp[i*2+2]&0x80);
+		unsigned addr=(data[i*2+1])+((data[i*2+2]&7)<<8);
+		bool status=!(data[i*2+2]&0x80);
 		cout<<"Turnout "<<addr<<", status "<<status<<'\n';
 		signal_turnout_event.emit(addr, status);
 	}
 }
 
-void Control::sensor_event_done(Error, const string &resp)
+void Control::sensor_event_done(const Reply &reply)
 {
-	for(unsigned i=0; resp[i]; i+=3)
+	const unsigned char *data=reply.get_data();
+	for(unsigned i=0; data[i]; i+=3)
 	{
-		unsigned module=(unsigned char)resp[i];
+		unsigned module=data[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<<((data[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, (data[i+1+j/8]>>(7-j%8))&1);
 	}
 }