PX4 Firmware
PX4 Autopilot Software http://px4.io
RCTest.cpp
Go to the documentation of this file.
1 #include <unit_test.h>
2 
3 #include <systemlib/err.h>
4 
5 #include <stdio.h>
6 #include <string.h>
7 #include <unistd.h>
8 
9 #include <drivers/drv_hrt.h>
10 
11 #define DSM_DEBUG
12 #include <lib/rc/sbus.h>
13 #include <lib/rc/dsm.h>
14 #include <lib/rc/st24.h>
15 #include <lib/rc/sumd.h>
16 #include <lib/rc/crsf.h>
17 
18 #if defined(CONFIG_ARCH_BOARD_PX4_SITL)
19 #define TEST_DATA_PATH "./test_data/"
20 #else
21 #define TEST_DATA_PATH "/fs/microsd"
22 #endif
23 
24 extern "C" __EXPORT int rc_tests_main(int argc, char *argv[]);
25 
26 class RCTest : public UnitTest
27 {
28 public:
29  bool run_tests() override;
30 
31 private:
32  bool crsfTest();
33  bool dsmTest(const char *filepath, unsigned expected_chancount, unsigned expected_dropcount, unsigned chan0);
34  bool dsmTest10Ch();
35  bool dsmTest12Ch();
36  bool sbus2Test();
37  bool st24Test();
38  bool sumdTest();
39 };
40 
42 {
49 
50  return (_tests_failed == 0);
51 }
52 
54 {
55  const char *filepath = TEST_DATA_PATH "crsf_rc_channels.txt";
56 
57  FILE *fp = fopen(filepath, "rt");
58 
59  ut_test(fp);
60  //PX4_INFO("loading data from: %s", filepath);
61 
62  const int line_size = 500;
63  char line[line_size];
64  bool has_decoded_values = false;
65  const int max_channels = 16;
66  uint16_t rc_values[max_channels];
67  uint16_t num_values = 0;
68  int line_counter = 1;
69 
70  while (fgets(line, line_size, fp) != nullptr) {
71 
72  if (strncmp(line, "INPUT ", 6) == 0) {
73 
74  if (has_decoded_values) {
75  PX4_ERR("Parser decoded values that are not in the test file (line=%i)", line_counter);
76  return false;
77  }
78 
79  // read the values
80  const char *file_buffer = line + 6;
81  int frame_len = 0;
82  uint8_t frame[300];
83  int offset;
84  int number;
85 
86  while (sscanf(file_buffer, "%x, %n", &number, &offset) > 0) {
87  frame[frame_len++] = number;
88  file_buffer += offset;
89  }
90 
91  // Pipe the data into the parser
93 
94  bool result = crsf_parse(now, frame, frame_len, rc_values, &num_values, max_channels);
95 
96  if (result) {
97  has_decoded_values = true;
98  }
99 
100  } else if (strncmp(line, "DECODED ", 8) == 0) {
101 
102  if (!has_decoded_values) {
103  PX4_ERR("Test file contains decoded values but the parser did not decode anything (line=%i)", line_counter);
104  return false;
105  }
106 
107  // read the values
108  const char *file_buffer = line + 8;
109  int offset;
110  int expected_rc_value;
111  int expected_num_channels = 0;
112 
113  while (sscanf(file_buffer, "%x, %n", &expected_rc_value, &offset) > 0) {
114 
115  // allow a small difference
116  if (abs(expected_rc_value - (int)rc_values[expected_num_channels]) > 10) {
117  PX4_ERR("File line: %i, channel: %i", line_counter, expected_num_channels);
118  ut_compare("Wrong decoded channel", expected_rc_value, rc_values[expected_num_channels]);
119  }
120 
121  file_buffer += offset;
122  ++expected_num_channels;
123  }
124 
125  if (expected_num_channels != num_values) {
126  PX4_ERR("File line: %d", line_counter);
127  ut_compare("Unexpected number of decoded channels", expected_num_channels, num_values);
128  }
129 
130  has_decoded_values = false;
131  }
132 
133  ++line_counter;
134  }
135 
136  return true;
137 }
138 
140 {
141  return dsmTest(TEST_DATA_PATH "dsm_x_data.txt", 10, 6, 1500);
142 }
143 
145 {
146  return dsmTest(TEST_DATA_PATH "dsm_x_dx9_data.txt", 12, 6, 1500);
147 }
148 
149 bool RCTest::dsmTest(const char *filepath, unsigned expected_chancount, unsigned expected_dropcount, unsigned chan0)
150 {
151 
152  FILE *fp;
153  fp = fopen(filepath, "rt");
154 
155  ut_test(fp);
156  //PX4_INFO("loading data from: %s", filepath);
157 
158  float f;
159  unsigned x;
160  int ret;
161 
162  // Trash the first 20 lines
163  for (unsigned i = 0; i < 20; i++) {
164  char buf[200];
165  (void)fgets(buf, sizeof(buf), fp);
166  }
167 
168  // Init the parser
169  uint8_t frame[30];
170  uint16_t rc_values[18];
171  uint16_t num_values;
172  bool dsm_11_bit;
173  unsigned dsm_frame_drops = 0;
174  uint16_t max_channels = sizeof(rc_values) / sizeof(rc_values[0]);
175 
176  int rate_limiter = 0;
177  unsigned last_drop = 0;
178 
179  dsm_proto_init();
180 
181  while (EOF != (ret = fscanf(fp, "%f,%x,,", &f, &x))) {
182 
183  if (ret <= 0) {
184  fclose(fp);
185  ut_test(ret > 0);
186  }
187 
188  frame[0] = x;
189  unsigned len = 1;
190 
191  // Pipe the data into the parser
192  bool result = dsm_parse(f * 1e6f, &frame[0], len, rc_values, &num_values,
193  &dsm_11_bit, &dsm_frame_drops, nullptr, max_channels);
194 
195  if (result) {
196  ut_compare("num_values == expected_chancount", num_values, expected_chancount);
197 
198  ut_test(abs((int)chan0 - (int)rc_values[0]) < 30);
199 
200  //PX4_INFO("decoded packet with %d channels and %s encoding:", num_values, (dsm_11_bit) ? "11 bit" : "10 bit");
201 
202  for (unsigned i = 0; i < num_values; i++) {
203  //PX4_INFO("chan #%u:\t%d", i, (int)rc_values[i]);
204  }
205  }
206 
207  if (last_drop != (dsm_frame_drops)) {
208  PX4_INFO("frame dropped, now #%d", (dsm_frame_drops));
209  last_drop = dsm_frame_drops;
210  }
211 
212  rate_limiter++;
213  }
214 
215  fclose(fp);
216 
217  ut_test(ret == EOF);
218  PX4_INFO("drop: %d", (int)last_drop);
219  ut_test(last_drop == expected_dropcount);
220 
221  return true;
222 }
223 
225 {
226  const char *filepath = TEST_DATA_PATH "sbus2_r7008SB.txt";
227 
228  FILE *fp;
229  fp = fopen(filepath, "rt");
230 
231  ut_test(fp);
232  //PX4_INFO("loading data from: %s", filepath);
233 
234  float f;
235  unsigned x;
236  int ret;
237 
238  // Trash the first 20 lines
239  for (unsigned i = 0; i < 20; i++) {
240  char buf[200];
241  (void)fgets(buf, sizeof(buf), fp);
242  }
243 
244  // Init the parser
245  uint8_t frame[SBUS_BUFFER_SIZE];
246  uint16_t rc_values[18];
247  uint16_t num_values;
248  unsigned sbus_frame_drops = 0;
249  unsigned sbus_frame_resets = 0;
250  bool sbus_failsafe;
251  bool sbus_frame_drop;
252  uint16_t max_channels = sizeof(rc_values) / sizeof(rc_values[0]);
253 
254  int rate_limiter = 0;
255  unsigned last_drop = 0;
256 
257  while (EOF != (ret = fscanf(fp, "%f,%x,,", &f, &x))) {
258 
259  if (ret <= 0) {
260  fclose(fp);
261  ut_test(ret > 0);
262  }
263 
264  frame[0] = x;
265  unsigned len = 1;
266 
267  // Pipe the data into the parser
269 
270  // if (rate_limiter % byte_offset == 0) {
271  bool result = sbus_parse(now, &frame[0], len, rc_values, &num_values,
272  &sbus_failsafe, &sbus_frame_drop, &sbus_frame_drops, max_channels);
273 
274  if (result) {
275  //PX4_INFO("decoded packet");
276  }
277 
278  // }
279 
280  if (last_drop != (sbus_frame_drops + sbus_frame_resets)) {
281  PX4_WARN("frame dropped, now #%d", (sbus_frame_drops + sbus_frame_resets));
282  last_drop = sbus_frame_drops + sbus_frame_resets;
283  }
284 
285  rate_limiter++;
286  }
287 
288  ut_test(ret == EOF);
289 
290  return true;
291 }
292 
294 {
295  const char *filepath = TEST_DATA_PATH "st24_data.txt";
296 
297  //PX4_INFO("loading data from: %s", filepath);
298 
299  FILE *fp;
300 
301  fp = fopen(filepath, "rt");
302  ut_test(fp);
303 
304  float f;
305  unsigned x;
306  int ret;
307 
308  // Trash the first 20 lines
309  for (unsigned i = 0; i < 20; i++) {
310  char buf[200];
311  (void)fgets(buf, sizeof(buf), fp);
312  }
313 
314  float last_time = 0;
315 
316  while (EOF != (ret = fscanf(fp, "%f,%x,,", &f, &x))) {
317 
318  if (ret <= 0) {
319  fclose(fp);
320  ut_test(ret > 0);
321  }
322 
323  if (((f - last_time) * 1000 * 1000) > 3000) {
324  // PX4_INFO("FRAME RESET\n\n");
325  }
326 
327  uint8_t b = static_cast<uint8_t>(x);
328 
329  last_time = f;
330 
331  // Pipe the data into the parser
332  //hrt_abstime now = hrt_absolute_time();
333 
334  uint8_t rssi;
335  uint8_t rx_count;
336  uint16_t channel_count;
337  uint16_t channels[20];
338 
339  if (!st24_decode(b, &rssi, &rx_count, &channel_count, channels, sizeof(channels) / sizeof(channels[0]))) {
340  //PX4_INFO("decoded: %u channels (converted to PPM range)", (unsigned)channel_count);
341 
342  for (unsigned i = 0; i < channel_count; i++) {
343  //int16_t val = channels[i];
344  //PX4_INFO("channel %u: %d 0x%03X", i, static_cast<int>(val), static_cast<int>(val));
345  }
346  }
347  }
348 
349  ut_test(ret == EOF);
350 
351  return true;
352 }
353 
355 {
356  const char *filepath = TEST_DATA_PATH "sumd_data.txt";
357 
358  //PX4_INFO("loading data from: %s", filepath);
359 
360  FILE *fp;
361 
362  fp = fopen(filepath, "rt");
363  ut_test(fp);
364 
365  float f;
366  unsigned x;
367  int ret;
368 
369  // Trash the first 20 lines
370  for (unsigned i = 0; i < 20; i++) {
371  char buf[200];
372  (void)fgets(buf, sizeof(buf), fp);
373  }
374 
375  float last_time = 0;
376 
377  while (EOF != (ret = fscanf(fp, "%f,%x,,", &f, &x))) {
378 
379  if (ret <= 0) {
380  fclose(fp);
381  ut_test(ret > 0);
382  }
383 
384  if (((f - last_time) * 1000 * 1000) > 3000) {
385  // PX4_INFO("FRAME RESET\n\n");
386  }
387 
388  uint8_t b = static_cast<uint8_t>(x);
389 
390  last_time = f;
391 
392  // Pipe the data into the parser
393  //hrt_abstime now = hrt_absolute_time();
394 
395  uint8_t rssi;
396  uint8_t rx_count;
397  uint16_t channel_count;
398  uint16_t channels[32];
399  bool sumd_failsafe;
400 
401 
402  if (!sumd_decode(b, &rssi, &rx_count, &channel_count, channels, 32, &sumd_failsafe)) {
403  //PX4_INFO("decoded: %u channels (converted to PPM range)", (unsigned)channel_count);
404 
405  for (unsigned i = 0; i < channel_count; i++) {
406  //int16_t val = channels[i];
407  //PX4_INFO("channel %u: %d 0x%03X", i, static_cast<int>(val), static_cast<int>(val));
408  }
409  }
410  }
411 
412  ut_test(ret == EOF);
413 
414  return true;
415 }
416 
417 
418 
420 
int sumd_decode(uint8_t byte, uint8_t *rssi, uint8_t *rx_count, uint16_t *channel_count, uint16_t *channels, uint16_t max_chan_count, bool *failsafe)
Decoder for SUMD/SUMH protocol.
Definition: sumd.cpp:111
bool sumdTest()
Definition: RCTest.cpp:354
RC protocol definition for Spektrum RC.
RC protocol definition for Graupner HoTT transmitter.
#define ut_declare_test_c(test_function, test_class)
Definition: unit_test.h:40
Definition: I2C.hpp:51
bool st24Test()
Definition: RCTest.cpp:293
bool dsmTest(const char *filepath, unsigned expected_chancount, unsigned expected_dropcount, unsigned chan0)
Definition: RCTest.cpp:149
bool crsf_parse(const uint64_t now, const uint8_t *frame, unsigned len, uint16_t *values, uint16_t *num_values, uint16_t max_channels)
Parse the CRSF protocol and extract RC channel data.
Definition: crsf.cpp:166
Base class to be used for unit tests.
Definition: unit_test.h:54
bool dsmTest12Ch()
Definition: RCTest.cpp:144
High-resolution timer with callouts and timekeeping.
int _tests_failed
The number of unit tests which failed.
Definition: unit_test.h:206
#define ut_test(test)
Used to assert a value within a unit test.
Definition: unit_test.h:124
bool dsmTest10Ch()
Definition: RCTest.cpp:139
bool crsfTest()
Definition: RCTest.cpp:53
RC protocol definition for Yuneec ST24 transmitter.
Vector< float, 6 > f(float t, const Matrix< float, 6, 1 > &, const Matrix< float, 3, 1 > &)
Definition: integration.cpp:8
Simple error/warning functions, heavily inspired by the BSD functions of the same names...
bool sbus2Test()
Definition: RCTest.cpp:224
bool dsm_parse(const uint64_t now, const uint8_t *frame, const unsigned len, uint16_t *values, uint16_t *num_values, bool *dsm_11_bit, unsigned *frame_drops, int8_t *rssi_percent, uint16_t max_channels)
Definition: dsm.cpp:640
#define SBUS_BUFFER_SIZE
Definition: sbus.h:50
#define TEST_DATA_PATH
Definition: RCTest.cpp:21
void dsm_proto_init()
Definition: dsm.cpp:273
__BEGIN_DECLS typedef uint64_t hrt_abstime
Absolute time, in microsecond units.
Definition: drv_hrt.h:58
static unsigned sbus_frame_drops
Definition: sbus.cpp:125
static unsigned dsm_frame_drops
Count of incomplete DSM frames.
Definition: dsm.cpp:79
int st24_decode(uint8_t byte, uint8_t *rssi, uint8_t *lost_count, uint16_t *channel_count, uint16_t *channels, uint16_t max_chan_count)
Decoder for ST24 protocol.
Definition: st24.cpp:98
__EXPORT int rc_tests_main(int argc, char *argv[])
RC protocol definition for S.BUS.
#define ut_compare(message, v1, v2)
Used to compare two integer values within a unit test.
Definition: unit_test.h:150
bool sbus_parse(uint64_t now, uint8_t *frame, unsigned len, uint16_t *values, uint16_t *num_values, bool *sbus_failsafe, bool *sbus_frame_drop, unsigned *frame_drops, uint16_t max_channels)
Definition: sbus.cpp:343
#define ut_run_test(test)
Runs a single unit test.
Definition: unit_test.h:96
RC protocol definition for CSRF (TBS Crossfire).
Dual< Scalar, N > abs(const Dual< Scalar, N > &a)
Definition: Dual.hpp:196
bool run_tests() override
Override to run your unit tests.
Definition: RCTest.cpp:41
__EXPORT hrt_abstime hrt_absolute_time(void)
Get absolute time in [us] (does not wrap).