X-Git-Url: http://git.tdb.fi/?p=model-railway-devices.git;a=blobdiff_plain;f=arducontrol%2Fmotorola.c;h=5bf15ad9e37ed8422cd93f063d128d34c0beada9;hp=e78ebfc34bbe6a0b1de6a939452aca3d96577f66;hb=b47b38e9dd282c5f8fb1ded25ade805e64d0cf1a;hpb=817742862a867673e4cbdb85d274481f7482e1ae

diff --git a/arducontrol/motorola.c b/arducontrol/motorola.c
index e78ebfc..5bf15ad 100644
--- a/arducontrol/motorola.c
+++ b/arducontrol/motorola.c
@@ -1,4 +1,6 @@
-#include "packet.h"
+#include "interface.h"
+#include "motorola.h"
+#include "output.h"
 
 static uint8_t motorola_speed_to_value(uint8_t speed)
 {
@@ -122,3 +124,84 @@ void motorola_solenoid_packet(uint8_t addr, uint8_t output, uint8_t state)
 
 	packet.ready = 1;
 }
+
+uint8_t motorola_command()
+{
+	if(cmd_buf[0]==MOTOROLA_SPEED || cmd_buf[0]==MOTOROLA_SPEED_DIRECTION || cmd_buf[0]==MOTOROLA_SPEED_FUNCTION)
+	{
+		if(cmd_length!=4)
+			return LENGTH_ERROR;
+
+		uint8_t addr = cmd_buf[1];
+		if(addr>80)
+			return INVALID_VALUE;
+
+		if(cmd_buf[2]&0x0E)
+			return INVALID_VALUE;
+		uint8_t aux = cmd_buf[2]&0x01;
+
+		uint8_t func = (cmd_buf[2]&0xF0)>>4;
+		if(cmd_buf[0]==MOTOROLA_SPEED_FUNCTION)
+		{
+			if(func<1 || func>4)
+				return INVALID_VALUE;
+		}
+		else if(cmd_buf[2]&0xFE)
+			return INVALID_VALUE;
+		uint8_t state = cmd_buf[2]&0x02;
+
+		uint8_t speed = cmd_buf[3]&0x7F;
+		if(speed>14)
+			return INVALID_VALUE;
+
+		uint8_t dir = !(cmd_buf[3]&0x80);
+
+		while(packet.ready && !packet.done) ;
+
+		if(cmd_buf[0]==MOTOROLA_SPEED)
+			motorola_locomotive_speed_packet(addr, aux, speed);
+		else if(cmd_buf[0]==MOTOROLA_SPEED_DIRECTION)
+			motorola_locomotive_speed_direction_packet(addr, aux, speed, dir);
+		else if(cmd_buf[0]==MOTOROLA_SPEED_FUNCTION)
+			motorola_locomotive_speed_function_packet(addr, aux, speed, func, state);
+	}
+	else if(cmd_buf[0]==MOTOROLA_REVERSE)
+	{
+		if(cmd_length!=3)
+			return LENGTH_ERROR;
+
+		uint8_t addr = cmd_buf[1];
+		if(addr>80)
+			return INVALID_VALUE;
+
+		if(cmd_buf[2]&0xFE)
+			return INVALID_VALUE;
+		uint8_t aux = cmd_buf[2]&0x01;
+
+		while(packet.ready && !packet.done) ;
+
+		motorola_locomotive_reverse_packet(addr, aux);
+	}
+	else if(cmd_buf[0]==MOTOROLA_SOLENOID)
+	{
+		if(cmd_length!=3)
+			return LENGTH_ERROR;
+
+		uint8_t addr = cmd_buf[1];
+		if(addr>80)
+			return INVALID_VALUE;
+
+		if(cmd_buf[2]&0x8E)
+			return INVALID_VALUE;
+		uint8_t output = (cmd_buf[2]&0x70)>>4;
+		uint8_t state = cmd_buf[2]&1;
+
+		while(packet.ready && !packet.done) ;
+
+		motorola_solenoid_packet(addr, output, state);
+	}
+	else
+		return INVALID_COMMAND;
+
+	return COMMAND_OK;
+}