state_machine_test.cpp
Go to the documentation of this file.
1 #include "common.h"
2 
3 #include "rosflight.h"
4 #include "mavlink.h"
5 #include "test_board.h"
6 
7 using namespace rosflight_firmware;
8 
9 TEST(state_machine_test, error_check)
10 {
11 
12  // Initialize the full firmware, so that the state_manager can do its thing
13  testBoard board;
14  Mavlink mavlink(board);
15  ROSflight rf(board, mavlink);
16 
17  // Initialize just a subset of the modules
18  // (some modules set errors when they initialize)
19  rf.board_.init_board();
20  rf.state_manager_.init();
21  rf.params_.init();
22 
23  // Should be in PREFLIGHT MODE
24  ASSERT_EQ(rf.state_manager_.state().armed, false);
25  ASSERT_EQ(rf.state_manager_.state().failsafe, false);
26  ASSERT_EQ(rf.state_manager_.state().error_codes, 0x00);
27  ASSERT_EQ(rf.state_manager_.state().error, false);
28 
29  // Try setting and clearing all the errors
30  for (int error = 0x0001; error <= StateManager::ERROR_UNCALIBRATED_IMU; error *= 2)
31  {
32  // set the error
34  ASSERT_EQ(rf.state_manager_.state().armed, false);
35  ASSERT_EQ(rf.state_manager_.state().failsafe, false);
36  ASSERT_EQ(rf.state_manager_.state().error_codes, error);
37  ASSERT_EQ(rf.state_manager_.state().error, true);
38 
39  // clear the error
41  ASSERT_EQ(rf.state_manager_.state().armed, false);
42  ASSERT_EQ(rf.state_manager_.state().failsafe, false);
44  ASSERT_EQ(rf.state_manager_.state().error, false);
45  }
46 
47  // Try combinations of errors
51  rf.state_manager_.set_error(error);
52  ASSERT_EQ(rf.state_manager_.state().armed, false);
53  ASSERT_EQ(rf.state_manager_.state().failsafe, false);
54  ASSERT_EQ(rf.state_manager_.state().error_codes, 0x32);
55  ASSERT_EQ(rf.state_manager_.state().error, true);
56 
57  // Add another error
59  ASSERT_EQ(rf.state_manager_.state().armed, false);
60  ASSERT_EQ(rf.state_manager_.state().failsafe, false);
61  ASSERT_EQ(rf.state_manager_.state().error_codes, 0x33);
62  ASSERT_EQ(rf.state_manager_.state().error, true);
63 
64  // Clear an error
66  ASSERT_EQ(rf.state_manager_.state().armed, false);
67  ASSERT_EQ(rf.state_manager_.state().failsafe, false);
68  ASSERT_EQ(rf.state_manager_.state().error_codes, 0x13);
69  ASSERT_EQ(rf.state_manager_.state().error, true);
70 
71  // Clear two errors
74  ASSERT_EQ(rf.state_manager_.state().armed, false);
75  ASSERT_EQ(rf.state_manager_.state().failsafe, false);
76  ASSERT_EQ(rf.state_manager_.state().error_codes, 0x01);
77  ASSERT_EQ(rf.state_manager_.state().error, true);
78 
79  // Clear final error
81  ASSERT_EQ(rf.state_manager_.state().armed, false);
82  ASSERT_EQ(rf.state_manager_.state().failsafe, false);
83  ASSERT_EQ(rf.state_manager_.state().error_codes, 0x00);
84  ASSERT_EQ(rf.state_manager_.state().error, false);
85 }
86 
87 TEST(state_machine_test, arm_check)
88 {
89  // Build the full firmware, so that the state_manager can do its thing
90  testBoard board;
91  Mavlink mavlink(board);
92  ROSflight rf(board, mavlink);
93 
94  // Initialize just a subset of the modules
95  // (some modules set errors when they initialize)
96  rf.board_.init_board();
97  rf.state_manager_.init();
98  rf.params_.init();
100 
101  // Should be in PREFLIGHT MODE
102  ASSERT_EQ(rf.state_manager_.state().armed, false);
103  ASSERT_EQ(rf.state_manager_.state().failsafe, false);
104  ASSERT_EQ(rf.state_manager_.state().error_codes, 0x00);
105  ASSERT_EQ(rf.state_manager_.state().error, false);
106 
107  //======================================================
108  // Basic arming test
109  //======================================================
110 
111  // Make sure that the parameter to calibrate before arm is off
113 
114  // Now add, an error, and then try to arm
117  ASSERT_EQ(rf.state_manager_.state().armed, false);
118  ASSERT_EQ(rf.state_manager_.state().failsafe, false);
120  ASSERT_EQ(rf.state_manager_.state().error, true);
121 
122 
123  // Clear the error, and then try again to arm
126  ASSERT_EQ(rf.state_manager_.state().armed, true);
127  ASSERT_EQ(rf.state_manager_.state().failsafe, false);
129  ASSERT_EQ(rf.state_manager_.state().error, false);
130 
131  // Disarm
133  ASSERT_EQ(rf.state_manager_.state().armed, false);
134  ASSERT_EQ(rf.state_manager_.state().failsafe, false);
136  ASSERT_EQ(rf.state_manager_.state().error, false);
137 
138 
139  //======================================================
140  // Preflight calibration arming test
141  //======================================================
142  // turn preflight calibration on
144 
145  // try to arm
147  // We shouldn't have armed yet
148  ASSERT_EQ(rf.state_manager_.state().armed, false);
149  ASSERT_EQ(rf.state_manager_.state().failsafe, false);
151  ASSERT_EQ(rf.state_manager_.state().error, false);
152 
153  // Set an error
155  // We shouldn't have armed yet
156  ASSERT_EQ(rf.state_manager_.state().armed, false);
157  ASSERT_EQ(rf.state_manager_.state().failsafe, false);
159  ASSERT_EQ(rf.state_manager_.state().error, true);
160 
161  // Clear the error, and try again
164  // Tell it that the calibration failed
166  // We shouldn't have armed yet
167  ASSERT_EQ(rf.state_manager_.state().armed, false);
168  ASSERT_EQ(rf.state_manager_.state().failsafe, false);
169  ASSERT_EQ(rf.state_manager_.state().error_codes, 0x00);
170  ASSERT_EQ(rf.state_manager_.state().error, false);
171 
172  // Try again, but this time the calibration succeeds
175  // We should be armed
176  ASSERT_EQ(rf.state_manager_.state().armed, true);
177  ASSERT_EQ(rf.state_manager_.state().failsafe, false);
178  ASSERT_EQ(rf.state_manager_.state().error_codes, 0x00);
179  ASSERT_EQ(rf.state_manager_.state().error, false);
180 
181  //======================================================
182  // Errors while armed test
183  //======================================================
184  // turn preflight calibration off
186 
187  // While armed, let's set some errors
189  // We should still be armed, but have an error
190  ASSERT_EQ(rf.state_manager_.state().armed, true);
191  ASSERT_EQ(rf.state_manager_.state().failsafe, false);
193  ASSERT_EQ(rf.state_manager_.state().error, true);
194 
195  // What happens if we disarm now?
197  ASSERT_EQ(rf.state_manager_.state().armed, false);
198  ASSERT_EQ(rf.state_manager_.state().failsafe, false);
200  ASSERT_EQ(rf.state_manager_.state().error, true);
201 
202  // Try to arm, it should fail
204  ASSERT_EQ(rf.state_manager_.state().armed, false);
205  ASSERT_EQ(rf.state_manager_.state().failsafe, false);
207  ASSERT_EQ(rf.state_manager_.state().error, true);
208 
209  // Clear the error and try again
211  ASSERT_EQ(rf.state_manager_.state().armed, false);
212  ASSERT_EQ(rf.state_manager_.state().failsafe, false);
213  ASSERT_EQ(rf.state_manager_.state().error_codes, 0x00);
214  ASSERT_EQ(rf.state_manager_.state().error, false);
216  ASSERT_EQ(rf.state_manager_.state().armed, true);
218  ASSERT_EQ(rf.state_manager_.state().armed, false);
219 }
220 
221 TEST(state_machine_test, arm_throttle_check)
222 {
223  testBoard board;
224  Mavlink mavlink(board);
225  ROSflight rf(board, mavlink);
226 
227  rf.init();
229  board.set_pwm_lost(false);
230 
231  uint16_t rc_values[8];
232  for (int i = 0; i < 8; i++)
233  {
234  rc_values[i] = (i > 3) ? 1000 : 1500;
235  }
236  rc_values[2] = 1000;
237  board.set_rc(rc_values);
238  step_firmware(rf, board, 100);
239 
240  // clear all errors
242 
243  // make sure we start out disarmed
244  ASSERT_EQ(false, rf.state_manager_.state().armed);
245 
246  // first, make sure we can arm if all is well
248  ASSERT_EQ(true, rf.state_manager_.state().armed);
249 
250  // now go back to disarmed to prepare for following tests
252  ASSERT_EQ(false, rf.state_manager_.state().armed);
253 
254  // for first set of tests, have min throttle enabled
256 
257  // make sure we can't arm with high RC throttle
258  rc_values[2] = 1500;
259  board.set_rc(rc_values);
260  step_firmware(rf, board, 100000);
261 
263  step_firmware(rf, board, 1200000);
264  ASSERT_EQ(false, rf.state_manager_.state().armed);
265 
266  // but make sure we can arm with low throttle with override switch off when min throttle is on
267  rc_values[2] = 1000;
268  rc_values[4] = 1000;
269  board.set_rc(rc_values);
270  step_firmware(rf, board, 100000);
271  ASSERT_EQ(false, rf.rc_.switch_on(RC::Switch::SWITCH_THROTTLE_OVERRIDE));
272 
274  step_firmware(rf, board, 1200000);
275  ASSERT_EQ(true, rf.state_manager_.state().armed);
276 
277  // disarm
279  step_firmware(rf, board, 1200000);
280  ASSERT_EQ(false, rf.state_manager_.state().armed);
281 
282  // now make sure that if min throttle is off, we can't arm with the override switch off
284 
286  step_firmware(rf, board, 1200000);
287  ASSERT_EQ(false, rf.state_manager_.state().armed);
288 
289  // but make sure we can arm with it on
290  rc_values[4] = 2000;
291  board.set_rc(rc_values);
292  step_firmware(rf, board, 100000);
293  ASSERT_EQ(true, rf.rc_.switch_on(RC::Switch::SWITCH_THROTTLE_OVERRIDE));
294 
296  step_firmware(rf, board, 1200000);
297  ASSERT_EQ(true, rf.state_manager_.state().armed);
298 }
299 
300 TEST(state_machine_test, failsafe_check)
301 {
302  // Build the full firmware, so that the state_manager can do its thing
303  testBoard board;
304  Mavlink mavlink(board);
305  ROSflight rf(board, mavlink);
306 
307  // Initialize just a subset of the modules
308  // (some modules set errors when they initialize)
309  rf.board_.init_board();
310  rf.state_manager_.init();
311  rf.params_.init();
312 
313  // Should be in PREFLIGHT MODE
314  ASSERT_EQ(rf.state_manager_.state().armed, false);
315  ASSERT_EQ(rf.state_manager_.state().failsafe, false);
316  ASSERT_EQ(rf.state_manager_.state().error_codes, 0x00);
317  ASSERT_EQ(rf.state_manager_.state().error, false);
318 
319  //======================================================
320  // RC Lost when disarmed - Should not be in failsafe, but in error
321  //======================================================
323  ASSERT_EQ(rf.state_manager_.state().armed, false);
324  ASSERT_EQ(rf.state_manager_.state().failsafe, false);
326  ASSERT_EQ(rf.state_manager_.state().error, true);
327 
328  // Try to arm
330  ASSERT_EQ(rf.state_manager_.state().armed, false);
331 
332  // Oh look! we have RC again
334  ASSERT_EQ(rf.state_manager_.state().armed, false);
335  ASSERT_EQ(rf.state_manager_.state().failsafe, false);
336  ASSERT_EQ(rf.state_manager_.state().error_codes, 0x00);
337  ASSERT_EQ(rf.state_manager_.state().error, false);
338 
339  //======================================================
340  // RC Lost when armed - should enter failsafe
341  //======================================================
342 
343  // Let's fly!
345  ASSERT_EQ(rf.state_manager_.state().armed, true);
346 
347  // Oh crap, we lost RC in the air
349  ASSERT_EQ(rf.state_manager_.state().armed, true);
350  ASSERT_EQ(rf.state_manager_.state().failsafe, true);
352  ASSERT_EQ(rf.state_manager_.state().error, true);
353 
354  // If by some magic we are able to disarm (perhaps via the computer)
356  ASSERT_EQ(rf.state_manager_.state().armed, false);
357  ASSERT_EQ(rf.state_manager_.state().failsafe, true);
359  ASSERT_EQ(rf.state_manager_.state().error, true);
360 
361  // Let's regain RC
363  ASSERT_EQ(rf.state_manager_.state().armed, false);
364  ASSERT_EQ(rf.state_manager_.state().failsafe, false);
365  ASSERT_EQ(rf.state_manager_.state().error_codes, 0x00);
366  ASSERT_EQ(rf.state_manager_.state().error, false);
367 
368  // Let's try this again!
370  ASSERT_EQ(rf.state_manager_.state().armed, true);
371 
372  // This is the worst receiver
374  ASSERT_EQ(rf.state_manager_.state().armed, true);
375  ASSERT_EQ(rf.state_manager_.state().failsafe, true);
376 
377  // It's going in and out!
379  ASSERT_EQ(rf.state_manager_.state().armed, true);
380  ASSERT_EQ(rf.state_manager_.state().failsafe, false);
381  ASSERT_EQ(rf.state_manager_.state().error_codes, 0x00);
382  ASSERT_EQ(rf.state_manager_.state().error, false);
383 }
384 
385 TEST(state_machine_test, corner_cases)
386 {
387  // Build the full firmware, so that the state_manager can do its thing
388  testBoard board;
389  Mavlink mavlink(board);
390  ROSflight rf(board, mavlink);
391 
392  // Initialize just a subset of the modules
393  // (some modules set errors when they initialize)
394  rf.board_.init_board();
395  rf.state_manager_.init();
396  rf.params_.init();
398 
399  // Should be in PREFLIGHT MODE
400  ASSERT_EQ(rf.state_manager_.state().armed, false);
401  ASSERT_EQ(rf.state_manager_.state().failsafe, false);
402  ASSERT_EQ(rf.state_manager_.state().error_codes, 0x00);
403  ASSERT_EQ(rf.state_manager_.state().error, false);
404 
405  //======================================================
406  // RC Lost when calibrating
407  //======================================================
408 
409  // turn preflight calibration on
411 
412  // Try to arm
414  ASSERT_EQ(rf.state_manager_.state().armed, false);
415 
416  // Lose RC during calibration - error, no failsafe
418  ASSERT_EQ(rf.state_manager_.state().armed, false);
419  ASSERT_EQ(rf.state_manager_.state().failsafe, false);
421  ASSERT_EQ(rf.state_manager_.state().error, true);
422 
423  // Regain RC, should be in preflight mode, no error
425  ASSERT_EQ(rf.state_manager_.state().armed, false);
426  ASSERT_EQ(rf.state_manager_.state().failsafe, false);
427  ASSERT_EQ(rf.state_manager_.state().error_codes, 0x00);
428  ASSERT_EQ(rf.state_manager_.state().error, false);
429 
431  ASSERT_EQ(rf.state_manager_.state().armed, false);
433  ASSERT_EQ(rf.state_manager_.state().armed, true);
434 }
virtual void init_board()=0
static volatile bool error
Definition: drv_i2c.c:92
bool switch_on(Switch channel)
Definition: rc.cpp:121
void set_rc(uint16_t *values)
Definition: test_board.cpp:40
TEST(state_machine_test, error_check)
const State & state() const
Definition: state_manager.h:82
void set_error(uint16_t error)
void clear_error(uint16_t error)
void step_firmware(rosflight_firmware::ROSflight &rf, rosflight_firmware::testBoard &board, uint32_t us)
Definition: common.cpp:30
void init()
Initialize parameter values.
Definition: param.cpp:90
void set_pwm_lost(bool lost)
Definition: test_board.cpp:53
void init()
Main initialization routine for the ROSflight autopilot flight stack.
Definition: rosflight.cpp:52
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
Author(s): Daniel Koch , James Jackson
autogenerated on Wed Jul 3 2019 19:59:25