12 #define OFFBOARD_X -1.0 13 #define OFFBOARD_Y 0.5 14 #define OFFBOARD_Z -0.7 15 #define OFFBOARD_F 0.9 18 #define RC_X ((RC_X_PWM - 1500) / 500.0 * rf.params_.get_param_float(PARAM_RC_MAX_ROLL)) 29 uint16_t rc_values[8];
46 for (
int i = 0;
i < 8;
i++)
67 float dummy_acc[3] = {0, 0, -9.80665};
68 float dummy_gyro[3] = {0, 0, 0};
79 board.set_rc(rc_values);
82 control_t output = rf.command_manager_.combined_control();
99 board.set_rc(rc_values);
102 control_t output = rf.command_manager_.combined_control();
115 EXPECT_EQ(rf.state_manager_.state().armed,
false);
118 board.set_rc(rc_values);
119 stepFirmware(500000);
120 EXPECT_EQ(rf.state_manager_.state().armed,
122 stepFirmware(600000);
123 EXPECT_EQ(rf.state_manager_.state().armed,
true);
131 board.set_rc(rc_values);
132 stepFirmware(1100000);
133 EXPECT_EQ(rf.state_manager_.state().armed,
false);
139 EXPECT_EQ(rf.state_manager_.state().armed,
true);
142 board.set_rc(rc_values);
143 stepFirmware(1100000);
144 EXPECT_EQ(rf.state_manager_.state().armed,
false);
152 board.set_rc(rc_values);
154 EXPECT_EQ(rf.state_manager_.state().armed,
true);
162 board.set_rc(rc_values);
164 EXPECT_EQ(rf.state_manager_.state().armed,
false);
173 board.set_rc(rc_values);
175 EXPECT_EQ(rf.state_manager_.state().armed,
true);
179 stepFirmware(1100000);
180 EXPECT_EQ(rf.state_manager_.state().armed,
true);
189 board.set_rc(rc_values);
191 EXPECT_EQ(rf.state_manager_.state().armed,
true);
199 EXPECT_EQ(rf.state_manager_.state().armed,
true);
203 board.set_rc(rc_values);
205 EXPECT_EQ(rf.state_manager_.state().armed,
false);
210 board.set_rc(rc_values);
211 stepFirmware(600000);
214 control_t output = rf.command_manager_.combined_control();
231 board.set_rc(rc_values);
232 stepFirmware(600000);
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);
239 EXPECT_EQ(output.y.type,
ANGLE);
241 EXPECT_EQ(output.z.type,
RATE);
249 board.set_pwm_lost(
true);
252 control_t output = rf.command_manager_.combined_control();
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);
271 board.set_pwm_lost(
true);
273 board.set_pwm_lost(
false);
276 EXPECT_EQ(rf.state_manager_.state().error,
false);
282 board.set_rc(rc_values);
286 EXPECT_EQ(rf.state_manager_.state().armed,
true);
287 board.set_pwm_lost(
true);
290 control_t output = rf.command_manager_.combined_control();
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);
309 board.set_rc(rc_values);
313 board.set_pwm_lost(
true);
315 board.set_pwm_lost(
false);
318 EXPECT_EQ(rf.state_manager_.state().armed,
true);
319 EXPECT_EQ(rf.state_manager_.state().error,
false);
325 stepFirmware(1100000);
329 offboard_command.stamp_ms = board.clock_millis();
330 rf.command_manager_.set_new_offboard_command(offboard_command);
333 control_t output = rf.command_manager_.combined_control();
342 stepFirmware(1100000);
346 offboard_command.stamp_ms = board.clock_millis();
347 rf.command_manager_.set_new_offboard_command(offboard_command);
350 control_t output = rf.command_manager_.combined_control();
359 stepFirmware(1100000);
362 board.set_rc(rc_values);
363 rf.command_manager_.set_new_offboard_command(offboard_command);
366 control_t output = rf.command_manager_.combined_control();
375 stepFirmware(1100000);
378 board.set_rc(rc_values);
379 rf.command_manager_.set_new_offboard_command(offboard_command);
382 control_t output = rf.command_manager_.combined_control();
391 stepFirmware(1100000);
394 board.set_rc(rc_values);
395 rf.command_manager_.set_new_offboard_command(offboard_command);
398 control_t output = rf.command_manager_.combined_control();
407 stepFirmware(1100000);
410 board.set_rc(rc_values);
411 setOffboard(offboard_command);
414 control_t output = rf.command_manager_.combined_control();
418 board.set_rc(rc_values);
420 stepFirmware(500000);
421 setOffboard(offboard_command);
422 output = rf.command_manager_.combined_control();
425 stepFirmware(600000);
426 setOffboard(offboard_command);
427 output = rf.command_manager_.combined_control();
430 setOffboard(offboard_command);
432 output = rf.command_manager_.combined_control();
438 stepFirmware(1100000);
440 setOffboard(offboard_command);
443 stepFirmware(timeout_us + 40000);
445 control_t output = rf.command_manager_.combined_control();
451 offboard_command.x.active =
false;
452 stepFirmware(1000000);
453 setOffboard(offboard_command);
456 control_t output = rf.command_manager_.combined_control();
465 offboard_command.x.type =
RATE;
466 stepFirmware(1000000);
467 setOffboard(offboard_command);
470 control_t output = rf.command_manager_.combined_control();
void stepFirmware(uint32_t us)
void set_imu(float *acc, float *gyro, uint64_t time_us)
void setOffboard(control_t &command)
uint64_t clock_micros() override
float get_param_float(uint16_t id) const
Get the value of a floating point parameter by id.
void run()
Main loop for the ROSflight autopilot flight stack.
const State & state() const
void set_new_offboard_command(control_t new_offboard_command)
void clear_error(uint16_t error)
TEST_F(CommandManagerTest, Default)
virtual uint32_t clock_millis()=0
StateManager state_manager_
CommandManager command_manager_
void init()
Main initialization routine for the ROSflight autopilot flight stack.
bool set_param_int(uint16_t id, int32_t value)
Sets the value of a parameter by ID and calls the parameter change callback.
#define EXPECT_CLOSE(x, y)