]> git.tdb.fi Git - model-railway-devices.git/blob - arducontrol/motorola.c
Make upload device configurable
[model-railway-devices.git] / arducontrol / motorola.c
1 #include "commands.h"
2 #include "motorola.h"
3 #include "output.h"
4
5 static uint8_t motorola_speed_to_value(uint8_t speed)
6 {
7         if(speed>14)
8                 return 15;
9         else if(speed)
10                 return speed+1;
11         else
12                 return 0;
13 }
14
15 static void motorola_common_packet(uint8_t addr, uint8_t aux, uint8_t value)
16 {
17         uint8_t i;
18
19         clear_packet();
20
21         packet.bit_duration = 2;
22         packet.length = 18*8;
23
24         for(i=0; i<4; ++i)
25         {
26                 uint8_t d = addr%3;
27                 addr /= 3;
28
29                 packet.data[i*2] = (d==0 ? 0x01 : 0x7F);
30                 packet.data[i*2+1] = (d==1 ? 0x7F : 0x01);
31         }
32
33         packet.data[8] = (aux ? 0x7F : 0x01);
34         packet.data[9] = packet.data[8];
35
36         for(i=0; i<4; ++i)
37         {
38                 packet.data[10+i*2] = ((value&1) ? 0x7F : 0x01);
39                 value >>= 1;
40         }
41
42         packet.repeat_count = 2;
43         // Duration of three trits
44         packet.repeat_delay = 96;
45         packet.final_delay = 224;
46 }
47
48 static void motorola_old_packet(uint8_t addr, uint8_t aux, uint8_t value)
49 {
50         uint8_t i;
51
52         motorola_common_packet(addr, aux, value);
53
54         for(i=0; i<4; ++i)
55                 packet.data[11+i*2] = packet.data[10+i*2];
56 }
57
58 void motorola_locomotive_speed_packet(uint8_t addr, uint8_t aux, uint8_t speed)
59 {
60         motorola_old_packet(addr, aux, motorola_speed_to_value(speed));
61
62         packet.ready = 1;
63 }
64
65 void motorola_locomotive_reverse_packet(uint8_t addr, uint8_t aux)
66 {
67         motorola_old_packet(addr, aux, 1);
68
69         packet.ready = 1;
70 }
71
72 void motorola_locomotive_speed_direction_packet(uint8_t addr, uint8_t aux, uint8_t speed, uint8_t dir)
73 {
74         motorola_common_packet(addr, aux, motorola_speed_to_value(speed));
75
76         packet.data[11] = (dir ? 0x01 : 0x7F);
77         packet.data[13] = (dir ? 0x7F : 0x01);
78         packet.data[15] = (dir ? 0x01 : 0x7F);
79         packet.data[17] = 0x80-packet.data[16];
80
81         packet.ready = 1;
82 }
83
84 void motorola_locomotive_speed_function_packet(uint8_t addr, uint8_t aux, uint8_t speed, uint8_t func, uint8_t state)
85 {
86         uint8_t i;
87         uint8_t value;
88
89         value = motorola_speed_to_value(speed);
90         motorola_common_packet(addr, aux, value);
91
92         /*
93         001 -> 011
94         010 -> 100
95         011 -> 110
96         100 -> 111
97         */
98         func += 2;
99         if(func>=5)
100                 ++func;
101         if(state)
102                 func |= 8;
103         if(func==value)
104                 func = ((value&8) ? 2 : 5) | (func&8);
105
106         for(i=0; i<4; ++i)
107         {
108                 packet.data[11+i*2] = ((func&1) ? 0x7F : 0x01);
109                 func >>= 1;
110         }
111
112         packet.ready = 1;
113 }
114
115 void motorola_solenoid_packet(uint8_t addr, uint8_t output, uint8_t state)
116 {
117         uint8_t value = output;
118         if(state)
119                 value |= 8;
120
121         motorola_old_packet(addr, 0, value);
122         packet.repeat_delay >>= 1;
123         packet.bit_duration = 1;
124
125         packet.ready = 1;
126 }
127
128 uint8_t motorola_command(const uint8_t *cmd_buf, uint8_t cmd_length)
129 {
130         if(cmd_buf[0]==MOTOROLA_SPEED || cmd_buf[0]==MOTOROLA_SPEED_DIRECTION || cmd_buf[0]==MOTOROLA_SPEED_FUNCTION)
131         {
132                 if(cmd_length!=4)
133                         return LENGTH_ERROR;
134
135                 uint8_t addr = cmd_buf[1];
136                 if(addr>80)
137                         return INVALID_VALUE;
138
139                 if(cmd_buf[2]&0x0C)
140                         return INVALID_VALUE;
141                 uint8_t aux = cmd_buf[2]&0x01;
142
143                 uint8_t func = (cmd_buf[2]&0xF0)>>4;
144                 if(cmd_buf[0]==MOTOROLA_SPEED_FUNCTION)
145                 {
146                         if(func<1 || func>4)
147                                 return INVALID_VALUE;
148                 }
149                 else if(cmd_buf[2]&0xFE)
150                         return INVALID_VALUE;
151                 uint8_t state = cmd_buf[2]&0x02;
152
153                 uint8_t speed = cmd_buf[3]&0x7F;
154                 if(speed>14)
155                         return INVALID_VALUE;
156
157                 uint8_t dir = !(cmd_buf[3]&0x80);
158
159                 while(packet.ready && !packet.done) ;
160
161                 if(cmd_buf[0]==MOTOROLA_SPEED)
162                         motorola_locomotive_speed_packet(addr, aux, speed);
163                 else if(cmd_buf[0]==MOTOROLA_SPEED_DIRECTION)
164                         motorola_locomotive_speed_direction_packet(addr, aux, speed, dir);
165                 else if(cmd_buf[0]==MOTOROLA_SPEED_FUNCTION)
166                         motorola_locomotive_speed_function_packet(addr, aux, speed, func, state);
167         }
168         else if(cmd_buf[0]==MOTOROLA_REVERSE)
169         {
170                 if(cmd_length!=3)
171                         return LENGTH_ERROR;
172
173                 uint8_t addr = cmd_buf[1];
174                 if(addr>80)
175                         return INVALID_VALUE;
176
177                 if(cmd_buf[2]&0xFE)
178                         return INVALID_VALUE;
179                 uint8_t aux = cmd_buf[2]&0x01;
180
181                 while(packet.ready && !packet.done) ;
182
183                 motorola_locomotive_reverse_packet(addr, aux);
184         }
185         else if(cmd_buf[0]==MOTOROLA_SOLENOID)
186         {
187                 if(cmd_length!=3)
188                         return LENGTH_ERROR;
189
190                 uint8_t addr = cmd_buf[1];
191                 if(addr>80)
192                         return INVALID_VALUE;
193
194                 if(cmd_buf[2]&0x8E)
195                         return INVALID_VALUE;
196                 uint8_t output = (cmd_buf[2]&0x70)>>4;
197                 uint8_t state = cmd_buf[2]&1;
198
199                 while(packet.ready && !packet.done) ;
200
201                 motorola_solenoid_packet(addr, output, state);
202         }
203         else
204                 return INVALID_COMMAND;
205
206         return COMMAND_OK;
207 }