Add railway control firmware

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

diff --git a/arducontrol/motorola.c b/arducontrol/motorola.c
new file mode 100644 (file)
index 0000000..e78ebfc
--- /dev/null
+++ b/arducontrol/motorola.c
@@ -0,0 +1,124 @@
+#include "packet.h"
+
+static uint8_t motorola_speed_to_value(uint8_t speed)
+{
+       if(speed>14)
+               return 15;
+       else if(speed)
+               return speed+1;
+       else
+               return 0;
+}
+
+static void motorola_common_packet(uint8_t addr, uint8_t aux, uint8_t value)
+{
+       uint8_t i;
+
+       clear_packet();
+
+       packet.bit_duration = 2;
+       packet.length = 18*8;
+
+       for(i=0; i<4; ++i)
+       {
+               uint8_t d = addr%3;
+               addr /= 3;
+
+               packet.data[i*2] = (d==0 ? 0x01 : 0x7F);
+               packet.data[i*2+1] = (d==1 ? 0x7F : 0x01);
+       }
+
+       packet.data[8] = (aux ? 0x7F : 0x01);
+       packet.data[9] = packet.data[8];
+
+       for(i=0; i<4; ++i)
+       {
+               packet.data[10+i*2] = ((value&1) ? 0x7F : 0x01);
+               value >>= 1;
+       }
+
+       packet.repeat_count = 2;
+       // Duration of three trits
+       packet.repeat_delay = 96;
+       packet.final_delay = 224;
+}
+
+static void motorola_old_packet(uint8_t addr, uint8_t aux, uint8_t value)
+{
+       uint8_t i;
+
+       motorola_common_packet(addr, aux, value);
+
+       for(i=0; i<4; ++i)
+               packet.data[11+i*2] = packet.data[10+i*2];
+}
+
+void motorola_locomotive_speed_packet(uint8_t addr, uint8_t aux, uint8_t speed)
+{
+       motorola_old_packet(addr, aux, motorola_speed_to_value(speed));
+
+       packet.ready = 1;
+}
+
+void motorola_locomotive_reverse_packet(uint8_t addr, uint8_t aux)
+{
+       motorola_old_packet(addr, aux, 1);
+
+       packet.ready = 1;
+}
+
+void motorola_locomotive_speed_direction_packet(uint8_t addr, uint8_t aux, uint8_t speed, uint8_t dir)
+{
+       motorola_common_packet(addr, aux, motorola_speed_to_value(speed));
+
+       packet.data[11] = (dir ? 0x01 : 0x7F);
+       packet.data[13] = (dir ? 0x7F : 0x01);
+       packet.data[15] = (dir ? 0x01 : 0x7F);
+       packet.data[17] = 0x80-packet.data[16];
+
+       packet.ready = 1;
+}
+
+void motorola_locomotive_speed_function_packet(uint8_t addr, uint8_t aux, uint8_t speed, uint8_t func, uint8_t state)
+{
+       uint8_t i;
+       uint8_t value;
+
+       value = motorola_speed_to_value(speed);
+       motorola_common_packet(addr, aux, value);
+
+       /*
+       001 -> 011
+       010 -> 100
+       011 -> 110
+       100 -> 111
+       */
+       func += 2;
+       if(func>=5)
+               ++func;
+       if(state)
+               func |= 8;
+       if(func==value)
+               func = ((value&8) ? 2 : 5) | (func&8);
+
+       for(i=0; i<4; ++i)
+       {
+               packet.data[11+i*2] = ((func&1) ? 0x7F : 0x01);
+               func >>= 1;
+       }
+
+       packet.ready = 1;
+}
+
+void motorola_solenoid_packet(uint8_t addr, uint8_t output, uint8_t state)
+{
+       uint8_t value = output;
+       if(state)
+               value |= 8;
+
+       motorola_old_packet(addr, 0, value);
+       packet.repeat_delay >>= 1;
+       packet.bit_duration = 1;
+
+       packet.ready = 1;
+}