command_manager_test.cpp
Go to the documentation of this file.
1 #include "common.h"
2 #include "rosflight.h"
3 #include "test_board.h"
4 #include "mavlink.h"
5 #include "cmath"
6 
7 #define CHN_LOW 1100
8 #define CHN_HIGH 1900
9 
10 
11 using namespace rosflight_firmware;
12 
13 TEST(command_manager_test, rc)
14 {
15  testBoard board;
16  Mavlink mavlink(board);
17  ROSflight rf(board, mavlink);
18 
19  // Initialize the firmware
20  rf.init();
21 
22  uint16_t rc_values[8];
23  for (int i = 0; i < 8; i++)
24  {
25  rc_values[i] = 1500;
26  }
27  rc_values[2] = 1000;
28 
29  rf.params_.set_param_int(PARAM_MIXER, 10);
30  float max_roll = rf.params_.get_param_float(PARAM_RC_MAX_ROLL);
31  float max_pitch = rf.params_.get_param_float(PARAM_RC_MAX_PITCH);
32  float max_yawrate = rf.params_.get_param_float(PARAM_RC_MAX_YAWRATE);
33 
34 
35  //=================================================
36  // RC Commands Test
37  //=================================================
38  // First, lets just try sending rc_commands alone
39  board.set_rc(rc_values);
40  step_firmware(rf, board, 20000);
41 
42  control_t output = rf.command_manager_.combined_control();
43  EXPECT_EQ(output.x.type, ANGLE);
44  EXPECT_CLOSE(output.x.value, 0.0);
45  EXPECT_EQ(output.y.type, ANGLE);
46  EXPECT_CLOSE(output.y.value, 0.0);
47  EXPECT_EQ(output.z.type, RATE);
48  EXPECT_CLOSE(output.z.value, 0.0);
49  EXPECT_EQ(output.F.type, THROTTLE);
50  EXPECT_CLOSE(output.F.value, 0.0);
51 
52  rc_values[0] = 2000;
53  rc_values[1] = 1000;
54  rc_values[2] = 1500;
55  rc_values[3] = 1250;
56  board.set_rc(rc_values);
57  step_firmware(rf, board, 20000);
58 
59  output = rf.command_manager_.combined_control();
60  EXPECT_EQ(output.x.type, ANGLE);
61  EXPECT_CLOSE(output.x.value, 1.0*max_roll);
62  EXPECT_EQ(output.y.type, ANGLE);
63  EXPECT_CLOSE(output.y.value, -1.0*max_pitch);
64  EXPECT_EQ(output.z.type, RATE);
65  EXPECT_CLOSE(output.z.value, -0.5*max_yawrate);
66  EXPECT_EQ(output.F.type, THROTTLE);
67  EXPECT_CLOSE(output.F.value, 0.5);
68 }
69 
70 
71 TEST(command_manager_test, rc_arm_disarm)
72 {
73  testBoard board;
74  Mavlink mavlink(board);
75  ROSflight rf(board, mavlink);
76 
77  // Make sure that rc is hooked up
78  board.set_pwm_lost(false);
79 
80  // Initialize the firmware
81  rf.init();
82 
83  uint16_t rc_values[8];
84  for (int i = 0; i < 8; i++)
85  {
86  rc_values[i] = 1500;
87  }
88  rc_values[2] = 1000;
89 
90  rf.params_.set_param_int(PARAM_MIXER, 10);
91  float max_roll = rf.params_.get_param_float(PARAM_RC_MAX_ROLL);
92  float max_pitch = rf.params_.get_param_float(PARAM_RC_MAX_PITCH);
93  float max_yawrate = rf.params_.get_param_float(PARAM_RC_MAX_YAWRATE);
94 
95  // Let's clear all errors in the state_manager
96  rf.state_manager_.clear_error(rf.state_manager_.state().error_codes);
97 
98  //=================================================
99  // RC Arming Test
100  //=================================================
101 
102  // Let's send an arming signal
103  rc_values[0] = 1500;
104  rc_values[1] = 1500;
105  rc_values[2] = 1000;
106  rc_values[3] = 2000;
107  board.set_rc(rc_values);
108  // Step halfway long enough to arm (shouldn't be armed yet)
109  step_firmware(rf, board, 500000);
110  EXPECT_EQ(rf.state_manager_.state().armed, false);
111 
112 
113  // Wait the rest of the time
114  step_firmware(rf, board, 600000);
115  // Check the output
116  control_t output = rf.command_manager_.combined_control();
117  EXPECT_EQ(output.x.type, ANGLE);
118  EXPECT_CLOSE(output.x.value, 0.0*max_roll);
119  EXPECT_EQ(output.y.type, ANGLE);
120  EXPECT_CLOSE(output.y.value, 0.0*max_pitch);
121  EXPECT_EQ(output.z.type, RATE);
122  EXPECT_CLOSE(output.z.value, 1.0*max_yawrate);
123  EXPECT_EQ(output.F.type, THROTTLE);
124  EXPECT_CLOSE(output.F.value, 0.0);
125 
126  // See that we are armed
127  EXPECT_EQ(rf.state_manager_.state().armed, true);
128 
129  // Let's send a disarming signal
130  rc_values[0] = 1500;
131  rc_values[1] = 1500;
132  rc_values[2] = 1000;
133  rc_values[3] = 1000;
134  board.set_rc(rc_values);
135  // Step long enough for an arm to happen
136  step_firmware(rf, board, 1200000);
137 
138  // See that we are disarmed
139  EXPECT_EQ(rf.state_manager_.state().armed, false);
140  EXPECT_EQ(rf.state_manager_.state().error, false);
141  EXPECT_EQ(rf.state_manager_.state().failsafe, false);
142 
143 
144  //=================================================
145  // Switch Arming Test
146  //=================================================
147  rf.params_.set_param_int(PARAM_RC_ARM_CHANNEL, 4);
148  rc_values[0] = 1500;
149  rc_values[1] = 1500;
150  rc_values[2] = 1000;
151  rc_values[3] = 1500;
152  rc_values[4] = 1500;
153 
154  // Set all stick neutral position
155  board.set_rc(rc_values);
156  step_firmware(rf, board, 20000);
157  // make sure we are still disarmed
158  EXPECT_EQ(rf.state_manager_.state().armed, false);
159  EXPECT_EQ(rf.state_manager_.state().error, false);
160  EXPECT_EQ(rf.state_manager_.state().failsafe, false);
161 
162  // flip the arm switch on
163  rc_values[4] = CHN_HIGH;
164  board.set_rc(rc_values);
165  step_firmware(rf, board, 20000);
166  // we should be armed
167  EXPECT_EQ(rf.state_manager_.state().armed, true);
168  EXPECT_EQ(rf.state_manager_.state().error, false);
169  EXPECT_EQ(rf.state_manager_.state().failsafe, false);
170 
171  // flip the arm switch off
172  rc_values[4] = CHN_LOW;
173  board.set_rc(rc_values);
174  step_firmware(rf, board, 20000);
175  // we should be disarmed
176  EXPECT_EQ(rf.state_manager_.state().armed, false);
177  EXPECT_EQ(rf.state_manager_.state().error, false);
178  EXPECT_EQ(rf.state_manager_.state().failsafe, false);
179 
180  // Try reversing the arm channel
181  rf.params_.set_param_int(PARAM_RC_SWITCH_5_DIRECTION, -1);
182  rc_values[4] = CHN_LOW;
183  board.set_rc(rc_values);
184  step_firmware(rf, board, 20000);
185  // we should be armed
186  EXPECT_EQ(rf.state_manager_.state().armed, true);
187  EXPECT_EQ(rf.state_manager_.state().error, false);
188  EXPECT_EQ(rf.state_manager_.state().failsafe, false);
189 
190  // now it should be off
191  rc_values[4] = CHN_HIGH;
192  board.set_rc(rc_values);
193  step_firmware(rf, board, 20000);
194  // we should be disarmed
195  EXPECT_EQ(rf.state_manager_.state().armed, false);
196  EXPECT_EQ(rf.state_manager_.state().error, false);
197  EXPECT_EQ(rf.state_manager_.state().failsafe, false);
198 
199 
200  //=================================================
201  // Edge Cases
202  //=================================================
203 
204  // Try to arm with the sticks
205  // Let's send an arming signal (it shouldn't work)
206  rc_values[0] = 1500;
207  rc_values[1] = 1500;
208  rc_values[2] = 1000;
209  rc_values[3] = 2000;
210  board.set_rc(rc_values);
211  step_firmware(rf, board, 1100000);
212  // Check the output
213  output = rf.command_manager_.combined_control();
214  EXPECT_EQ(rf.state_manager_.state().armed, false);
215 
216  // Go back to arming with sticks
217  rf.params_.set_param_int(PARAM_RC_ARM_CHANNEL, -1);
218  rf.params_.set_param_int(PARAM_RC_SWITCH_5_DIRECTION, -1);
219 
220  // try to arm with all the switches (first put all switches CHN_HIGH)
221  rc_values[0] = 1500;
222  rc_values[1] = 1500;
223  rc_values[2] = 1000;
224  rc_values[3] = 1500;
225  rc_values[4] = CHN_HIGH;
226  rc_values[5] = CHN_HIGH;
227  rc_values[6] = CHN_HIGH;
228  rc_values[7] = CHN_HIGH;
229  board.set_rc(rc_values);
230  step_firmware(rf, board, 20000);
231  // we should not be armed
232  EXPECT_EQ(rf.state_manager_.state().armed, false);
233  // now all switches CHN_LOW
234  rc_values[4] = CHN_LOW;
235  rc_values[5] = CHN_LOW;
236  rc_values[6] = CHN_LOW;
237  rc_values[7] = CHN_LOW;
238  board.set_rc(rc_values);
239  step_firmware(rf, board, 20000);
240  // we should not be armed
241  EXPECT_EQ(rf.state_manager_.state().armed, false);
242 }
243 
244 
245 TEST(command_manager_test, rc_failsafe_test)
246 {
247  testBoard board;
248  Mavlink mavlink(board);
249  ROSflight rf(board, mavlink);
250 
251  // Make sure that rc is hooked up
252  board.set_pwm_lost(false);
253 
254  // Initialize the firmware
255  rf.init();
256 
257  uint16_t rc_values[8];
258  for (int i = 0; i < 8; i++)
259  {
260  rc_values[i] = 1500;
261  }
262  rc_values[2] = 1000;
263 
264  rf.params_.set_param_int(PARAM_MIXER, 10);
265  float max_roll = rf.params_.get_param_float(PARAM_RC_MAX_ROLL);
266  float max_pitch = rf.params_.get_param_float(PARAM_RC_MAX_PITCH);
267  float max_yawrate = rf.params_.get_param_float(PARAM_RC_MAX_YAWRATE);
268  float failsafe_throttle = rf.params_.get_param_float(PARAM_FAILSAFE_THROTTLE);
269 
270  // Let's clear all errors in the state_manager
271  rf.state_manager_.clear_error(rf.state_manager_.state().error_codes);
272 
273  //=================================================
274  // Disarmed Failsafe
275  //=================================================
276 
277  // Let's lose rc while disarmed
278  board.set_pwm_lost(true);
279  step_firmware(rf, board, 20000);
280  // Check the output - This should be the last rc value
281  control_t output = rf.command_manager_.combined_control();
282  EXPECT_EQ(output.x.type, ANGLE);
283  EXPECT_CLOSE(output.x.value, 0.0*max_roll);
284  EXPECT_EQ(output.y.type, ANGLE);
285  EXPECT_CLOSE(output.y.value, 0.0*max_pitch);
286  EXPECT_EQ(output.z.type, RATE);
287  EXPECT_CLOSE(output.z.value, 0.0*max_yawrate);
288  EXPECT_EQ(output.F.type, THROTTLE);
289  EXPECT_CLOSE(output.F.value, 0.0);
290 
291  // We should also be disarmed and in error
292  EXPECT_EQ(rf.state_manager_.state().armed, false);
293  EXPECT_EQ(rf.state_manager_.state().failsafe, false);
294  EXPECT_EQ(rf.state_manager_.state().error, true);
295  EXPECT_EQ(rf.state_manager_.state().error_codes, StateManager::ERROR_RC_LOST);
296 
297  // Lets regain rc
298  board.set_pwm_lost(false);
299  board.set_rc(rc_values);
300  step_firmware(rf, board, 20000);
301  output = rf.command_manager_.combined_control();
302  EXPECT_CLOSE(output.x.value, 0.0*max_roll);
303  EXPECT_CLOSE(output.y.value, 0.0*max_pitch);
304  EXPECT_CLOSE(output.z.value, 0.0*max_yawrate);
305  EXPECT_CLOSE(output.F.value, 0.0);
306 
307  // We should still be disarmed, but no failsafe or error
308  EXPECT_EQ(rf.state_manager_.state().armed, false);
309  EXPECT_EQ(rf.state_manager_.state().failsafe, false);
310  EXPECT_EQ(rf.state_manager_.state().error, false);
311  EXPECT_EQ(rf.state_manager_.state().error_codes, 0x00);
312 
313  //=================================================
314  // Armed Failsafe
315  //=================================================
316 
317  // Let's send an arming signal
318  rc_values[0] = 1500;
319  rc_values[1] = 1500;
320  rc_values[2] = 1000;
321  rc_values[3] = 2000;
322  board.set_rc(rc_values);
323  // Step long enough for an arm to happen
324  while (board.clock_millis() < 1200000)
325  {
326  step_firmware(rf, board, 20000);
327  }
328 
329  // check that we are armed
330  EXPECT_EQ(rf.state_manager_.state().armed, true);
331  EXPECT_EQ(rf.state_manager_.state().error, false);
332  EXPECT_EQ(rf.state_manager_.state().failsafe, false);
333 
334  // Set a command on the sticks
335  rc_values[0] = 1750;
336  rc_values[1] = 1250;
337  rc_values[2] = 1600;
338  rc_values[3] = CHN_LOW;
339 
340  // Lost RC
341  board.set_rc(rc_values);
342  board.set_pwm_lost(true);
343  step_firmware(rf, board, 20000);
344  // Check the output - This should be the failsafe control
345  output = rf.command_manager_.combined_control();
346  EXPECT_EQ(output.x.type, ANGLE);
347  EXPECT_CLOSE(output.x.value, 0.0*max_roll);
348  EXPECT_EQ(output.y.type, ANGLE);
349  EXPECT_CLOSE(output.y.value, 0.0*max_pitch);
350  EXPECT_EQ(output.z.type, RATE);
351  EXPECT_CLOSE(output.z.value, 0.0*max_yawrate);
352  EXPECT_EQ(output.F.type, THROTTLE);
353  EXPECT_CLOSE(output.F.value, failsafe_throttle);
354 
355  // We should still be armed, but now in failsafe
356  EXPECT_EQ(rf.state_manager_.state().armed, true);
357  EXPECT_EQ(rf.state_manager_.state().error, true);
358  EXPECT_EQ(rf.state_manager_.state().failsafe, true);
359 
360  // Regain RC
361  board.set_pwm_lost(false);
362  step_firmware(rf, board, 20000);
363  // Check the output - This should be our rc control
364  output = rf.command_manager_.combined_control();
365  EXPECT_EQ(output.x.type, ANGLE);
366  EXPECT_CLOSE(output.x.value, 0.5*max_roll);
367  EXPECT_EQ(output.y.type, ANGLE);
368  EXPECT_CLOSE(output.y.value, -0.5*max_pitch);
369  EXPECT_EQ(output.z.type, RATE);
370  EXPECT_CLOSE(output.z.value, -0.8*max_yawrate);
371  EXPECT_EQ(output.F.type, THROTTLE);
372  EXPECT_CLOSE(output.F.value, 0.6);
373 }
374 
375 
376 
377 #define OFFBOARD_X -1.0
378 #define OFFBOARD_Y 0.5
379 #define OFFBOARD_Z -0.7
380 #define OFFBOARD_F 0.9
381 
382 #define RC_X_PWM 1800
383 #define RC_X ((RC_X_PWM - 1500)/500.0 * rf.params_.get_param_float(PARAM_RC_MAX_ROLL))
384 
385 TEST(command_manager_test, rc_offboard_muxing_test)
386 {
387 
388  testBoard board;
389  Mavlink mavlink(board);
390  ROSflight rf(board, mavlink);
391 
392  // Make sure that rc is hooked up
393  board.set_pwm_lost(false);
394 
395  // Initialize the firmware
396  rf.init();
397  rf.params_.set_param_int(PARAM_MIXER, 10);
398  rf.params_.set_param_int(PARAM_RC_OVERRIDE_TAKE_MIN_THROTTLE, false);
399 
400  uint16_t rc_values[8];
401  for (int i = 0; i < 8; i++)
402  {
403  rc_values[i] = 1500;
404  }
405  rc_values[2] = 1000;
406 
407  // Let's clear all errors in the state_manager
408  rf.state_manager_.clear_error(rf.state_manager_.state().error_codes);
409 
410  //=================================================
411  // Offboard Command Integration
412  //=================================================
413 
414  control_t offboard_command =
415  {
416  20000,
417  {true, ANGLE, OFFBOARD_X},
418  {true, ANGLE, OFFBOARD_Y},
419  {true, RATE, OFFBOARD_Z},
420  {true, THROTTLE, OFFBOARD_F}
421  };
422 
423  // First, just set an offboard command and rc, mux it and see what happens
424  board.set_rc(rc_values);
425  // step a bunch of times to clear the "lag time" on RC
426  while (board.clock_micros() < 1000000)
427  {
428  step_firmware(rf, board, 20000);
429  }
430 
431  offboard_command.stamp_ms = board.clock_millis();
432  rf.command_manager_.set_new_offboard_command(offboard_command);
433  step_firmware(rf, board, 20000);
434 
435  // We don't have an override channel mapped, so this should be offboard
436  control_t output = rf.command_manager_.combined_control();
437  EXPECT_CLOSE(output.x.value, OFFBOARD_X);
438  EXPECT_CLOSE(output.y.value, OFFBOARD_Y);
439  EXPECT_CLOSE(output.z.value, OFFBOARD_Z);
440  EXPECT_CLOSE(output.F.value, OFFBOARD_F);
441 
442  rf.params_.set_param_int(PARAM_RC_ATTITUDE_OVERRIDE_CHANNEL, 4);
443  rf.params_.set_param_int(PARAM_RC_THROTTLE_OVERRIDE_CHANNEL, 4);
444  rf.params_.set_param_int(PARAM_RC_SWITCH_5_DIRECTION, 1);
445 
446  // ensure that the override switch is off
447  rc_values[4] = CHN_LOW;
448 
449  board.set_rc(rc_values);
450  offboard_command.stamp_ms = board.clock_millis();
451  rf.command_manager_.set_new_offboard_command(offboard_command);
452  step_firmware(rf, board, 20000);
453 
454  // This should be offboard
455  output = rf.command_manager_.combined_control();
456  EXPECT_CLOSE(output.x.value, OFFBOARD_X);
457  EXPECT_CLOSE(output.y.value, OFFBOARD_Y);
458  EXPECT_CLOSE(output.z.value, OFFBOARD_Z);
459  EXPECT_CLOSE(output.F.value, OFFBOARD_F);
460 
461  // flip override switch on
462  rc_values[4] = CHN_HIGH;
463  board.set_rc(rc_values);
464  offboard_command.stamp_ms = board.clock_millis();
465  rf.command_manager_.set_new_offboard_command(offboard_command);
466  step_firmware(rf, board, 20000);
467 
468  // This should be RC
469  output = rf.command_manager_.combined_control();
470  EXPECT_CLOSE(output.x.value, 0.0);
471  EXPECT_CLOSE(output.y.value, 0.0);
472  EXPECT_CLOSE(output.z.value, 0.0);
473  EXPECT_CLOSE(output.F.value, 0.0);
474 
475 
476  //=================================================
477  // Partial Offboard Command Integration
478  //=================================================
479 
480  // Only override attitude
481  rf.params_.set_param_int(PARAM_RC_ATTITUDE_OVERRIDE_CHANNEL, 4);
482  rf.params_.set_param_int(PARAM_RC_THROTTLE_OVERRIDE_CHANNEL, -1);
483 
484  board.set_rc(rc_values);
485  offboard_command.stamp_ms = board.clock_millis();
486  rf.command_manager_.set_new_offboard_command(offboard_command);
487  step_firmware(rf, board, 20000);
488 
489  // Throttle should be offboard
490  output = rf.command_manager_.combined_control();
491  EXPECT_CLOSE(output.x.value, 0.0);
492  EXPECT_CLOSE(output.y.value, 0.0);
493  EXPECT_CLOSE(output.z.value, 0.0);
494  EXPECT_CLOSE(output.F.value, OFFBOARD_F);
495 
496 
497  // Only override throttle
498  rf.params_.set_param_int(PARAM_RC_ATTITUDE_OVERRIDE_CHANNEL, -1);
499  rf.params_.set_param_int(PARAM_RC_THROTTLE_OVERRIDE_CHANNEL, 4);
500 
501  board.set_rc(rc_values);
502  offboard_command.stamp_ms = board.clock_millis();
503  rf.command_manager_.set_new_offboard_command(offboard_command);
504  step_firmware(rf, board, 20000);
505 
506  // Throttle should be rc
507  output = rf.command_manager_.combined_control();
508  EXPECT_CLOSE(output.x.value, OFFBOARD_X);
509  EXPECT_CLOSE(output.y.value, OFFBOARD_Y);
510  EXPECT_CLOSE(output.z.value, OFFBOARD_Z);
511  EXPECT_CLOSE(output.F.value, 0.0);
512 
513 
514  //=================================================
515  // RC Intervention
516  //=================================================
517 
518  // Only override attitude
519  rf.params_.set_param_int(PARAM_RC_ATTITUDE_OVERRIDE_CHANNEL, 4);
520  rf.params_.set_param_int(PARAM_RC_THROTTLE_OVERRIDE_CHANNEL, 4);
521 
522  // switch is off, but roll channel is deviated
523  rc_values[4] = CHN_LOW;
524  rc_values[0] = RC_X_PWM;
525  rc_values[2] = 1000; // move throttle to neutral position
526 
527  board.set_rc(rc_values);
528  offboard_command.stamp_ms = board.clock_millis();
529  rf.command_manager_.set_new_offboard_command(offboard_command);
530  step_firmware(rf, board, 20000);
531 
532  output = rf.command_manager_.combined_control();
533  EXPECT_CLOSE(output.x.value, RC_X); // This channel should be overwritten
534  EXPECT_CLOSE(output.y.value, OFFBOARD_Y);
535  EXPECT_CLOSE(output.z.value, OFFBOARD_Z);
536  EXPECT_CLOSE(output.F.value, OFFBOARD_F);
537 
538  //=================================================
539  // RC Override Lag Test
540  //=================================================
541 
542  // Switch is still off, and rc no longer deviated. There should still be lag though
543  rc_values[0] = 1500;
544  rc_values[2] = 1000;
545  board.set_rc(rc_values);
546  uint64_t start_ms = board.clock_millis();
547  step_firmware(rf, board, 20000);
548  while (board.clock_millis() < rf.params_.get_param_int(PARAM_OVERRIDE_LAG_TIME) + start_ms)
549  {
550  output = rf.command_manager_.combined_control();
551  EXPECT_CLOSE(output.x.value, 0.0); // This channel should be overwritten
552  EXPECT_CLOSE(output.y.value, OFFBOARD_Y);
553  EXPECT_CLOSE(output.z.value, OFFBOARD_Z);
554  EXPECT_CLOSE(output.F.value, OFFBOARD_F);
555  offboard_command.stamp_ms = board.clock_millis();
556  rf.command_manager_.set_new_offboard_command(offboard_command);
557  step_firmware(rf, board, 20000);
558  }
559  offboard_command.stamp_ms = board.clock_millis();
560  rf.command_manager_.set_new_offboard_command(offboard_command);
561  step_firmware(rf, board, 20000);
562  output = rf.command_manager_.combined_control();
563  EXPECT_CLOSE(output.x.value, -1.0); // This channel should no longer be overwritten
564 
565 
566  //=================================================
567  // Offboard Command Stale
568  //=================================================
569 
570  start_ms = board.clock_millis();
571  while (board.clock_millis() < rf.params_.get_param_int(PARAM_OFFBOARD_TIMEOUT) + start_ms)
572  {
573  output = rf.command_manager_.combined_control();
574  EXPECT_CLOSE(output.x.value, OFFBOARD_X); // Offboard Command is still valid
575  EXPECT_CLOSE(output.y.value, OFFBOARD_Y);
576  EXPECT_CLOSE(output.z.value, OFFBOARD_Z);
577  EXPECT_CLOSE(output.F.value, OFFBOARD_F);
578  step_firmware(rf, board, 20000);
579  }
580 
581  // Offboard command has timed out -> revert to RC
582  output = rf.command_manager_.combined_control();
583  EXPECT_CLOSE(output.x.value, 0.0);
584  EXPECT_CLOSE(output.y.value, 0.0);
585  EXPECT_CLOSE(output.z.value, 0.0);
586  EXPECT_CLOSE(output.F.value, 0.0);
587 }
588 
589 
590 TEST(command_manager_test, partial_muxing_test)
591 {
592 
593  testBoard board;
594  Mavlink mavlink(board);
595  ROSflight rf(board, mavlink);
596 
597  // Make sure that rc is hooked up
598  board.set_pwm_lost(false);
599 
600  // Initialize the firmware
601  rf.init();
602 
603  uint16_t rc_values[8];
604  for (int i = 0; i < 8; i++)
605  {
606  rc_values[i] = 1500;
607  }
608  rc_values[2] = 1000;
609  rf.params_.set_param_int(PARAM_MIXER, 10);
610 
611  // Let's clear all errors in the state_manager
612  rf.state_manager_.clear_error(rf.state_manager_.state().error_codes);
613 
614  //=================================================
615  // Min Throttle Test
616  //=================================================
617 
618  rf.params_.set_param_int(PARAM_RC_OVERRIDE_TAKE_MIN_THROTTLE, true);
619 
620  control_t offboard_command =
621  {
622  20000,
623  {true, ANGLE, OFFBOARD_X},
624  {true, ANGLE, OFFBOARD_Y},
625  {true, RATE, OFFBOARD_Z},
626  {true, THROTTLE, OFFBOARD_F}
627  };
628 
629  // step a bunch of times to clear the "lag time" on RC
630  board.set_rc(rc_values);
631  while (board.clock_micros() < 2000000)
632  {
633  step_firmware(rf, board, 20000);
634  }
635 
636  rc_values[2] = 1200;
637 
638  // RC is the min throttle
639  board.set_rc(rc_values);
640  offboard_command.stamp_ms = board.clock_millis();
641  rf.command_manager_.set_new_offboard_command(offboard_command);
642  step_firmware(rf, board, 20000);
643  control_t output = rf.command_manager_.combined_control();
644  EXPECT_CLOSE(output.F.value, 0.2);
645 
646  // Now, offboard is the min throttle
647  offboard_command.F.value = 0.2;
648  rc_values[2] = 1500;
649 
650  board.set_rc(rc_values);
651  offboard_command.stamp_ms = board.clock_millis();
652  rf.command_manager_.set_new_offboard_command(offboard_command);
653  step_firmware(rf, board, 20000);
654  output = rf.command_manager_.combined_control();
655  EXPECT_CLOSE(output.F.value, 0.2);
656 
657  // Okay, remove the Min throttle setting
658  rf.params_.set_param_int(PARAM_RC_OVERRIDE_TAKE_MIN_THROTTLE, false);
659 
660  rc_values[2] = 1200;
661  offboard_command.F.value = OFFBOARD_F;
662 
663  // RC is the min throttle
664  board.set_rc(rc_values);
665  offboard_command.stamp_ms = board.clock_millis();
666  rf.command_manager_.set_new_offboard_command(offboard_command);
667  step_firmware(rf, board, 20000);
668  output = rf.command_manager_.combined_control();
669  // We should get offboard command, even though RC is lower
670  EXPECT_CLOSE(output.F.value, OFFBOARD_F);
671 
672 
673  // Now, let's disable the pitch channel on the onboard command
674  offboard_command.y.active = false;
675  offboard_command.stamp_ms = board.clock_millis();
676  rf.command_manager_.set_new_offboard_command(offboard_command);
677 
678  step_firmware(rf, board, 20000);
679  output = rf.command_manager_.combined_control();
680  EXPECT_CLOSE(output.x.value, OFFBOARD_X);
681  EXPECT_CLOSE(output.y.value, 0.0);
682  EXPECT_CLOSE(output.z.value, OFFBOARD_Z);
683  EXPECT_CLOSE(output.F.value, OFFBOARD_F);
684 
685  // Let's change the type on the x channel
686  offboard_command.x.type = RATE;
687  offboard_command.stamp_ms = board.clock_millis();
688  rf.command_manager_.set_new_offboard_command(offboard_command);
689 
690  step_firmware(rf, board, 20000);
691  output = rf.command_manager_.combined_control();
692  EXPECT_EQ(output.x.type, RATE);
693  EXPECT_EQ(output.y.type, ANGLE);
694  EXPECT_EQ(output.z.type, RATE);
695  EXPECT_EQ(output.F.type, THROTTLE);
696 }
rosflight_firmware::testBoard::clock_millis
uint32_t clock_millis() override
Definition: test_board.cpp:75
CHN_LOW
#define CHN_LOW
Definition: command_manager_test.cpp:7
OFFBOARD_Y
#define OFFBOARD_Y
Definition: command_manager_test.cpp:378
rosflight_firmware::StateManager::State::error_codes
uint16_t error_codes
Definition: state_manager.h:51
rosflight_firmware::control_t
Definition: command_manager.h:60
rosflight_firmware::testBoard
Definition: test_board.h:41
rosflight_firmware::CommandManager::combined_control
const control_t & combined_control() const
Definition: command_manager.h:185
rosflight_firmware::control_t::stamp_ms
uint32_t stamp_ms
Definition: command_manager.h:62
rosflight_firmware::THROTTLE
Definition: command_manager.h:49
rosflight_firmware
Definition: airbourne_board.cpp:34
rosflight_firmware::Mavlink
Definition: mavlink.h:50
OFFBOARD_Z
#define OFFBOARD_Z
Definition: command_manager_test.cpp:379
rosflight_firmware::StateManager::State::error
bool error
Definition: state_manager.h:50
OFFBOARD_F
#define OFFBOARD_F
Definition: command_manager_test.cpp:380
rosflight_firmware::testBoard::clock_micros
uint64_t clock_micros() override
Definition: test_board.cpp:79
rosflight_firmware::PARAM_RC_MAX_PITCH
Definition: param.h:189
rosflight_firmware::testBoard::set_rc
void set_rc(uint16_t *values)
Definition: test_board.cpp:40
rosflight_firmware::StateManager::State::armed
bool armed
Definition: state_manager.h:48
rosflight_firmware::Params::get_param_float
float get_param_float(uint16_t id) const
Get the value of a floating point parameter by id.
Definition: param.h:326
RC_X
#define RC_X
Definition: command_manager_test.cpp:383
test_board.h
rosflight_firmware::PARAM_OFFBOARD_TIMEOUT
Definition: param.h:216
rosflight_firmware::control_t::x
control_channel_t x
Definition: command_manager.h:63
rosflight_firmware::StateManager::State::failsafe
bool failsafe
Definition: state_manager.h:49
TEST
TEST(command_manager_test, rc)
Definition: command_manager_test.cpp:13
rosflight_firmware::PARAM_OVERRIDE_LAG_TIME
Definition: param.h:184
rosflight_firmware::PARAM_MIXER
Definition: param.h:197
rosflight_firmware::ROSflight
Definition: include/rosflight.h:50
rosflight_firmware::control_t::y
control_channel_t y
Definition: command_manager.h:64
rosflight_firmware::PARAM_RC_ARM_CHANNEL
Definition: param.h:175
rosflight_firmware::StateManager::state
const State & state() const
Definition: state_manager.h:82
rosflight_firmware::CommandManager::set_new_offboard_command
void set_new_offboard_command(control_t new_offboard_command)
Definition: command_manager.cpp:251
rosflight_firmware::PARAM_RC_ATTITUDE_OVERRIDE_CHANNEL
Definition: param.h:172
rosflight_firmware::StateManager::clear_error
void clear_error(uint16_t error)
Definition: state_manager.cpp:82
CHN_HIGH
#define CHN_HIGH
Definition: command_manager_test.cpp:8
rosflight_firmware::control_channel_t::type
control_type_t type
Definition: command_manager.h:56
param_parser.i
int i
Definition: param_parser.py:19
RC_X_PWM
#define RC_X_PWM
Definition: command_manager_test.cpp:382
OFFBOARD_X
#define OFFBOARD_X
Definition: command_manager_test.cpp:377
rosflight_firmware::ROSflight::params_
Params params_
Definition: include/rosflight.h:62
rosflight_firmware::PARAM_FAILSAFE_THROTTLE
Definition: param.h:115
rosflight_firmware::StateManager::ERROR_RC_LOST
Definition: state_manager.h:72
rosflight_firmware::Params::get_param_int
int get_param_int(uint16_t id) const
Get the value of an integer parameter by id.
Definition: param.h:316
step_firmware
void step_firmware(rosflight_firmware::ROSflight &rf, rosflight_firmware::testBoard &board, uint32_t us)
Definition: common.cpp:30
rosflight_firmware::control_channel_t::value
float value
Definition: command_manager.h:57
rosflight_firmware::ROSflight::state_manager_
StateManager state_manager_
Definition: include/rosflight.h:70
rosflight_firmware::PARAM_RC_OVERRIDE_TAKE_MIN_THROTTLE
Definition: param.h:185
rosflight_firmware::testBoard::set_pwm_lost
void set_pwm_lost(bool lost)
Definition: test_board.cpp:53
rosflight_firmware::ROSflight::command_manager_
CommandManager command_manager_
Definition: include/rosflight.h:64
rosflight_firmware::PARAM_RC_MAX_YAWRATE
Definition: param.h:192
rosflight_firmware::control_t::z
control_channel_t z
Definition: command_manager.h:65
rosflight_firmware::PARAM_RC_SWITCH_5_DIRECTION
Definition: param.h:178
rosflight_firmware::PARAM_RC_MAX_ROLL
Definition: param.h:188
rosflight_firmware::ROSflight::init
void init()
Main initialization routine for the ROSflight autopilot flight stack.
Definition: rosflight.cpp:52
rosflight_firmware::control_t::F
control_channel_t F
Definition: command_manager.h:66
rosflight_firmware::RATE
Definition: command_manager.h:47
rosflight_firmware::Params::set_param_int
bool set_param_int(uint16_t id, int32_t value)
Sets the value of a parameter by ID and calls the parameter change callback.
Definition: param.cpp:377
rosflight_firmware::PARAM_RC_THROTTLE_OVERRIDE_CHANNEL
Definition: param.h:173
rosflight_firmware::ANGLE
Definition: command_manager.h:48
rosflight_firmware::control_channel_t::active
bool active
Definition: command_manager.h:55
EXPECT_CLOSE
#define EXPECT_CLOSE(x, y)
Definition: test/common.h:50

rosflight_firmware
Author(s): Daniel Koch , James Jackson
autogenerated on Wed Jul 3 2019 19:59:24