From 1287aebae4646fff26cac7c4af0069f6b10dd061 Mon Sep 17 00:00:00 2001 From: Phil Tokumaru Date: Mon, 18 Nov 2024 17:18:15 -0800 Subject: [PATCH] Updated test fixture and some bug fixes as well --- boards/varmint_h7 | 2 +- include/board.h | 1 - include/rc.h | 3 +++ src/rc.cpp | 14 +++++++++++++- test/command_manager_test.cpp | 23 ++++++++++++++--------- test/state_machine_test.cpp | 11 +++++++---- test/test_board.cpp | 12 +----------- test/test_board.h | 3 --- 8 files changed, 39 insertions(+), 30 deletions(-) diff --git a/boards/varmint_h7 b/boards/varmint_h7 index 8591bcb7..4d3696a7 160000 --- a/boards/varmint_h7 +++ b/boards/varmint_h7 @@ -1 +1 @@ -Subproject commit 8591bcb7ca448ad80beee6379b87258a5576c5b2 +Subproject commit 4d3696a7e0f76cb8fff5db9e283bbb7d75668007 diff --git a/include/board.h b/include/board.h index 76903066..f8763916 100644 --- a/include/board.h +++ b/include/board.h @@ -100,7 +100,6 @@ class Board // RC virtual void rc_init(rc_type_t rc_type) = 0; - virtual bool rc_lost() = 0; virtual bool rc_read(rosflight_firmware::RcStruct * rc) = 0; // PWM diff --git a/include/rc.h b/include/rc.h index c9f240de..57032bbc 100644 --- a/include/rc.h +++ b/include/rc.h @@ -87,6 +87,9 @@ class RC : public ParamListenerInterface float read_chan(uint8_t chan); bool receive(void); + // _FAKE_ RC receive. Use set_chan for testing only! + uint16_t fake_rx(uint16_t *chan, uint16_t len, bool lost, bool failsafe); + private: RcStruct rc_={}; diff --git a/src/rc.cpp b/src/rc.cpp index cc4c3257..3557d014 100644 --- a/src/rc.cpp +++ b/src/rc.cpp @@ -175,7 +175,7 @@ bool RC::check_rc_lost() bool failsafe = false; // If the board reports that we have lost RC, tell the state manager - if (RF_.board_.rc_lost()) { + if(rc_.frameLost | rc_.failsafeActivated) { failsafe = true; } else { // go into failsafe if we get an invalid RC command for any channel @@ -285,4 +285,16 @@ bool RC::new_command() } } +uint16_t RC::fake_rx(uint16_t *chan, uint16_t len, bool lost, bool failsafe) { + len = (len(chan[i])-1000.)/1000.; + rc_.failsafeActivated = failsafe; + rc_.frameLost = lost; + rc_.nChan = RC_STRUCT_CHANNELS; + rc_.timestamp = RF_.board_.clock_micros(); + run(); + return len; +}; + + } // namespace rosflight_firmware diff --git a/test/command_manager_test.cpp b/test/command_manager_test.cpp index 5226f67b..6f4f1d07 100644 --- a/test/command_manager_test.cpp +++ b/test/command_manager_test.cpp @@ -27,6 +27,7 @@ class CommandManagerTest : public ::testing::Test ROSflight rf; float last_set_rc = 0; + bool rc_lost=false; uint16_t rc_values[8]; float max_roll, max_pitch, max_yawrate; @@ -44,8 +45,7 @@ class CommandManagerTest : public ::testing::Test void SetUp() override { rf.init(); - rf.state_manager_.clear_error( - rf.state_manager_.state().error_codes); // Clear All Errors to Start + rf.state_manager_.clear_error(rf.state_manager_.state().error_codes); // Clear All Errors to Start rf.params_.set_param_int(PARAM_CALIBRATE_GYRO_ON_ARM, false); for (int i = 0; i < 8; i++) { rc_values[i] = 1500; } @@ -72,7 +72,7 @@ class CommandManagerTest : public ::testing::Test while (board.clock_micros() < start_time_us + us) { if (board.clock_millis() > last_set_rc + 20) { last_set_rc = board.clock_millis(); - board.set_rc(rc_values); + rf.rc_.fake_rx(rc_values,8, rc_lost, false); // the false is for failsafe. } board.set_imu(dummy_acc, dummy_gyro, board.clock_micros() + 1000); rf.run(); @@ -240,7 +240,8 @@ TEST_F(CommandManagerTest, RCOutput) TEST_F(CommandManagerTest, LoseRCDisarmed) { - board.set_pwm_lost(true); + + rc_lost = true; stepFirmware(50000); control_t output = rf.command_manager_.combined_control(); @@ -262,9 +263,10 @@ TEST_F(CommandManagerTest, LoseRCDisarmed) TEST_F(CommandManagerTest, RegainRCDisarmed) { - board.set_pwm_lost(true); + rc_lost = true; stepFirmware(40000); - board.set_pwm_lost(false); + + rc_lost = false; stepFirmware(40000); EXPECT_EQ(rf.state_manager_.state().error, false); @@ -277,7 +279,8 @@ TEST_F(CommandManagerTest, LoseRCArmed) rf.state_manager_.set_event(StateManager::EVENT_REQUEST_ARM); EXPECT_EQ(rf.state_manager_.state().armed, true); - board.set_pwm_lost(true); + + rc_lost = true; stepFirmware(20000); control_t output = rf.command_manager_.combined_control(); @@ -302,9 +305,11 @@ TEST_F(CommandManagerTest, RegainRCArmed) stepFirmware(50000); rf.state_manager_.set_event(StateManager::EVENT_REQUEST_ARM); - board.set_pwm_lost(true); + + rc_lost = true; stepFirmware(20000); - board.set_pwm_lost(false); + + rc_lost = false; stepFirmware(20000); EXPECT_EQ(rf.state_manager_.state().armed, true); diff --git a/test/state_machine_test.cpp b/test/state_machine_test.cpp index 235f5f87..ab359eb6 100644 --- a/test/state_machine_test.cpp +++ b/test/state_machine_test.cpp @@ -32,7 +32,7 @@ class StateMachineTest : public ::testing::Test uint16_t rc_values[8]; for (int i = 0; i < 8; i++) { rc_values[i] = 1500; } rc_values[2] = 1000; - board.set_rc(rc_values); + rf.rc_.fake_rx(rc_values,8,false,false); stepFirmware(100000); } @@ -291,7 +291,8 @@ TEST_F(StateMachineTest, ArmIfThrottleLow) uint16_t rc_values[8]; for (int i = 0; i < 8; i++) { rc_values[i] = (i > 3) ? 1000 : 1500; } rc_values[2] = 1000; - board.set_rc(rc_values); + rf.rc_.fake_rx(rc_values,8,false,false); + step_firmware(rf, board, 100); rf.state_manager_.set_event(StateManager::EVENT_REQUEST_ARM); EXPECT_EQ(true, rf.state_manager_.state().armed); @@ -303,7 +304,8 @@ TEST_F(StateMachineTest, ArmIfThrottleHighWithMinThrottle) uint16_t rc_values[8]; for (int i = 0; i < 8; i++) { rc_values[i] = (i > 3) ? 1000 : 1500; } rc_values[2] = 1500; - board.set_rc(rc_values); + rf.rc_.fake_rx(rc_values,8,false,false); + step_firmware(rf, board, 100000); rf.state_manager_.set_event(StateManager::EVENT_REQUEST_ARM); @@ -317,7 +319,8 @@ TEST_F(StateMachineTest, DontArmIfThrottleHighWithoutMinThrottle) uint16_t rc_values[8]; for (int i = 0; i < 8; i++) { rc_values[i] = (i > 3) ? 1000 : 1500; } rc_values[2] = 1500; - board.set_rc(rc_values); + rf.rc_.fake_rx(rc_values,8,false,false); + step_firmware(rf, board, 100000); rf.state_manager_.set_event(StateManager::EVENT_REQUEST_ARM); diff --git a/test/test_board.cpp b/test/test_board.cpp index dbc7f792..1fcf2e57 100644 --- a/test/test_board.cpp +++ b/test/test_board.cpp @@ -36,16 +36,9 @@ namespace rosflight_firmware { -void testBoard::set_rc(uint16_t * values) -{ - for (int i = 0; i < 8; i++) { rc_values[i] = values[i]; } - new_rc_ = true; -} void testBoard::set_time(uint64_t time_us) { time_us_ = time_us; } -void testBoard::set_pwm_lost(bool lost) { rc_lost_ = lost; } - void testBoard::set_imu(float * acc, float * gyro, uint64_t time_us) { time_us_ = time_us; @@ -130,12 +123,9 @@ void testBoard::battery_current_set_multiplier(double multiplier) {} // PWM // TODO make these deal in normalized (-1 to 1 or 0 to 1) values (not pwm-specific) void testBoard::rc_init(rc_type_t rc_type) {(void)rc_type;} -bool testBoard::rc_lost() { return rc_lost_; } - bool testBoard::rc_read(rosflight_firmware::RcStruct * rc_struct) { - // ???? what goes here ??? - return true; + return false; } void testBoard::pwm_write(float * value, uint32_t channels) {} void testBoard::pwm_init(const float * rate, uint32_t channels) {} diff --git a/test/test_board.h b/test/test_board.h index 0834a160..158a2703 100644 --- a/test/test_board.h +++ b/test/test_board.h @@ -43,7 +43,6 @@ class testBoard : public Board uint16_t rc_values[8] = {1500, 1500, 1000, 1500, 1500, 1500, 1500, 1500}; bool new_rc_ = false; uint64_t time_us_ = 0; - bool rc_lost_ = false; float acc_[3] = {0, 0, 0}; float gyro_[3] = {0, 0, 0}; bool new_imu_ = false; @@ -95,7 +94,6 @@ class testBoard : public Board // RC void rc_init(rc_type_t rc_type) override; - bool rc_lost() override; bool rc_read(rosflight_firmware::RcStruct * rc) override; // PWM @@ -125,7 +123,6 @@ class testBoard : public Board void backup_memory_clear(); void set_imu(float * acc, float * gyro, uint64_t time_us); - void set_rc(uint16_t * values); void set_time(uint64_t time_us); void set_pwm_lost(bool lost); };