X-Git-Url: http://git.tdb.fi/?a=blobdiff_plain;f=source%2Flibmarklin%2Flocomotive.cpp;fp=source%2Flibmarklin%2Flocomotive.cpp;h=ecacea19a833af43f918823523951740d9a0e69c;hb=6c61179fe09af2f5366d50f10aadbf5f83438087;hp=0000000000000000000000000000000000000000;hpb=6028010256e4ce2c4fdd355e91e0cfef1573de64;p=r2c2.git

diff --git a/source/libmarklin/locomotive.cpp b/source/libmarklin/locomotive.cpp
new file mode 100644
index 0000000..ecacea1
--- /dev/null
+++ b/source/libmarklin/locomotive.cpp
@@ -0,0 +1,116 @@
+#include <msp/time/timer.h>
+#include <msp/time/units.h>
+#include "command.h"
+#include "constants.h"
+#include "control.h"
+#include "locomotive.h"
+
+using namespace std;
+using namespace Msp;
+
+namespace Marklin {
+
+Locomotive::Locomotive(Control &c, unsigned a):
+	control(c),
+	addr(a),
+	speed(0),
+	reverse(false),
+	funcs(0)
+{
+	control.add_locomotive(this);
+
+	refresh_status();
+}
+
+void Locomotive::set_speed(unsigned spd)
+{
+	speed=min(spd, 14U);
+
+	send_command(false);
+}
+
+void Locomotive::set_reverse(bool rev)
+{
+	if(rev==reverse)
+		return;
+
+	if(speed)
+	{
+		(new Time::Timer((500+speed*150)*Time::msec))->signal_timeout.connect(sigc::mem_fun(this, &Locomotive::reverse_timeout));
+		set_speed(0);
+	}
+	else
+	{
+		reverse=rev;
+		send_command(false);
+	}
+}
+
+void Locomotive::set_function(unsigned func, bool state)
+{
+	if(state)
+		funcs|=1<<func;
+	else
+		funcs&=~(1<<func);
+
+	send_command(true);
+}
+
+void Locomotive::refresh_status()
+{
+	char cmd[3];
+	cmd[0]=CMD_LOK_STATUS;
+	cmd[1]=addr&0xFF;
+	cmd[2]=(addr>>8)&0xFF;
+	control.command(string(cmd,3)).signal_done.connect(sigc::mem_fun(this,&Locomotive::status_reply));
+}
+
+void Locomotive::send_command(bool setf)
+{
+	char cmd[16];
+	cmd[0]=CMD_LOK;
+	cmd[1]=addr&0xFF;
+	cmd[2]=(addr>>8)&0xFF;
+
+	if(speed==0)
+		cmd[3]=0;
+	else if(speed==1)
+		cmd[3]=2;
+	else
+		cmd[3]=(speed*19-18)/2;
+	
+	cmd[4]=(reverse?0:0x20) | ((funcs&1)?0x10:0);
+
+	if(setf)
+	{
+		cmd[4]|=0x80;
+		for(unsigned i=0; i<4; ++i)
+			if((funcs>>i)&2)
+				cmd[4]|=(1<<i);
+	}
+	control.command(string(cmd,5));
+}
+
+void Locomotive::status_reply(Error err, const string &reply)
+{
+	if(err==ERR_NO_ERROR)
+	{
+		if((unsigned char)reply[0]<=1)
+			speed=0;
+		else
+			speed=(unsigned char)reply[0]*2/19+1;
+		reverse=(reply[1]&0x20)?false:true;
+		funcs=(reply[1]&0xF)<<1;
+		if(reply[1]&0x10)
+			funcs|=1;
+	}
+}
+
+bool Locomotive::reverse_timeout()
+{
+	reverse=!reverse;
+	send_command(true);
+	return false;
+}
+
+} // namespace Marklin