Organize arducontrol code by functionality

[model-railway-devices.git] / arducontrol / motorola.c

diff --git a/arducontrol/motorola.c b/arducontrol/motorola.c
index e78ebfc34bbe6a0b1de6a939452aca3d96577f66..5bf15ad9e37ed8422cd93f063d128d34c0beada9 100644 (file)
--- 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;
+}