command_manager_test.cpp
Go to the documentation of this file.
1 #include "common.h"
2 #include "mavlink.h"
3 #include "test_board.h"
4 
5 #include "rosflight.h"
6 
7 #include <cmath>
8 
9 #define CHN_LOW 1100
10 #define CHN_HIGH 1900
11 
12 #define OFFBOARD_X -1.0
13 #define OFFBOARD_Y 0.5
14 #define OFFBOARD_Z -0.7
15 #define OFFBOARD_F 0.9
16 
17 #define RC_X_PWM 1800
18 #define RC_X ((RC_X_PWM - 1500) / 500.0 * rf.params_.get_param_float(PARAM_RC_MAX_ROLL))
19 
20 using namespace rosflight_firmware;
21 
22 class CommandManagerTest : public ::testing::Test
23 {
24 public:
28 
29  uint16_t rc_values[8];
30  float max_roll, max_pitch, max_yawrate;
31 
32  control_t offboard_command = {20000,
33  {true, ANGLE, OFFBOARD_X},
34  {true, ANGLE, OFFBOARD_Y},
35  {true, RATE, OFFBOARD_Z},
36  {true, THROTTLE, OFFBOARD_F}};
37 
38  CommandManagerTest() : mavlink(board), rf(board, mavlink) {}
39 
40  void SetUp() override
41  {
42  rf.init();
43  rf.state_manager_.clear_error(rf.state_manager_.state().error_codes); // Clear All Errors to Start
45 
46  for (int i = 0; i < 8; i++)
47  {
48  rc_values[i] = 1500;
49  }
50  rc_values[2] = 1000;
51 
56  }
57 
58  void setOffboard(control_t& command)
59  {
60  command.stamp_ms = rf.board_.clock_millis();
62  }
63 
64  void stepFirmware(uint32_t us)
65  {
66  uint64_t start_time_us = board.clock_micros();
67  float dummy_acc[3] = {0, 0, -9.80665};
68  float dummy_gyro[3] = {0, 0, 0};
69  while (board.clock_micros() < start_time_us + us)
70  {
71  board.set_imu(dummy_acc, dummy_gyro, board.clock_micros() + 1000);
72  rf.run();
73  }
74  }
75 };
76 
78 {
79  board.set_rc(rc_values);
80  stepFirmware(20000);
81 
82  control_t output = rf.command_manager_.combined_control();
83  EXPECT_EQ(output.x.type, ANGLE);
84  EXPECT_CLOSE(output.x.value, 0.0);
85  EXPECT_EQ(output.y.type, ANGLE);
86  EXPECT_CLOSE(output.y.value, 0.0);
87  EXPECT_EQ(output.z.type, RATE);
88  EXPECT_CLOSE(output.z.value, 0.0);
89  EXPECT_EQ(output.F.type, THROTTLE);
90  EXPECT_CLOSE(output.F.value, 0.0);
91 }
92 
94 {
95  rc_values[0] = 2000;
96  rc_values[1] = 1000;
97  rc_values[2] = 1500;
98  rc_values[3] = 1250;
99  board.set_rc(rc_values);
100  stepFirmware(20000);
101 
102  control_t output = rf.command_manager_.combined_control();
103  EXPECT_EQ(output.x.type, ANGLE);
104  EXPECT_CLOSE(output.x.value, 1.0 * max_roll);
105  EXPECT_EQ(output.y.type, ANGLE);
106  EXPECT_CLOSE(output.y.value, -1.0 * max_pitch);
107  EXPECT_EQ(output.z.type, RATE);
108  EXPECT_CLOSE(output.z.value, -0.5 * max_yawrate);
109  EXPECT_EQ(output.F.type, THROTTLE);
110  EXPECT_CLOSE(output.F.value, 0.5);
111 }
112 
113 TEST_F(CommandManagerTest, ArmWithSticksByDefault)
114 {
115  EXPECT_EQ(rf.state_manager_.state().armed, false);
116  rc_values[2] = 1000;
117  rc_values[3] = 2000;
118  board.set_rc(rc_values);
119  stepFirmware(500000);
120  EXPECT_EQ(rf.state_manager_.state().armed,
121  false); // need to wait 1 second, shouldn't be armed yet
122  stepFirmware(600000);
123  EXPECT_EQ(rf.state_manager_.state().armed, true);
124 }
125 
126 TEST_F(CommandManagerTest, DontArmWithSticksWhenUsingSwitch)
127 {
128  rf.params_.set_param_int(PARAM_RC_ARM_CHANNEL, 4);
129  rc_values[2] = 1000; // throttle low
130  rc_values[3] = 2000; // yaw right
131  board.set_rc(rc_values);
132  stepFirmware(1100000);
133  EXPECT_EQ(rf.state_manager_.state().armed, false);
134 }
135 
136 TEST_F(CommandManagerTest, DisarmWithSticksByDefault)
137 {
138  rf.state_manager_.set_event(StateManager::EVENT_REQUEST_ARM);
139  EXPECT_EQ(rf.state_manager_.state().armed, true);
140  rc_values[2] = 1000; // throttle low
141  rc_values[3] = 1000; // yaw left
142  board.set_rc(rc_values);
143  stepFirmware(1100000);
144  EXPECT_EQ(rf.state_manager_.state().armed, false);
145 }
146 
147 TEST_F(CommandManagerTest, ArmWithSwitch)
148 {
149  rf.params_.set_param_int(PARAM_RC_ARM_CHANNEL, 4);
150  rc_values[2] = 1000; // throttle low
151  rc_values[4] = CHN_HIGH; // switch on
152  board.set_rc(rc_values);
153  stepFirmware(50000); // Immediate
154  EXPECT_EQ(rf.state_manager_.state().armed, true);
155 }
156 
157 TEST_F(CommandManagerTest, DisarmWithStick)
158 {
159  rf.params_.set_param_int(PARAM_RC_ARM_CHANNEL, 4);
160  rf.state_manager_.set_event(StateManager::EVENT_REQUEST_ARM);
161  rc_values[4] = CHN_LOW; // throttle low
162  board.set_rc(rc_values);
163  stepFirmware(50000); // Immediate
164  EXPECT_EQ(rf.state_manager_.state().armed, false);
165 }
166 
167 TEST_F(CommandManagerTest, DontDisarmWithSticksWhenUsingSwitch)
168 {
169  rf.params_.set_param_int(PARAM_RC_ARM_CHANNEL, 4);
170  rc_values[4] = CHN_HIGH; // switch on
171  rc_values[2] = 1000; // throttle low
172  rf.state_manager_.set_event(StateManager::EVENT_REQUEST_ARM);
173  board.set_rc(rc_values);
174  stepFirmware(50000);
175  EXPECT_EQ(rf.state_manager_.state().armed, true);
176 
177  rc_values[2] = 1000; // throttle low
178  rc_values[3] = 1000; // yaw left
179  stepFirmware(1100000);
180  EXPECT_EQ(rf.state_manager_.state().armed, true); // don't disarm
181 }
182 
183 TEST_F(CommandManagerTest, ArmStickReversed)
184 {
185  rf.params_.set_param_int(PARAM_RC_SWITCH_5_DIRECTION, -1);
186  rf.params_.set_param_int(PARAM_RC_ARM_CHANNEL, 4);
187  rc_values[2] = 1000; // throttle low
188  rc_values[4] = CHN_LOW; // switch on
189  board.set_rc(rc_values);
190  stepFirmware(50000); // Immediate
191  EXPECT_EQ(rf.state_manager_.state().armed, true);
192 }
193 
194 TEST_F(CommandManagerTest, DisarmStickReversed)
195 {
196  rf.params_.set_param_int(PARAM_RC_SWITCH_5_DIRECTION, -1);
197  rf.params_.set_param_int(PARAM_RC_ARM_CHANNEL, 4);
198  rf.state_manager_.set_event(StateManager::EVENT_REQUEST_ARM);
199  EXPECT_EQ(rf.state_manager_.state().armed, true);
200 
201  rc_values[2] = 1000; // throttle low
202  rc_values[4] = CHN_HIGH; // switch on
203  board.set_rc(rc_values);
204  stepFirmware(50000); // Immediate
205  EXPECT_EQ(rf.state_manager_.state().armed, false);
206 }
207 
208 TEST_F(CommandManagerTest, DefaultRCOutputd)
209 {
210  board.set_rc(rc_values);
211  stepFirmware(600000);
212 
213  // Check the output
214  control_t output = rf.command_manager_.combined_control();
215  EXPECT_EQ(output.x.type, ANGLE);
216  EXPECT_CLOSE(output.x.value, 0.0);
217  EXPECT_EQ(output.y.type, ANGLE);
218  EXPECT_CLOSE(output.y.value, 0.0);
219  EXPECT_EQ(output.z.type, RATE);
220  EXPECT_CLOSE(output.z.value, 0.0);
221  EXPECT_EQ(output.F.type, THROTTLE);
222  EXPECT_CLOSE(output.F.value, 0.0);
223 }
224 
226 {
227  rc_values[0] = 1250;
228  rc_values[1] = 1750;
229  rc_values[2] = 1500;
230  rc_values[3] = 2000;
231  board.set_rc(rc_values);
232  stepFirmware(600000);
233 
234  // Check the output
235  EXPECT_EQ(rf.state_manager_.state().armed, false);
236  control_t output = rf.command_manager_.combined_control();
237  EXPECT_EQ(output.x.type, ANGLE);
238  EXPECT_CLOSE(output.x.value, max_roll * -0.5);
239  EXPECT_EQ(output.y.type, ANGLE);
240  EXPECT_CLOSE(output.y.value, max_pitch * 0.5);
241  EXPECT_EQ(output.z.type, RATE);
242  EXPECT_CLOSE(output.z.value, max_yawrate);
243  EXPECT_EQ(output.F.type, THROTTLE);
244  EXPECT_CLOSE(output.F.value, 0.5);
245 }
246 
247 TEST_F(CommandManagerTest, LoseRCDisarmed)
248 {
249  board.set_pwm_lost(true);
250  stepFirmware(20000);
251 
252  control_t output = rf.command_manager_.combined_control();
253  EXPECT_EQ(output.x.type, ANGLE);
254  EXPECT_CLOSE(output.x.value, 0.0 * max_roll);
255  EXPECT_EQ(output.y.type, ANGLE);
256  EXPECT_CLOSE(output.y.value, 0.0 * max_pitch);
257  EXPECT_EQ(output.z.type, RATE);
258  EXPECT_CLOSE(output.z.value, 0.0 * max_yawrate);
259  EXPECT_EQ(output.F.type, THROTTLE);
260  EXPECT_CLOSE(output.F.value, 0.0);
261 
262  // We should also be disarmed and in error
263  EXPECT_EQ(rf.state_manager_.state().armed, false);
264  EXPECT_EQ(rf.state_manager_.state().failsafe, false);
265  EXPECT_EQ(rf.state_manager_.state().error, true);
266  EXPECT_EQ(rf.state_manager_.state().error_codes, StateManager::ERROR_RC_LOST);
267 }
268 
269 TEST_F(CommandManagerTest, RegainRCDisarmed)
270 {
271  board.set_pwm_lost(true);
272  stepFirmware(40000);
273  board.set_pwm_lost(false);
274  stepFirmware(40000);
275 
276  EXPECT_EQ(rf.state_manager_.state().error, false);
277  EXPECT_EQ(rf.state_manager_.state().error_codes, StateManager::ERROR_NONE);
278 }
279 
281 {
282  board.set_rc(rc_values);
283  stepFirmware(50000);
284 
285  rf.state_manager_.set_event(StateManager::EVENT_REQUEST_ARM);
286  EXPECT_EQ(rf.state_manager_.state().armed, true);
287  board.set_pwm_lost(true);
288  stepFirmware(20000);
289 
290  control_t output = rf.command_manager_.combined_control();
291  EXPECT_EQ(output.x.type, ANGLE);
292  EXPECT_CLOSE(output.x.value, 0.0 * max_roll);
293  EXPECT_EQ(output.y.type, ANGLE);
294  EXPECT_CLOSE(output.y.value, 0.0 * max_pitch);
295  EXPECT_EQ(output.z.type, RATE);
296  EXPECT_CLOSE(output.z.value, 0.0 * max_yawrate);
297  EXPECT_EQ(output.F.type, THROTTLE);
298  EXPECT_CLOSE(output.F.value, rf.params_.get_param_float(PARAM_FAILSAFE_THROTTLE));
299 
300  // We should also be disarmed and in error
301  EXPECT_EQ(rf.state_manager_.state().armed, true);
302  EXPECT_EQ(rf.state_manager_.state().failsafe, true);
303  EXPECT_EQ(rf.state_manager_.state().error, true);
304  EXPECT_EQ(rf.state_manager_.state().error_codes, StateManager::ERROR_RC_LOST);
305 }
306 
307 TEST_F(CommandManagerTest, RegainRCArmed)
308 {
309  board.set_rc(rc_values);
310  stepFirmware(50000);
311  rf.state_manager_.set_event(StateManager::EVENT_REQUEST_ARM);
312 
313  board.set_pwm_lost(true);
314  stepFirmware(20000);
315  board.set_pwm_lost(false);
316  stepFirmware(20000);
317 
318  EXPECT_EQ(rf.state_manager_.state().armed, true);
319  EXPECT_EQ(rf.state_manager_.state().error, false);
320  EXPECT_EQ(rf.state_manager_.state().error_codes, StateManager::ERROR_NONE);
321 }
322 
323 TEST_F(CommandManagerTest, OffboardCommandMuxNoMinThrottle)
324 {
325  stepFirmware(1100000); // Get past LAG_TIME
326  rf.params_.set_param_int(PARAM_RC_OVERRIDE_TAKE_MIN_THROTTLE, false);
327 
328  // set the new offboard command
329  offboard_command.stamp_ms = board.clock_millis();
330  rf.command_manager_.set_new_offboard_command(offboard_command);
331  stepFirmware(20000);
332 
333  control_t output = rf.command_manager_.combined_control();
334  EXPECT_CLOSE(output.x.value, OFFBOARD_X);
335  EXPECT_CLOSE(output.y.value, OFFBOARD_Y);
336  EXPECT_CLOSE(output.z.value, OFFBOARD_Z);
337  EXPECT_CLOSE(output.F.value, OFFBOARD_F);
338 }
339 
340 TEST_F(CommandManagerTest, OffboardCommandMuxMinThrottle)
341 {
342  stepFirmware(1100000); // Get past LAG_TIME
343  rf.params_.set_param_int(PARAM_RC_OVERRIDE_TAKE_MIN_THROTTLE, true);
344 
345  // set the new offboard command
346  offboard_command.stamp_ms = board.clock_millis();
347  rf.command_manager_.set_new_offboard_command(offboard_command);
348  stepFirmware(20000);
349 
350  control_t output = rf.command_manager_.combined_control();
351  EXPECT_CLOSE(output.x.value, OFFBOARD_X);
352  EXPECT_CLOSE(output.y.value, OFFBOARD_Y);
353  EXPECT_CLOSE(output.z.value, OFFBOARD_Z);
354  EXPECT_CLOSE(output.F.value, 0.0);
355 }
356 
357 TEST_F(CommandManagerTest, OffboardCommandMuxRollDeviation)
358 {
359  stepFirmware(1100000); // Get past LAG_TIME
360  rf.params_.set_param_int(PARAM_RC_OVERRIDE_TAKE_MIN_THROTTLE, true);
361  rc_values[0] = 1250;
362  board.set_rc(rc_values);
363  rf.command_manager_.set_new_offboard_command(offboard_command);
364  stepFirmware(40000);
365 
366  control_t output = rf.command_manager_.combined_control();
367  EXPECT_CLOSE(output.x.value, -0.5 * rf.params_.get_param_float(PARAM_RC_MAX_ROLL));
368  EXPECT_CLOSE(output.y.value, OFFBOARD_Y);
369  EXPECT_CLOSE(output.z.value, OFFBOARD_Z);
370  EXPECT_CLOSE(output.F.value, 0.0);
371 }
372 
373 TEST_F(CommandManagerTest, OffboardCommandMuxPitchDeviation)
374 {
375  stepFirmware(1100000); // Get past LAG_TIME
376  rf.params_.set_param_int(PARAM_RC_OVERRIDE_TAKE_MIN_THROTTLE, true);
377  rc_values[1] = 1750;
378  board.set_rc(rc_values);
379  rf.command_manager_.set_new_offboard_command(offboard_command);
380  stepFirmware(40000);
381 
382  control_t output = rf.command_manager_.combined_control();
383  EXPECT_CLOSE(output.x.value, OFFBOARD_X);
384  EXPECT_CLOSE(output.y.value, 0.5 * rf.params_.get_param_float(PARAM_RC_MAX_PITCH));
385  EXPECT_CLOSE(output.z.value, OFFBOARD_Z);
386  EXPECT_CLOSE(output.F.value, 0.0);
387 }
388 
389 TEST_F(CommandManagerTest, OffboardCommandMuxYawrateDeviation)
390 {
391  stepFirmware(1100000); // Get past LAG_TIME
392  rf.params_.set_param_int(PARAM_RC_OVERRIDE_TAKE_MIN_THROTTLE, true);
393  rc_values[3] = 1250;
394  board.set_rc(rc_values);
395  rf.command_manager_.set_new_offboard_command(offboard_command);
396  stepFirmware(40000);
397 
398  control_t output = rf.command_manager_.combined_control();
399  EXPECT_CLOSE(output.x.value, OFFBOARD_X);
400  EXPECT_CLOSE(output.y.value, OFFBOARD_Y);
401  EXPECT_CLOSE(output.z.value, -0.5 * rf.params_.get_param_float(PARAM_RC_MAX_YAWRATE));
402  EXPECT_CLOSE(output.F.value, 0.0);
403 }
404 
405 TEST_F(CommandManagerTest, OffboardCommandMuxLag)
406 {
407  stepFirmware(1100000); // Get past LAG_TIME
408  rf.params_.set_param_int(PARAM_RC_OVERRIDE_TAKE_MIN_THROTTLE, true);
409  rc_values[0] = 1250;
410  board.set_rc(rc_values);
411  setOffboard(offboard_command);
412  stepFirmware(40000);
413 
414  control_t output = rf.command_manager_.combined_control();
415  EXPECT_CLOSE(output.x.value, -0.5 * rf.params_.get_param_float(PARAM_RC_MAX_ROLL));
416 
417  rc_values[0] = 1500; // return stick to center
418  board.set_rc(rc_values);
419 
420  stepFirmware(500000);
421  setOffboard(offboard_command);
422  output = rf.command_manager_.combined_control();
423  EXPECT_CLOSE(output.x.value, 0.0); // lag
424 
425  stepFirmware(600000);
426  setOffboard(offboard_command);
427  output = rf.command_manager_.combined_control();
428  EXPECT_CLOSE(output.x.value, 0.0); // lag
429 
430  setOffboard(offboard_command);
431  stepFirmware(20000);
432  output = rf.command_manager_.combined_control();
433  EXPECT_CLOSE(output.x.value, OFFBOARD_X);
434 }
435 
436 TEST_F(CommandManagerTest, StaleOffboardCommand)
437 {
438  stepFirmware(1100000); // Get past LAG_TIME
439  rf.params_.set_param_int(PARAM_RC_OVERRIDE_TAKE_MIN_THROTTLE, true);
440  setOffboard(offboard_command);
441 
442  int timeout_us = rf.params_.get_param_int(PARAM_OFFBOARD_TIMEOUT) * 1000;
443  stepFirmware(timeout_us + 40000);
444 
445  control_t output = rf.command_manager_.combined_control();
446  EXPECT_CLOSE(output.x.value, 0.0);
447 }
448 
450 {
451  offboard_command.x.active = false;
452  stepFirmware(1000000);
453  setOffboard(offboard_command);
454  stepFirmware(30000);
455 
456  control_t output = rf.command_manager_.combined_control();
457  EXPECT_CLOSE(output.x.value, 0.0);
458  EXPECT_CLOSE(output.y.value, OFFBOARD_Y);
459  EXPECT_CLOSE(output.z.value, OFFBOARD_Z);
460  EXPECT_CLOSE(output.F.value, 0.0);
461 }
462 
464 {
465  offboard_command.x.type = RATE;
466  stepFirmware(1000000);
467  setOffboard(offboard_command);
468  stepFirmware(30000);
469 
470  control_t output = rf.command_manager_.combined_control();
471  EXPECT_EQ(output.x.type, RATE);
472  EXPECT_EQ(output.y.type, ANGLE);
473  EXPECT_EQ(output.z.type, RATE);
474  EXPECT_EQ(output.F.type, THROTTLE);
475 }
void stepFirmware(uint32_t us)
#define CHN_LOW
#define OFFBOARD_Y
void set_imu(float *acc, float *gyro, uint64_t time_us)
Definition: test_board.cpp:57
#define OFFBOARD_Z
void setOffboard(control_t &command)
#define OFFBOARD_F
uint64_t clock_micros() override
Definition: test_board.cpp:80
float get_param_float(uint16_t id) const
Get the value of a floating point parameter by id.
Definition: param.h:326
void run()
Main loop for the ROSflight autopilot flight stack.
Definition: rosflight.cpp:100
const State & state() const
void set_new_offboard_command(control_t new_offboard_command)
void clear_error(uint16_t error)
#define CHN_HIGH
TEST_F(CommandManagerTest, Default)
#define OFFBOARD_X
virtual uint32_t clock_millis()=0
void init()
Main initialization routine for the ROSflight autopilot flight stack.
Definition: rosflight.cpp:55
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:366
#define EXPECT_CLOSE(x, y)
Definition: test/common.h:52


rosflight_firmware
Author(s): Daniel Koch , James Jackson
autogenerated on Thu Apr 15 2021 05:07:46