-
Notifications
You must be signed in to change notification settings - Fork 4
/
RxCommandParser.cpp
231 lines (193 loc) · 5.75 KB
/
RxCommandParser.cpp
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
#include "mbed.h"
#include "RxCommandParser.hpp"
#include "ROBOT_CONFIG.hpp"
extern void u_printf(const char *fmt, ...); // Defined in main()
RxCommandParser::RxCommandParser(EthernetInterface *net) {
auto_ch1 = 1024;
auto_ch2 = 1024;
_rx_sock = new UDPSocket();
_rx_sock->open(net);
_rx_sock->bind(12346);
#ifdef USER_DIGITAL_OUT_0
user_dout_0 = new DigitalOut(USER_DIGITAL_OUT_0);
*user_dout_0 = 0;
#endif // USER_DIGITAL_OUT_0
#ifdef USER_PWM_OUT_0
user_pout_0 = new PwmOut(USER_PWM_OUT_0);
user_pout_0->period(0.02);
user_pout_0->write(0.0);
#endif // USER_PWM_OUT_0
#ifdef USER_PWM_OUT_1
user_pout_1 = new PwmOut(USER_PWM_OUT_1);
user_pout_1->period(0.02);
user_pout_1->write(0.0);
#endif // USER_PWM_OUT_1
#ifdef USER_PWM_OUT_2
user_pout_2 = new PwmOut(USER_PWM_OUT_2);
user_pout_2->period(0.02);
user_pout_2->write(0.0);
#endif // USER_PWM_OUT_2
#ifdef USER_PWM_OUT_3
user_pout_3 = new PwmOut(USER_PWM_OUT_3);
user_pout_3->period(0.02);
user_pout_3->write(0.0);
#endif // USER_PWM_OUT_3
#ifdef USER_SERVO_OUT_0 // assume a Futaba-style pulse-width in micro-seconds
user_servo_0 = new PwmOut(USER_SERVO_OUT_0);
user_servo_0->period_ms(15); // same period as Futaba S.Bus
user_servo_0->pulsewidth_us(0);
#endif // USER_SERVO_OUT_0
#ifdef USER_SERVO_OUT_1 // assume a Futaba-style pulse-width in micro-seconds
user_servo_1 = new PwmOut(USER_SERVO_OUT_1);
user_servo_1->period_ms(15); // same period as Futaba S.Bus
user_servo_1->pulsewidth_us(0);
#endif // USER_SERVO_OUT_1
#ifdef USER_SERVO_OUT_2 // assume a Futaba-style pulse-width in micro-seconds
user_servo_2 = new PwmOut(USER_SERVO_OUT_2);
user_servo_2->period_ms(15); // same period as Futaba S.Bus
user_servo_2->pulsewidth_us(0);
#endif // USER_SERVO_OUT_2
#ifdef USER_SERVO_OUT_3 // assume a Futaba-style pulse-width in micro-seconds
user_servo_3 = new PwmOut(USER_SERVO_OUT_3);
user_servo_3->period_ms(15); // same period as Futaba S.Bus
user_servo_3->pulsewidth_us(0);
#endif // USER_SERVO_OUT_3
}
void RxCommandParser::Start() {
main_thread.start(callback(this, &RxCommandParser::main_worker));
}
void RxCommandParser::setRelay(bool value) {
#ifdef USER_DIGITAL_OUT_0
if (value) {
*user_dout_0 = 1;
} else {
*user_dout_0 = 0;
}
#endif // USER_DIGITAL_OUT_0
}
void RxCommandParser::main_worker() {
/*
* Here we receive throttle and steering control from the auto-pilot computer
*/
SocketAddress sockAddr;
char inputBuffer[65];
memset(inputBuffer, 0, 65);
uint64_t _last_autopilot = 0;
uint16_t *control = (uint16_t *) &(inputBuffer[0]);
_rx_sock->set_blocking(true);
_rx_sock->set_timeout(500);
while (true) {
int n = _rx_sock->recvfrom(&sockAddr, inputBuffer, 64);
inputBuffer[n] = 0;
uint64_t ts = rtos::Kernel::get_ms_count();
if (ts - _last_autopilot > 500) {
if (auto_ch1 != 1024 || auto_ch2 != 1024) {
u_printf("Timeout: resetting auto sbus values\n");
auto_ch1 = 1024;
auto_ch2 = 1024;
}
}
if (n == 2*sizeof(uint16_t)) {
_last_autopilot = ts;
auto_ch1 = control[0];
auto_ch2 = control[1];
} else if (n > 4) {
if (strncmp(inputBuffer, "set ", 4) == 0) {
if (n > 9 && strncmp(&(inputBuffer[4]), "relay", 5) == 0) {
#ifdef USER_DIGITAL_OUT_0
// ascii "1" is 49
if (inputBuffer[9] == 49) {
*user_dout_0 = 1;
} else {
*user_dout_0 = 0;
}
#endif // USER_DIGITAL_OUT_0
} else if (n > 8 && strncmp(&(inputBuffer[4]), "PWM", 3) == 0) {
u_printf(inputBuffer);
switch (inputBuffer[7]) {
case '0':
#ifdef USER_PWM_OUT_0
float val = (float) atoi(&(inputBuffer[8])) / 255.0;
if (val < 0.0) {
val = 0.0;
} else if (val > 1.0) {
val = 1.0;
}
user_pout_0->write(val);
u_printf("--- set PWM0: %f\n", val);
#endif // USER_PWM_OUT_0
break;
}
} else if (n >= 11 && strncmp(&(inputBuffer[4]), "servo", 3) == 0) {
u_printf(inputBuffer);
int val = 0;
switch (inputBuffer[9]) {
case '0':
#ifdef USER_SERVO_OUT_0
val = atoi(&(inputBuffer[10]));
// Futaba servo PWs are usually about 300 to 1700 us, but we
// will allow anything that is physically possible
if (val < 0) {
val = 0;
} else if (val > 15000) {
val = 15000;
}
user_servo_0->pulsewidth_us(val);
u_printf("--- set servo0: %d\n", val);
#endif // USER_SERVO_OUT_0
break;
case '1':
#ifdef USER_SERVO_OUT_1
val = atoi(&(inputBuffer[10]));
// Futaba servo PWs are usually about 300 to 1700 us, but we
// will allow anything that is physically possible
if (val < 0) {
val = 0;
} else if (val > 15000) {
val = 15000;
}
user_servo_1->pulsewidth_us(val);
u_printf("--- set servo1: %d\n", val);
#endif // USER_SERVO_OUT_1
break;
case '2':
#ifdef USER_SERVO_OUT_2
val = atoi(&(inputBuffer[10]));
// Futaba servo PWs are usually about 300 to 1700 us, but we
// will allow anything that is physically possible
if (val < 0) {
val = 0;
} else if (val > 15000) {
val = 15000;
}
user_servo_2->pulsewidth_us(val);
u_printf("--- set servo2: %d\n", val);
#endif // USER_SERVO_OUT_2
break;
case '3':
#ifdef USER_SERVO_OUT_3
val = atoi(&(inputBuffer[10]));
// Futaba servo PWs are usually about 300 to 1700 us, but we
// will allow anything that is physically possible
if (val < 0) {
val = 0;
} else if (val > 15000) {
val = 15000;
}
user_servo_3->pulsewidth_us(val);
u_printf("--- set servo3: %d\n", val);
#endif // USER_SERVO_OUT_3
break;
}
}
}
} else if (n > 0) {
inputBuffer[n] = 0;
printf("rx %d bytes:\r\n", n);
printf(inputBuffer);
printf("\r\n");
} else {
//printf("empty packet\r\n");
}
}
}