From 7df589dc4cbbe3b1e353d73fc36964e64980535b Mon Sep 17 00:00:00 2001 From: Paul Nykiel Date: Mon, 16 Jan 2023 20:07:55 +0100 Subject: [PATCH 01/17] Initial test to run 4ms timer --- README.md | 11 +- Src/Application/application.c | 105 ++++++----- Src/Application/application.h | 13 +- Src/Application/mode_handler.c | 8 +- Src/Components/imu.c | 198 +++++++++++---------- Src/Components/imu.h | 1 + Src/Components/system.c | 8 +- Src/Components/system.h | 6 +- Tests/LowLevel/Application/application.cpp | 20 +-- Tests/LowLevel/Components/system.cpp | 8 +- 10 files changed, 210 insertions(+), 168 deletions(-) diff --git a/README.md b/README.md index 6168908..90a4cf6 100644 --- a/README.md +++ b/README.md @@ -4,9 +4,9 @@ * [Test Reports](https://toolboxplane.github.io/FlightControllerSoftware/report/) * [Coverage Reports](https://toolboxplane.github.io/FlightControllerSoftware/coverage/) -## Building and flashing +## Building and flashing -### Build +### Build To compile the firmware run: @@ -57,15 +57,16 @@ The error IDs are: | System | Timer Runtime | ◯◯◯🔴 ◯◯🔴◯ | | | Watchdog | ◯◯🔴◯ ◯◯🔴◯ | | | Brownout | ◯◯🔴🔴 ◯◯🔴◯ | -| ERROR_HANDLER_GROUP_IMU | Init timeout error | ◯◯◯🔴 ◯◯🔴🔴 | +| IMU | Init timeout error | ◯◯◯🔴 ◯◯🔴🔴 | | | Init self test error | ◯◯🔴◯ ◯◯🔴🔴 | | | Status error | ◯◯🔴🔴 ◯◯🔴🔴 | +| | Read timeout | ◯🔴◯◯ ◯◯🔴🔴 | | Flightcomputer | - | xxxx ◯🔴◯◯ | | Remote | - | xxxx ◯🔴◯🔴 | -| Mode Handler | No ERROR_HANDLER_GROUP_IMU Data | ◯◯◯🔴 ◯🔴🔴◯ | +| Mode Handler | No IMU Data | ◯◯◯🔴 ◯🔴🔴◯ | | | No FCP Data | ◯◯🔴◯ ◯🔴🔴◯ | | | No Remote Data | ◯◯🔴🔴 ◯🔴🔴◯ | -| ERROR_HANDLER_GROUP_BNO055 | Unexpected read success | ◯◯◯🔴 ◯🔴🔴🔴 | +| BNO055 | Unexpected read success | ◯◯◯🔴 ◯🔴🔴🔴 | | | Unexpected write success | ◯◯🔴◯ ◯🔴🔴🔴 | | | Read fail | ◯◯🔴🔴 ◯🔴🔴🔴 | | | Write fail | ◯🔴◯◯ ◯🔴🔴🔴 | diff --git a/Src/Application/application.c b/Src/Application/application.c index ca1df4e..3edbde4 100644 --- a/Src/Application/application.c +++ b/Src/Application/application.c @@ -26,55 +26,75 @@ enum { FLIGHT_COMPUTER_SEND_PERIOD = (uint16_t) (100 / 16.383) }; */ enum { SERVO_REMOTE_OFFSET = 1000U / 2 }; -static volatile uint8_t fcps_send_mux = 0; +typedef enum { + TIMER_STATE_MODE_HANDLER, + TIMER_STATE_CONTROLLER, + TIMER_STATE_ACTUATORS, + TIMER_STATE_SEND_FCP, + TIMER_STATE_EMPTY +} timer_state_t; + +static volatile uint8_t fcps_send_mux; +static volatile timer_state_t timer_state; static void timer_tick(void) { - imu_data_t imu_data; - remote_data_t remote_data; - flight_computer_set_point_t flightcomputer_setpoint; - mode_handler_mode_t mode = mode_handler_handle(&imu_data, &remote_data, &flightcomputer_setpoint); + static imu_data_t imu_data; + static remote_data_t remote_data; + static flight_computer_set_point_t flightcomputer_setpoint; + static mode_handler_mode_t mode; + static actuator_cmd_t actuator_cmd; - /* - * Calculate outputs - */ - actuator_cmd_t actuator_cmd; - switch (mode) { - case MODE_FLIGHTCOMPUTER: { - controller_result_t controller_result = - controller_update(&imu_data, flightcomputer_setpoint.roll, flightcomputer_setpoint.pitch); - actuator_cmd.elevon_left = controller_result.elevon_left; - actuator_cmd.elevon_right = controller_result.elevon_right; - actuator_cmd.motor = flightcomputer_setpoint.motor; + switch (timer_state) { + case TIMER_STATE_MODE_HANDLER: + mode = MODE_FAILSAFE; // mode_handler_handle(&imu_data, &remote_data, &flightcomputer_setpoint); + imu_data = imu_get_latest_data(); + timer_state = TIMER_STATE_CONTROLLER; break; - } - case MODE_REMOTE: - actuator_cmd.motor = remote_data.throttle_mixed; - actuator_cmd.elevon_left = remote_data.elevon_left_mixed - SERVO_REMOTE_OFFSET; - actuator_cmd.elevon_right = remote_data.elevon_right_mixed - SERVO_REMOTE_OFFSET; + case TIMER_STATE_CONTROLLER: + switch (mode) { + case MODE_FLIGHTCOMPUTER: { + controller_result_t controller_result = + controller_update(&imu_data, flightcomputer_setpoint.roll, flightcomputer_setpoint.pitch); + actuator_cmd.elevon_left = controller_result.elevon_left; + actuator_cmd.elevon_right = controller_result.elevon_right; + actuator_cmd.motor = flightcomputer_setpoint.motor; + break; + } + case MODE_REMOTE: + actuator_cmd.motor = remote_data.throttle_mixed; + actuator_cmd.elevon_left = remote_data.elevon_left_mixed - SERVO_REMOTE_OFFSET; + actuator_cmd.elevon_right = remote_data.elevon_right_mixed - SERVO_REMOTE_OFFSET; + break; + case MODE_STABILISED_FAILSAFE: { + controller_result_t controller_result = controller_update(&imu_data, 0, 0); + actuator_cmd.elevon_left = controller_result.elevon_left; + actuator_cmd.elevon_right = controller_result.elevon_right; + actuator_cmd.motor = 0; + break; + } + case MODE_FAILSAFE: + actuator_cmd.motor = 0; + actuator_cmd.elevon_left = 0; + actuator_cmd.elevon_right = 0; + break; + } + timer_state = TIMER_STATE_ACTUATORS; break; - case MODE_STABILISED_FAILSAFE: { - controller_result_t controller_result = controller_update(&imu_data, 0, 0); - actuator_cmd.elevon_left = controller_result.elevon_left; - actuator_cmd.elevon_right = controller_result.elevon_right; - actuator_cmd.motor = 0; + case TIMER_STATE_ACTUATORS: + actuators_set(&actuator_cmd); + timer_state = TIMER_STATE_SEND_FCP; break; - } - case MODE_FAILSAFE: - actuator_cmd.motor = 0; - actuator_cmd.elevon_left = 0; - actuator_cmd.elevon_right = 0; + case TIMER_STATE_SEND_FCP: + fcps_send_mux += 1; + if (fcps_send_mux >= FLIGHT_COMPUTER_SEND_PERIOD) { + flight_computer_send(&imu_data, &remote_data, &actuator_cmd); + fcps_send_mux = 0; + } + timer_state = TIMER_STATE_EMPTY; + break; + case TIMER_STATE_EMPTY: + timer_state = TIMER_STATE_MODE_HANDLER; break; - } - - actuators_set(&actuator_cmd); - - /* - * Send information to FCPS - */ - fcps_send_mux += 1; - if (fcps_send_mux >= FLIGHT_COMPUTER_SEND_PERIOD) { - flight_computer_send(&imu_data, &remote_data, &actuator_cmd); - fcps_send_mux = 0; } /* @@ -96,6 +116,7 @@ void application_init(void) { system_post_init(); fcps_send_mux = 0; + timer_state = TIMER_STATE_MODE_HANDLER; while (true) { system_reset_watchdog(); diff --git a/Src/Application/application.h b/Src/Application/application.h index 83e4b39..e3c65e9 100644 --- a/Src/Application/application.h +++ b/Src/Application/application.h @@ -25,9 +25,9 @@ * This function never returns but calls wdt_reset in an infinite loop, if this idle does not run for 30ms * (i.e. the system is continuously blocked by interrupts) a reset is performed. * - * The timer performs the following tasks: - * * Call the mode handler (::mode_handler_handle) to determine the current mode - * * Calculate the actuator commands depending on the mode: + * The timer runs a state machine which cycles through the following tasks: + * 1. Call the mode handler (::mode_handler_handle) to determine the current mode + * 2. Calculate the actuator commands depending on the mode: * * In flight-computer mode: call the controller (::controller_update) with the flight-computer set point and the * imu data and use the result as elevon commands, the motor command is taken directly from the flight-computer * set point @@ -36,8 +36,11 @@ * * In stabilised failsafe mode: call the controller with roll: 0, pitch: 0 as set point and the imu data * and use the result as elevon commands, the motor command is always 0 * * In failsafe mode: set all three commands to 0 - * * Pass the actuator commands to the actuators (::actuators_set) - * * Every FLIGHT_COMPUTER_SEND_PERIOD frame: pass the current data to ::flight-computer_send + * 3. Pass the actuator commands to the actuators (::actuators_set) + * 4. Every FLIGHT_COMPUTER_SEND_PERIOD time this state is reached: pass the current data to ::flight-computer_send + * 5. Empty task + * + * Every call to the timer the following tasks are performed: * * Call ::imu_start_sampling * */ diff --git a/Src/Application/mode_handler.c b/Src/Application/mode_handler.c index bb47113..073f1bb 100644 --- a/Src/Application/mode_handler.c +++ b/Src/Application/mode_handler.c @@ -10,9 +10,9 @@ #include "error_handler.h" enum { - IMU_TIMEOUT = (uint16_t) (100 / 16.384), - FLIGHTCOMPUTER_TIMEOUT = (uint16_t) ((2 * 100) / 16.384), - REMOTE_TIMEOUT = (uint16_t) (100 / 16.384), + IMU_TIMEOUT = (int) ((10 * 100) / (5 * 4.096)), + FLIGHTCOMPUTER_TIMEOUT = (int) ((2 * 100) / (5 * 4.096)), + REMOTE_TIMEOUT = (int) (100 / (5 * 4.096)), }; static uint8_t imu_timeout_counter; @@ -20,7 +20,7 @@ static uint8_t flightcomputer_timeout_counter; static uint8_t remote_timeout_counter; void mode_handler_init(void) { - imu_timeout_counter = IMU_TIMEOUT; + imu_timeout_counter = 0; // IMU_TIMEOUT; flightcomputer_timeout_counter = FLIGHTCOMPUTER_TIMEOUT; remote_timeout_counter = REMOTE_TIMEOUT; } diff --git a/Src/Components/imu.c b/Src/Components/imu.c index 27564d5..4d0b790 100644 --- a/Src/Components/imu.c +++ b/Src/Components/imu.c @@ -30,98 +30,52 @@ typedef enum { } bno_sampling_state_t; // NOLINTNEXTLINE(cppcoreguidelines-avoid-non-const-global-variables) only read if callback ready, behaviour checked -static volatile bno055_response_t init_response = write_fail; +static volatile bno055_response_t response = write_fail; // NOLINTNEXTLINE(cppcoreguidelines-avoid-non-const-global-variables) clear interaction in init, always properly set static volatile bool callback_ready = false; -static volatile bno_sampling_state_t bno_sampling_state = EUL; static volatile imu_data_t imu_datas[2]; -static bno055_calib_status_t calib_status; -static bno055_status_t bno_status; -static volatile uint8_t current_sample_state_id = 0; +static volatile bno055_calib_status_t calib_status; +static volatile bno055_status_t bno_status; + static volatile bool sampling_complete = false; -static void bno_init_callback(bno055_response_t response) { - init_response = response; - callback_ready = true; -} +static volatile bno_sampling_state_t current_sample_state; +static volatile uint8_t current_sample_state_id; -static void bno_sample_callback(bno055_response_t response) { - imu_data_t *imu_data = (imu_data_t *) (&imu_datas[current_sample_state_id]); - if (response == read_success) { - switch (bno_sampling_state) { - case EUL: - bno_sampling_state = GYR; - // imu_start_sampling(); - break; - case GYR: - bno_sampling_state = ACC; - // imu_start_sampling(); - break; - case ACC: - bno_sampling_state = STATUS; - // imu_start_sampling(); - break; - case STATUS: - if (bno_status != BNO055_STATUS_SENSOR_FUSION_ALGORITHM_RUNNING) { - error_handler_handle_warning(ERROR_HANDLER_GROUP_IMU, IMU_ERROR_STATUS); - imu_data->imu_ok = false; - } else { - imu_data->imu_ok = true; - } - bno_sampling_state = CALIB_STAT; - // imu_start_sampling(); - break; - case CALIB_STAT: - current_sample_state_id = 1 - current_sample_state_id; - if (calib_status.sys_status < SYS_CALIB_TRESH || calib_status.gyr_status < GYR_CALIB_TRESH || - calib_status.acc_status < ACC_CALIB_TRESH || calib_status.mag_status < MAG_CALIB_TRESH) { - imu_data->imu_ok = false; - } - sampling_complete = true; - bno_sampling_state = EUL; - break; - } - } else if (response != bus_over_run_error) { // Bus overrun just happens... - error_handler_handle_warning(ERROR_HANDLER_GROUP_BNO055, response + 1); - imu_data->imu_ok = false; - current_sample_state_id = 1 - current_sample_state_id; - bno_sampling_state = EUL; - } +static void bno_callback(bno055_response_t response_) { + response = response_; + callback_ready = true; } void imu_init(void) { - imu_datas[0].imu_ok = false; - imu_datas[1].imu_ok = false; - - current_sample_state_id = 0; - + // Initialize physical connection bno055_init(); // Set to config mode callback_ready = false; - bno055_write_opr_mode(BNO055_OPR_MODE_CONFIG_MODE, bno_init_callback); + bno055_write_opr_mode(BNO055_OPR_MODE_CONFIG_MODE, bno_callback); _delay_ms(INIT_RESPONSE_TIMEOUT_MS); if (!callback_ready) { error_handler_handle_error(ERROR_HANDLER_GROUP_IMU, IMU_ERROR_INIT_TIMEOUT); return; } - if (init_response != write_success) { - error_handler_handle_error(ERROR_HANDLER_GROUP_BNO055, init_response + 1); + if (response != write_success) { + error_handler_handle_error(ERROR_HANDLER_GROUP_BNO055, response + 1); return; } // Run Self Test callback_ready = false; bno055_self_test_result_t self_test_result; - bno055_read_self_test(&self_test_result, bno_init_callback); + bno055_read_self_test(&self_test_result, bno_callback); _delay_ms(INIT_RESPONSE_TIMEOUT_MS); if (!callback_ready) { error_handler_handle_error(ERROR_HANDLER_GROUP_IMU, IMU_ERROR_INIT_TIMEOUT); return; } - if (init_response != read_success) { - error_handler_handle_error(ERROR_HANDLER_GROUP_BNO055, init_response + 1); + if (response != read_success) { + error_handler_handle_error(ERROR_HANDLER_GROUP_BNO055, response + 1); return; } if (!self_test_result.acc_passed || !self_test_result.gyr_passed || !self_test_result.mag_passed || @@ -134,14 +88,14 @@ void imu_init(void) { callback_ready = false; bno055_write_unit_selection(BNO055_UNIT_SEL_ACC_MPS2, BNO055_UNIT_SEL_ANGULAR_RATE_DPS, BNO055_UNIT_SEL_EULER_ANGLES_DEGREES, BNO055_UNIT_SEL_TEMPERATURE_CELSIUS, - BNO055_UNIT_SEL_ORIENTATION_DEF_WINDOWS, bno_init_callback); + BNO055_UNIT_SEL_ORIENTATION_DEF_WINDOWS, bno_callback); _delay_ms(INIT_RESPONSE_TIMEOUT_MS); if (!callback_ready) { error_handler_handle_error(ERROR_HANDLER_GROUP_IMU, IMU_ERROR_INIT_TIMEOUT); return; } - if (init_response != write_success) { - error_handler_handle_error(ERROR_HANDLER_GROUP_BNO055, init_response + 1); + if (response != write_success) { + error_handler_handle_error(ERROR_HANDLER_GROUP_BNO055, response + 1); return; } @@ -156,64 +110,126 @@ void imu_init(void) { * - IMU Z: Negative Yaw (-Z) */ callback_ready = false; - bno055_write_remap_axis(BNO055_AXIS_REMAP_Y_AXIS, BNO055_AXIS_REMAP_X_AXIS, BNO055_AXIS_REMAP_Z_AXIS, bno_init_callback); + bno055_write_remap_axis(BNO055_AXIS_REMAP_Y_AXIS, BNO055_AXIS_REMAP_X_AXIS, BNO055_AXIS_REMAP_Z_AXIS, bno_callback); _delay_ms(INIT_RESPONSE_TIMEOUT_MS); if (!callback_ready) { error_handler_handle_error(ERROR_HANDLER_GROUP_IMU, IMU_ERROR_INIT_TIMEOUT); return; } - if (init_response != write_success) { - error_handler_handle_error(ERROR_HANDLER_GROUP_BNO055, init_response + 1); + if (response != write_success) { + error_handler_handle_error(ERROR_HANDLER_GROUP_BNO055, response + 1); return; } callback_ready = false; bno055_write_remap_axis_sign(BNO055_AXIS_REMAP_SIGN_POSITIVE, BNO055_AXIS_REMAP_SIGN_POSITIVE, - BNO055_AXIS_REMAP_SIGN_NEGATIVE, bno_init_callback); + BNO055_AXIS_REMAP_SIGN_NEGATIVE, bno_callback); _delay_ms(INIT_RESPONSE_TIMEOUT_MS); if (!callback_ready) { error_handler_handle_error(ERROR_HANDLER_GROUP_IMU, IMU_ERROR_INIT_TIMEOUT); return; } - if (init_response != write_success) { - error_handler_handle_error(ERROR_HANDLER_GROUP_BNO055, init_response + 1); + if (response != write_success) { + error_handler_handle_error(ERROR_HANDLER_GROUP_BNO055, response + 1); return; } // Set to NDOF-FMC-OFF callback_ready = false; - bno055_write_opr_mode(BNO055_OPR_MODE_NDOF_FMC_OFF, bno_init_callback); + bno055_write_opr_mode(BNO055_OPR_MODE_NDOF_FMC_OFF, bno_callback); _delay_ms(INIT_RESPONSE_TIMEOUT_MS); if (!callback_ready) { error_handler_handle_error(ERROR_HANDLER_GROUP_IMU, IMU_ERROR_INIT_TIMEOUT); return; } - if (init_response != write_success) { - error_handler_handle_error(ERROR_HANDLER_GROUP_BNO055, init_response + 1); + if (response != write_success) { + error_handler_handle_error(ERROR_HANDLER_GROUP_BNO055, response + 1); return; } - bno_sampling_state = EUL; + // Preparation for sampling + imu_datas[0].imu_ok = false; + imu_datas[1].imu_ok = false; + + // Set the state as if at the end of successful measurement loop + callback_ready = true; + response = read_success; + current_sample_state_id = 1; + current_sample_state = CALIB_STAT; + } void imu_start_sampling(void) { + if (!callback_ready) { + //error_handler_handle_warning(ERROR_HANDLER_GROUP_IMU, IMU_ERROR_READ_TIMEOUT); + return; + } + imu_data_t *imu_data = (imu_data_t *) (&imu_datas[current_sample_state_id]); - switch (bno_sampling_state) { - case EUL: - bno055_read_eul_xyz_2_mul_16(&imu_data->heading_mul_16, bno_sample_callback); - break; - case GYR: - bno055_read_gyr_xyz_mul_16(&imu_data->d_heading_mul_16, bno_sample_callback); - break; - case ACC: - bno055_read_acc_xyz_mul_100(&imu_data->acc_x_mul_100, bno_sample_callback); - break; - case STATUS: - bno055_read_system_status(&bno_status, bno_sample_callback); - break; - case CALIB_STAT: - bno055_read_calib_status(&calib_status, bno_sample_callback); - break; + callback_ready = false; + + if (response != read_success) { + if (response != bus_over_run_error) { + error_handler_handle_warning(ERROR_HANDLER_GROUP_BNO055, response + 1); + } + + switch (current_sample_state) { + case EUL: + bno055_read_eul_xyz_2_mul_16(&imu_data->heading_mul_16, bno_callback); + break; + case GYR: + bno055_read_gyr_xyz_mul_16(&imu_data->d_heading_mul_16, bno_callback); + break; + case ACC: + bno055_read_acc_xyz_mul_100(&imu_data->acc_x_mul_100, bno_callback); + break; + case STATUS: + bno055_read_system_status(&bno_status, bno_callback); + break; + case CALIB_STAT: + bno055_read_calib_status(&calib_status, bno_callback); + break; + } + } else { + switch (current_sample_state) { + case EUL: + current_sample_state = GYR; + bno055_read_gyr_xyz_mul_16(&imu_data->d_heading_mul_16, bno_callback); + break; + case GYR: + current_sample_state = ACC; + bno055_read_acc_xyz_mul_100(&imu_data->acc_x_mul_100, bno_callback); + break; + case ACC: + current_sample_state = STATUS; + bno055_read_system_status(&bno_status, bno_callback); + break; + case STATUS: + // Status post-processing + if (bno_status != BNO055_STATUS_SENSOR_FUSION_ALGORITHM_RUNNING) { + error_handler_handle_warning(ERROR_HANDLER_GROUP_IMU, IMU_ERROR_STATUS); + imu_data->imu_ok = false; + } else { + imu_data->imu_ok = true; + } + + current_sample_state = CALIB_STAT; + bno055_read_calib_status(&calib_status, bno_callback); + break; + case CALIB_STAT: + // Calib-stat post-processing + if (calib_status.sys_status < SYS_CALIB_TRESH || calib_status.gyr_status < GYR_CALIB_TRESH || + calib_status.acc_status < ACC_CALIB_TRESH || calib_status.mag_status < MAG_CALIB_TRESH) { + imu_data->imu_ok = false; + } + current_sample_state_id = 1 - current_sample_state_id; + sampling_complete = true; + imu_data = (imu_data_t *) (&imu_datas[current_sample_state_id]); + + current_sample_state = EUL; + bno055_read_eul_xyz_2_mul_16(&imu_data->heading_mul_16, bno_callback); + break; + } } } diff --git a/Src/Components/imu.h b/Src/Components/imu.h index bdfb671..8d59d8a 100644 --- a/Src/Components/imu.h +++ b/Src/Components/imu.h @@ -43,6 +43,7 @@ typedef enum { IMU_ERROR_INIT_TIMEOUT = 1, IMU_ERROR_INIT_SELF_TEST = 2, IMU_ERROR_STATUS = 3, + IMU_ERROR_READ_TIMEOUT = 4 } imu_error_t; /** diff --git a/Src/Components/system.c b/Src/Components/system.c index 997d663..972c599 100644 --- a/Src/Components/system.c +++ b/Src/Components/system.c @@ -13,9 +13,9 @@ #include #include -enum { MAX_TIME_SLOT_USAGE = (uint8_t) (12 / 16.384 * 255) }; +enum { MAX_TIME_SLOT_USAGE = (uint8_t) (3.5 / 4.096 * 255) }; -static system_timer_16_384ms_callback callback; +static system_timer_4_096ms_callback_t callback; static void internal_timer_callback(void) { callback(); @@ -25,7 +25,7 @@ static void internal_timer_callback(void) { } } -void system_pre_init(system_timer_16_384ms_callback timer_callback) { +void system_pre_init(system_timer_4_096ms_callback_t timer_callback) { cli(); callback = timer_callback; @@ -42,7 +42,7 @@ void system_pre_init(system_timer_16_384ms_callback timer_callback) { void system_post_init(void) { // Runs at 16.384ms interval, the BNO055 provides data at 100Hz, the output can be updated at 50Hz - timer_8bit_init(prescaler_1024, &internal_timer_callback); + timer_8bit_init(prescaler_256, &internal_timer_callback); wdt_enable(WDTO_30MS); } diff --git a/Src/Components/system.h b/Src/Components/system.h index d6fc36d..5f3fe19 100644 --- a/Src/Components/system.h +++ b/Src/Components/system.h @@ -16,7 +16,7 @@ typedef enum { SYSTEM_ERROR_TIMER_RUNTIME = 1, SYSTEM_ERROR_WATCHDOG = 2, SYSTEM /** * The type of function to be called by the system component. */ -typedef void (*system_timer_16_384ms_callback)(void); +typedef void (*system_timer_4_096ms_callback_t)(void); /** * @brief Run initial system setup. @@ -29,13 +29,13 @@ typedef void (*system_timer_16_384ms_callback)(void); * * @param timer_callback the function to be called by the timer. */ -void system_pre_init(system_timer_16_384ms_callback timer_callback); +void system_pre_init(system_timer_4_096ms_callback_t timer_callback); /** * @brief Run system setup after component initialization. * * The post-initialization consists of the following task: - * * Start the timer with a period of 16.384ms + * * Start the timer with a period of 4.096ms * * Set the watchdog to 30ms */ void system_post_init(void); diff --git a/Tests/LowLevel/Application/application.cpp b/Tests/LowLevel/Application/application.cpp index 7322893..201d3e6 100644 --- a/Tests/LowLevel/Application/application.cpp +++ b/Tests/LowLevel/Application/application.cpp @@ -46,9 +46,9 @@ TEST(TEST_NAME, timer_mode_fcp) { auto systemHandle = mock::system.getHandle(); auto controllerHandle = mock::controller.getHandle(); - system_timer_16_384ms_callback timer_callback = nullptr; + system_timer_4_096ms_callback_t timer_callback = nullptr; systemHandle.overrideFunc( - [&timer_callback](system_timer_16_384ms_callback callback) { timer_callback = callback; }); + [&timer_callback](system_timer_4_096ms_callback_t callback) { timer_callback = callback; }); systemHandle.overrideFunc( []() { throw std::runtime_error{"EXCEPTION TO BREAK LOOP FOR TESTS"}; }); @@ -127,9 +127,9 @@ TEST(TEST_NAME, timer_mode_remote) { auto systemHandle = mock::system.getHandle(); auto controllerHandle = mock::controller.getHandle(); - system_timer_16_384ms_callback timer_callback = nullptr; + system_timer_4_096ms_callback_t timer_callback = nullptr; systemHandle.overrideFunc( - [&timer_callback](system_timer_16_384ms_callback callback) { timer_callback = callback; }); + [&timer_callback](system_timer_4_096ms_callback_t callback) { timer_callback = callback; }); systemHandle.overrideFunc( []() { throw std::runtime_error{"EXCEPTION TO BREAK LOOP FOR TESTS"}; }); @@ -190,9 +190,9 @@ TEST(TEST_NAME, timer_mode_stabilised_failsafe) { auto systemHandle = mock::system.getHandle(); auto controllerHandle = mock::controller.getHandle(); - system_timer_16_384ms_callback timer_callback = nullptr; + system_timer_4_096ms_callback_t timer_callback = nullptr; systemHandle.overrideFunc( - [&timer_callback](system_timer_16_384ms_callback callback) { timer_callback = callback; }); + [&timer_callback](system_timer_4_096ms_callback_t callback) { timer_callback = callback; }); systemHandle.overrideFunc( []() { throw std::runtime_error{"EXCEPTION TO BREAK LOOP FOR TESTS"}; }); @@ -271,9 +271,9 @@ TEST(TEST_NAME, timer_mode_failsafe) { auto systemHandle = mock::system.getHandle(); auto controllerHandle = mock::controller.getHandle(); - system_timer_16_384ms_callback timer_callback = nullptr; + system_timer_4_096ms_callback_t timer_callback = nullptr; systemHandle.overrideFunc( - [&timer_callback](system_timer_16_384ms_callback callback) { timer_callback = callback; }); + [&timer_callback](system_timer_4_096ms_callback_t callback) { timer_callback = callback; }); systemHandle.overrideFunc( []() { throw std::runtime_error{"EXCEPTION TO BREAK LOOP FOR TESTS"}; }); @@ -335,9 +335,9 @@ TEST(TEST_NAME, timer_send_period) { auto systemHandle = mock::system.getHandle(); auto controllerHandle = mock::controller.getHandle(); - system_timer_16_384ms_callback timer_callback = nullptr; + system_timer_4_096ms_callback_t timer_callback = nullptr; systemHandle.overrideFunc( - [&timer_callback](system_timer_16_384ms_callback callback) { timer_callback = callback; }); + [&timer_callback](system_timer_4_096ms_callback_t callback) { timer_callback = callback; }); systemHandle.overrideFunc( []() { throw std::runtime_error{"EXCEPTION TO BREAK LOOP FOR TESTS"}; }); diff --git a/Tests/LowLevel/Components/system.cpp b/Tests/LowLevel/Components/system.cpp index f24852e..cd2a4fd 100644 --- a/Tests/LowLevel/Components/system.cpp +++ b/Tests/LowLevel/Components/system.cpp @@ -78,7 +78,7 @@ TEST(TEST_NAME, timer_runtime_0) { timer_8bit_callback_t internalCallback = nullptr; timerHandle.overrideFunc( [&internalCallback](timer_8bit_clock_option_t /*clock_option*/, - system_timer_16_384ms_callback timerCallback) { internalCallback = timerCallback; }); + system_timer_4_096ms_callback_t timerCallback) { internalCallback = timerCallback; }); timerHandle.overrideFunc([]() { return 0; }); system_pre_init(timer_callback); @@ -104,7 +104,7 @@ TEST(TEST_NAME, timer_runtime_max) { timer_8bit_callback_t internalCallback = nullptr; timerHandle.overrideFunc( [&internalCallback](timer_8bit_clock_option_t /*clock_option*/, - system_timer_16_384ms_callback timerCallback) { internalCallback = timerCallback; }); + system_timer_4_096ms_callback_t timerCallback) { internalCallback = timerCallback; }); timerHandle.overrideFunc([]() { return 255; }); system_pre_init(timer_callback); @@ -130,7 +130,7 @@ TEST(TEST_NAME, timer_runtime_under_limit) { timer_8bit_callback_t internalCallback = nullptr; timerHandle.overrideFunc( [&internalCallback](timer_8bit_clock_option_t /*clock_option*/, - system_timer_16_384ms_callback timerCallback) { internalCallback = timerCallback; }); + system_timer_4_096ms_callback_t timerCallback) { internalCallback = timerCallback; }); // 12/16,384*255=186.767578125 timerHandle.overrideFunc([]() { return 186; }); @@ -157,7 +157,7 @@ TEST(TEST_NAME, timer_runtime_over_limit) { timer_8bit_callback_t internalCallback = nullptr; timerHandle.overrideFunc( [&internalCallback](timer_8bit_clock_option_t /*clock_option*/, - system_timer_16_384ms_callback timerCallback) { internalCallback = timerCallback; }); + system_timer_4_096ms_callback_t timerCallback) { internalCallback = timerCallback; }); // 12/16,384*255=186.767578125 timerHandle.overrideFunc([]() { return 187; }); From ce75a901bf71260fef4aeaed2567b2b532c4b3ab Mon Sep 17 00:00:00 2001 From: Paul Nykiel Date: Wed, 18 Jan 2023 19:42:19 +0100 Subject: [PATCH 02/17] Reverted timer changes --- Src/Application/application.c | 108 +++++++++------------ Src/Application/application.h | 13 +-- Src/Application/mode_handler.c | 8 +- Src/Components/imu.c | 2 +- Src/Components/imu.h | 27 ++++-- Src/Components/system.c | 8 +- Src/Components/system.h | 6 +- Tests/LowLevel/Application/application.cpp | 20 ++-- Tests/LowLevel/Components/system.cpp | 8 +- 9 files changed, 95 insertions(+), 105 deletions(-) diff --git a/Src/Application/application.c b/Src/Application/application.c index 3edbde4..62efcce 100644 --- a/Src/Application/application.c +++ b/Src/Application/application.c @@ -26,81 +26,61 @@ enum { FLIGHT_COMPUTER_SEND_PERIOD = (uint16_t) (100 / 16.383) }; */ enum { SERVO_REMOTE_OFFSET = 1000U / 2 }; -typedef enum { - TIMER_STATE_MODE_HANDLER, - TIMER_STATE_CONTROLLER, - TIMER_STATE_ACTUATORS, - TIMER_STATE_SEND_FCP, - TIMER_STATE_EMPTY -} timer_state_t; - -static volatile uint8_t fcps_send_mux; -static volatile timer_state_t timer_state; +static volatile uint8_t fcps_send_mux = 0; static void timer_tick(void) { - static imu_data_t imu_data; - static remote_data_t remote_data; - static flight_computer_set_point_t flightcomputer_setpoint; - static mode_handler_mode_t mode; - static actuator_cmd_t actuator_cmd; + imu_data_t imu_data; + remote_data_t remote_data; + flight_computer_set_point_t flightcomputer_setpoint; + mode_handler_mode_t mode = mode_handler_handle(&imu_data, &remote_data, &flightcomputer_setpoint); - switch (timer_state) { - case TIMER_STATE_MODE_HANDLER: - mode = MODE_FAILSAFE; // mode_handler_handle(&imu_data, &remote_data, &flightcomputer_setpoint); - imu_data = imu_get_latest_data(); - timer_state = TIMER_STATE_CONTROLLER; - break; - case TIMER_STATE_CONTROLLER: - switch (mode) { - case MODE_FLIGHTCOMPUTER: { - controller_result_t controller_result = - controller_update(&imu_data, flightcomputer_setpoint.roll, flightcomputer_setpoint.pitch); - actuator_cmd.elevon_left = controller_result.elevon_left; - actuator_cmd.elevon_right = controller_result.elevon_right; - actuator_cmd.motor = flightcomputer_setpoint.motor; - break; - } - case MODE_REMOTE: - actuator_cmd.motor = remote_data.throttle_mixed; - actuator_cmd.elevon_left = remote_data.elevon_left_mixed - SERVO_REMOTE_OFFSET; - actuator_cmd.elevon_right = remote_data.elevon_right_mixed - SERVO_REMOTE_OFFSET; - break; - case MODE_STABILISED_FAILSAFE: { - controller_result_t controller_result = controller_update(&imu_data, 0, 0); - actuator_cmd.elevon_left = controller_result.elevon_left; - actuator_cmd.elevon_right = controller_result.elevon_right; - actuator_cmd.motor = 0; - break; - } - case MODE_FAILSAFE: - actuator_cmd.motor = 0; - actuator_cmd.elevon_left = 0; - actuator_cmd.elevon_right = 0; - break; - } - timer_state = TIMER_STATE_ACTUATORS; + /* + * Calculate outputs + */ + actuator_cmd_t actuator_cmd; + switch (mode) { + case MODE_FLIGHTCOMPUTER: { + controller_result_t controller_result = + controller_update(&imu_data, flightcomputer_setpoint.roll, flightcomputer_setpoint.pitch); + actuator_cmd.elevon_left = controller_result.elevon_left; + actuator_cmd.elevon_right = controller_result.elevon_right; + actuator_cmd.motor = flightcomputer_setpoint.motor; break; - case TIMER_STATE_ACTUATORS: - actuators_set(&actuator_cmd); - timer_state = TIMER_STATE_SEND_FCP; + } + case MODE_REMOTE: + actuator_cmd.motor = remote_data.throttle_mixed; + actuator_cmd.elevon_left = remote_data.elevon_left_mixed - SERVO_REMOTE_OFFSET; + actuator_cmd.elevon_right = remote_data.elevon_right_mixed - SERVO_REMOTE_OFFSET; break; - case TIMER_STATE_SEND_FCP: - fcps_send_mux += 1; - if (fcps_send_mux >= FLIGHT_COMPUTER_SEND_PERIOD) { - flight_computer_send(&imu_data, &remote_data, &actuator_cmd); - fcps_send_mux = 0; - } - timer_state = TIMER_STATE_EMPTY; + case MODE_STABILISED_FAILSAFE: { + controller_result_t controller_result = controller_update(&imu_data, 0, 0); + actuator_cmd.elevon_left = controller_result.elevon_left; + actuator_cmd.elevon_right = controller_result.elevon_right; + actuator_cmd.motor = 0; break; - case TIMER_STATE_EMPTY: - timer_state = TIMER_STATE_MODE_HANDLER; + } + case MODE_FAILSAFE: + actuator_cmd.motor = 0; + actuator_cmd.elevon_left = 0; + actuator_cmd.elevon_right = 0; break; } + actuators_set(&actuator_cmd); + + /* + * Send information to FCPS + */ + fcps_send_mux += 1; + if (fcps_send_mux >= FLIGHT_COMPUTER_SEND_PERIOD) { + flight_computer_send(&imu_data, &remote_data, &actuator_cmd); + fcps_send_mux = 0; + } + /* * Read the next IMU data */ - imu_start_sampling(); + //imu_start_sampling(); } void application_init(void) { @@ -116,9 +96,9 @@ void application_init(void) { system_post_init(); fcps_send_mux = 0; - timer_state = TIMER_STATE_MODE_HANDLER; while (true) { system_reset_watchdog(); + imu_start_sampling(); } } diff --git a/Src/Application/application.h b/Src/Application/application.h index e3c65e9..83e4b39 100644 --- a/Src/Application/application.h +++ b/Src/Application/application.h @@ -25,9 +25,9 @@ * This function never returns but calls wdt_reset in an infinite loop, if this idle does not run for 30ms * (i.e. the system is continuously blocked by interrupts) a reset is performed. * - * The timer runs a state machine which cycles through the following tasks: - * 1. Call the mode handler (::mode_handler_handle) to determine the current mode - * 2. Calculate the actuator commands depending on the mode: + * The timer performs the following tasks: + * * Call the mode handler (::mode_handler_handle) to determine the current mode + * * Calculate the actuator commands depending on the mode: * * In flight-computer mode: call the controller (::controller_update) with the flight-computer set point and the * imu data and use the result as elevon commands, the motor command is taken directly from the flight-computer * set point @@ -36,11 +36,8 @@ * * In stabilised failsafe mode: call the controller with roll: 0, pitch: 0 as set point and the imu data * and use the result as elevon commands, the motor command is always 0 * * In failsafe mode: set all three commands to 0 - * 3. Pass the actuator commands to the actuators (::actuators_set) - * 4. Every FLIGHT_COMPUTER_SEND_PERIOD time this state is reached: pass the current data to ::flight-computer_send - * 5. Empty task - * - * Every call to the timer the following tasks are performed: + * * Pass the actuator commands to the actuators (::actuators_set) + * * Every FLIGHT_COMPUTER_SEND_PERIOD frame: pass the current data to ::flight-computer_send * * Call ::imu_start_sampling * */ diff --git a/Src/Application/mode_handler.c b/Src/Application/mode_handler.c index 073f1bb..bb47113 100644 --- a/Src/Application/mode_handler.c +++ b/Src/Application/mode_handler.c @@ -10,9 +10,9 @@ #include "error_handler.h" enum { - IMU_TIMEOUT = (int) ((10 * 100) / (5 * 4.096)), - FLIGHTCOMPUTER_TIMEOUT = (int) ((2 * 100) / (5 * 4.096)), - REMOTE_TIMEOUT = (int) (100 / (5 * 4.096)), + IMU_TIMEOUT = (uint16_t) (100 / 16.384), + FLIGHTCOMPUTER_TIMEOUT = (uint16_t) ((2 * 100) / 16.384), + REMOTE_TIMEOUT = (uint16_t) (100 / 16.384), }; static uint8_t imu_timeout_counter; @@ -20,7 +20,7 @@ static uint8_t flightcomputer_timeout_counter; static uint8_t remote_timeout_counter; void mode_handler_init(void) { - imu_timeout_counter = 0; // IMU_TIMEOUT; + imu_timeout_counter = IMU_TIMEOUT; flightcomputer_timeout_counter = FLIGHTCOMPUTER_TIMEOUT; remote_timeout_counter = REMOTE_TIMEOUT; } diff --git a/Src/Components/imu.c b/Src/Components/imu.c index 4d0b790..fac2813 100644 --- a/Src/Components/imu.c +++ b/Src/Components/imu.c @@ -161,7 +161,7 @@ void imu_init(void) { void imu_start_sampling(void) { if (!callback_ready) { - //error_handler_handle_warning(ERROR_HANDLER_GROUP_IMU, IMU_ERROR_READ_TIMEOUT); + error_handler_handle_warning(ERROR_HANDLER_GROUP_IMU, IMU_ERROR_READ_TIMEOUT); return; } diff --git a/Src/Components/imu.h b/Src/Components/imu.h index 8d59d8a..0b05a60 100644 --- a/Src/Components/imu.h +++ b/Src/Components/imu.h @@ -85,24 +85,37 @@ typedef enum { void imu_init(void); /** - * @brief Start sampling of the sensor. + * @brief Parse result and start next sampling process * - * This reads all data into an internal imu_data_t struct, if an error occurs sampling is aborted and no new data - * is reported. + * If the last sampling process is not completed the function shall call + * error_handler_handle_warning(ERROR_HANDLER_GROUP_IMU, IMU_ERROR_READ_TIMEOUT). * - * @warning Currently only one datum is read per call to this function, i.e. multiple calls are necessary to read - * all data, see #41. + * Otherwise the following steps shall be performed: + * * If the last response was read-success: + * * If the last datum was Status set imu_ok to false if the mode is not "Sensor Fusion Algorithm Running" + * * If the last datum was Calib-Status: + * * set imu_ok to false if any of the fields is less than the respective threshold + * * make the result available (such that ::imu_get_latest_data returns the data and ::imu_data_available + * returns true) + * * Read the next datum, the (cyclic-) order is: + * 1. Euler angles + * 2. Gyroscopic angular rate + * 3. Acceleration + * * Otherwise: + * * Call error_handler_handle_warning(ERROR_HANDLER_GROUP_BNO055, response + 1) + * * Re-read the current datum */ void imu_start_sampling(void); /** - * Get the last, fully received, set of measurements. + * @brief Get the last, fully received, set of measurements. + * * @return a struct containing all measurement data. */ imu_data_t imu_get_latest_data(void); /** - * Checks whether a new set of measurements was collected since the last call to get_latest_data(). + * @brief Checks whether a new set of measurements was collected since the last call to get_latest_data(). * @return true if new data is available. */ bool imu_data_available(void); diff --git a/Src/Components/system.c b/Src/Components/system.c index 972c599..997d663 100644 --- a/Src/Components/system.c +++ b/Src/Components/system.c @@ -13,9 +13,9 @@ #include #include -enum { MAX_TIME_SLOT_USAGE = (uint8_t) (3.5 / 4.096 * 255) }; +enum { MAX_TIME_SLOT_USAGE = (uint8_t) (12 / 16.384 * 255) }; -static system_timer_4_096ms_callback_t callback; +static system_timer_16_384ms_callback callback; static void internal_timer_callback(void) { callback(); @@ -25,7 +25,7 @@ static void internal_timer_callback(void) { } } -void system_pre_init(system_timer_4_096ms_callback_t timer_callback) { +void system_pre_init(system_timer_16_384ms_callback timer_callback) { cli(); callback = timer_callback; @@ -42,7 +42,7 @@ void system_pre_init(system_timer_4_096ms_callback_t timer_callback) { void system_post_init(void) { // Runs at 16.384ms interval, the BNO055 provides data at 100Hz, the output can be updated at 50Hz - timer_8bit_init(prescaler_256, &internal_timer_callback); + timer_8bit_init(prescaler_1024, &internal_timer_callback); wdt_enable(WDTO_30MS); } diff --git a/Src/Components/system.h b/Src/Components/system.h index 5f3fe19..d6fc36d 100644 --- a/Src/Components/system.h +++ b/Src/Components/system.h @@ -16,7 +16,7 @@ typedef enum { SYSTEM_ERROR_TIMER_RUNTIME = 1, SYSTEM_ERROR_WATCHDOG = 2, SYSTEM /** * The type of function to be called by the system component. */ -typedef void (*system_timer_4_096ms_callback_t)(void); +typedef void (*system_timer_16_384ms_callback)(void); /** * @brief Run initial system setup. @@ -29,13 +29,13 @@ typedef void (*system_timer_4_096ms_callback_t)(void); * * @param timer_callback the function to be called by the timer. */ -void system_pre_init(system_timer_4_096ms_callback_t timer_callback); +void system_pre_init(system_timer_16_384ms_callback timer_callback); /** * @brief Run system setup after component initialization. * * The post-initialization consists of the following task: - * * Start the timer with a period of 4.096ms + * * Start the timer with a period of 16.384ms * * Set the watchdog to 30ms */ void system_post_init(void); diff --git a/Tests/LowLevel/Application/application.cpp b/Tests/LowLevel/Application/application.cpp index 201d3e6..7322893 100644 --- a/Tests/LowLevel/Application/application.cpp +++ b/Tests/LowLevel/Application/application.cpp @@ -46,9 +46,9 @@ TEST(TEST_NAME, timer_mode_fcp) { auto systemHandle = mock::system.getHandle(); auto controllerHandle = mock::controller.getHandle(); - system_timer_4_096ms_callback_t timer_callback = nullptr; + system_timer_16_384ms_callback timer_callback = nullptr; systemHandle.overrideFunc( - [&timer_callback](system_timer_4_096ms_callback_t callback) { timer_callback = callback; }); + [&timer_callback](system_timer_16_384ms_callback callback) { timer_callback = callback; }); systemHandle.overrideFunc( []() { throw std::runtime_error{"EXCEPTION TO BREAK LOOP FOR TESTS"}; }); @@ -127,9 +127,9 @@ TEST(TEST_NAME, timer_mode_remote) { auto systemHandle = mock::system.getHandle(); auto controllerHandle = mock::controller.getHandle(); - system_timer_4_096ms_callback_t timer_callback = nullptr; + system_timer_16_384ms_callback timer_callback = nullptr; systemHandle.overrideFunc( - [&timer_callback](system_timer_4_096ms_callback_t callback) { timer_callback = callback; }); + [&timer_callback](system_timer_16_384ms_callback callback) { timer_callback = callback; }); systemHandle.overrideFunc( []() { throw std::runtime_error{"EXCEPTION TO BREAK LOOP FOR TESTS"}; }); @@ -190,9 +190,9 @@ TEST(TEST_NAME, timer_mode_stabilised_failsafe) { auto systemHandle = mock::system.getHandle(); auto controllerHandle = mock::controller.getHandle(); - system_timer_4_096ms_callback_t timer_callback = nullptr; + system_timer_16_384ms_callback timer_callback = nullptr; systemHandle.overrideFunc( - [&timer_callback](system_timer_4_096ms_callback_t callback) { timer_callback = callback; }); + [&timer_callback](system_timer_16_384ms_callback callback) { timer_callback = callback; }); systemHandle.overrideFunc( []() { throw std::runtime_error{"EXCEPTION TO BREAK LOOP FOR TESTS"}; }); @@ -271,9 +271,9 @@ TEST(TEST_NAME, timer_mode_failsafe) { auto systemHandle = mock::system.getHandle(); auto controllerHandle = mock::controller.getHandle(); - system_timer_4_096ms_callback_t timer_callback = nullptr; + system_timer_16_384ms_callback timer_callback = nullptr; systemHandle.overrideFunc( - [&timer_callback](system_timer_4_096ms_callback_t callback) { timer_callback = callback; }); + [&timer_callback](system_timer_16_384ms_callback callback) { timer_callback = callback; }); systemHandle.overrideFunc( []() { throw std::runtime_error{"EXCEPTION TO BREAK LOOP FOR TESTS"}; }); @@ -335,9 +335,9 @@ TEST(TEST_NAME, timer_send_period) { auto systemHandle = mock::system.getHandle(); auto controllerHandle = mock::controller.getHandle(); - system_timer_4_096ms_callback_t timer_callback = nullptr; + system_timer_16_384ms_callback timer_callback = nullptr; systemHandle.overrideFunc( - [&timer_callback](system_timer_4_096ms_callback_t callback) { timer_callback = callback; }); + [&timer_callback](system_timer_16_384ms_callback callback) { timer_callback = callback; }); systemHandle.overrideFunc( []() { throw std::runtime_error{"EXCEPTION TO BREAK LOOP FOR TESTS"}; }); diff --git a/Tests/LowLevel/Components/system.cpp b/Tests/LowLevel/Components/system.cpp index cd2a4fd..f24852e 100644 --- a/Tests/LowLevel/Components/system.cpp +++ b/Tests/LowLevel/Components/system.cpp @@ -78,7 +78,7 @@ TEST(TEST_NAME, timer_runtime_0) { timer_8bit_callback_t internalCallback = nullptr; timerHandle.overrideFunc( [&internalCallback](timer_8bit_clock_option_t /*clock_option*/, - system_timer_4_096ms_callback_t timerCallback) { internalCallback = timerCallback; }); + system_timer_16_384ms_callback timerCallback) { internalCallback = timerCallback; }); timerHandle.overrideFunc([]() { return 0; }); system_pre_init(timer_callback); @@ -104,7 +104,7 @@ TEST(TEST_NAME, timer_runtime_max) { timer_8bit_callback_t internalCallback = nullptr; timerHandle.overrideFunc( [&internalCallback](timer_8bit_clock_option_t /*clock_option*/, - system_timer_4_096ms_callback_t timerCallback) { internalCallback = timerCallback; }); + system_timer_16_384ms_callback timerCallback) { internalCallback = timerCallback; }); timerHandle.overrideFunc([]() { return 255; }); system_pre_init(timer_callback); @@ -130,7 +130,7 @@ TEST(TEST_NAME, timer_runtime_under_limit) { timer_8bit_callback_t internalCallback = nullptr; timerHandle.overrideFunc( [&internalCallback](timer_8bit_clock_option_t /*clock_option*/, - system_timer_4_096ms_callback_t timerCallback) { internalCallback = timerCallback; }); + system_timer_16_384ms_callback timerCallback) { internalCallback = timerCallback; }); // 12/16,384*255=186.767578125 timerHandle.overrideFunc([]() { return 186; }); @@ -157,7 +157,7 @@ TEST(TEST_NAME, timer_runtime_over_limit) { timer_8bit_callback_t internalCallback = nullptr; timerHandle.overrideFunc( [&internalCallback](timer_8bit_clock_option_t /*clock_option*/, - system_timer_4_096ms_callback_t timerCallback) { internalCallback = timerCallback; }); + system_timer_16_384ms_callback timerCallback) { internalCallback = timerCallback; }); // 12/16,384*255=186.767578125 timerHandle.overrideFunc([]() { return 187; }); From a69cd34bcb916c45474e56f5d1dbf64669243557 Mon Sep 17 00:00:00 2001 From: Paul Nykiel Date: Wed, 18 Jan 2023 20:04:23 +0100 Subject: [PATCH 03/17] Finished IMU requirements, started with test structure --- Src/Components/imu.h | 8 +- Tests/LowLevel/Components/imu.cpp | 459 +++++++++++++++++++++++------- 2 files changed, 359 insertions(+), 108 deletions(-) diff --git a/Src/Components/imu.h b/Src/Components/imu.h index 0b05a60..01b8913 100644 --- a/Src/Components/imu.h +++ b/Src/Components/imu.h @@ -98,9 +98,11 @@ void imu_init(void); * * make the result available (such that ::imu_get_latest_data returns the data and ::imu_data_available * returns true) * * Read the next datum, the (cyclic-) order is: - * 1. Euler angles - * 2. Gyroscopic angular rate - * 3. Acceleration + * 1. Euler angles (::bno055_read_eul_xyz_2_mul_16) + * 2. Gyroscopic angular rate (::bno055_read_gyr_xyz_mul_16) + * 3. Acceleration (::bno055_read_acc_xyz_mul_100) + * 4. System State (::bno055_read_system_status) + * 5. Calibration status (::bno055_read_calib_status) * * Otherwise: * * Call error_handler_handle_warning(ERROR_HANDLER_GROUP_BNO055, response + 1) * * Re-read the current datum diff --git a/Tests/LowLevel/Components/imu.cpp b/Tests/LowLevel/Components/imu.cpp index 173abb9..84db3b1 100644 --- a/Tests/LowLevel/Components/imu.cpp +++ b/Tests/LowLevel/Components/imu.cpp @@ -502,146 +502,395 @@ TEST(TEST_NAME, init__success) { EXPECT_TRUE(delayHandle.functionGotCalled<_delay_ms>(20)); } - -TEST(TEST_NAME, read__full_read) { - GTEST_SKIP_("Non requirement compliant workaround for non datasheet compliant sensor!"); +TEST(TEST_NAME, start_sampling__no_response) { + /* + * Init + */ auto bnoHandle = mock::bno055.getHandle(); - bool alreadyCalled = false; - - bnoHandle.overrideFunc([&alreadyCalled](int16_t *out, bno055_callback_t callback) { - if (not alreadyCalled) { - out[0] = 101; - out[1] = 102; - out[2] = 103; - alreadyCalled = true; - callback(read_success); - } - // Break the sampling process - }); + auto delayHandle = mock::delay.getHandle(); - bnoHandle.overrideFunc([](int16_t *out, bno055_callback_t callback) { - out[0] = 104; - out[1] = 105; - out[2] = 106; - callback(read_success); + bnoHandle.overrideFunc( + [](auto /*op_mode*/, bno055_callback_t callback) { callback(write_success); }); + bnoHandle.overrideFunc([](bno055_self_test_result_t *out, bno055_callback_t callback) { + out->mcu_passed = true; + out->acc_passed = true; + out->gyr_passed = true; + out->mag_passed = true; + callback(read_success); }); + bnoHandle.overrideFunc( + [](bno055_unit_sel_acc /*acc_unit*/, bno055_unit_sel_angular_rate /*angular_rate_unit*/, + bno055_unit_sel_euler_angles /*euler_angles_unit*/, bno055_unit_sel_temperature /*temperature_unit*/, + bno055_unit_sel_orientation_def /*orientation_def*/, + bno055_callback_t callback) { callback(write_success); }); + bnoHandle.overrideFunc( + [](bno055_axis_remap_axis_t /*new_x*/, bno055_axis_remap_axis_t /*new_y*/, + bno055_axis_remap_axis_t /*new_z*/, bno055_callback_t callback) { callback(write_success); }); + bnoHandle.overrideFunc( + [](bno055_axis_remap_sign_t /*new_x_sign*/, bno055_axis_remap_sign_t /*new_y_sign*/, + bno055_axis_remap_sign_t /*new_z_sign*/, bno055_callback_t callback) { callback(write_success); }); - bnoHandle.overrideFunc([](int16_t *out, bno055_callback_t callback) { - out[0] = 107; - out[1] = 108; - out[2] = 109; - callback(read_success); - }); + imu_init(); + EXPECT_TRUE(bnoHandle.functionGotCalled()); + EXPECT_TRUE(delayHandle.functionGotCalled<_delay_ms>(20)); + EXPECT_TRUE(delayHandle.functionGotCalled<_delay_ms>(20)); + EXPECT_TRUE(delayHandle.functionGotCalled<_delay_ms>(20)); + EXPECT_TRUE(delayHandle.functionGotCalled<_delay_ms>(20)); + EXPECT_TRUE(delayHandle.functionGotCalled<_delay_ms>(20)); + EXPECT_TRUE(delayHandle.functionGotCalled<_delay_ms>(20)); - bnoHandle.overrideFunc([](bno055_status_t *out, bno055_callback_t callback) { - *out = bno055_status_t::BNO055_STATUS_SENSOR_FUSION_ALGORITHM_RUNNING; - callback(read_success); - }); + /* + * Test: + * * Call start sampling, expect call to euler-angles + * * Call start sampling without calling callback, expect warning + */ - bnoHandle.overrideFunc([](bno055_calib_status_t *out, bno055_callback_t callback) { - out->acc_status = 0; - out->gyr_status = 0; - out->mag_status = 0; - out->sys_status = 0; - callback(read_success); - }); + EXPECT_TRUE(false); +} +TEST(TEST_NAME, start_sampling__read_error) { + /* + * Init + */ + auto bnoHandle = mock::bno055.getHandle(); + auto delayHandle = mock::delay.getHandle(); - EXPECT_FALSE(imu_data_available()); - EXPECT_NO_THROW(imu_start_sampling()); - EXPECT_TRUE(imu_data_available()); + bnoHandle.overrideFunc( + [](auto /*op_mode*/, bno055_callback_t callback) { callback(write_success); }); + bnoHandle.overrideFunc([](bno055_self_test_result_t *out, bno055_callback_t callback) { + out->mcu_passed = true; + out->acc_passed = true; + out->gyr_passed = true; + out->mag_passed = true; + callback(read_success); + }); + bnoHandle.overrideFunc( + [](bno055_unit_sel_acc /*acc_unit*/, bno055_unit_sel_angular_rate /*angular_rate_unit*/, + bno055_unit_sel_euler_angles /*euler_angles_unit*/, bno055_unit_sel_temperature /*temperature_unit*/, + bno055_unit_sel_orientation_def /*orientation_def*/, + bno055_callback_t callback) { callback(write_success); }); + bnoHandle.overrideFunc( + [](bno055_axis_remap_axis_t /*new_x*/, bno055_axis_remap_axis_t /*new_y*/, + bno055_axis_remap_axis_t /*new_z*/, bno055_callback_t callback) { callback(write_success); }); + bnoHandle.overrideFunc( + [](bno055_axis_remap_sign_t /*new_x_sign*/, bno055_axis_remap_sign_t /*new_y_sign*/, + bno055_axis_remap_sign_t /*new_z_sign*/, bno055_callback_t callback) { callback(write_success); }); - auto data = imu_get_latest_data(); + imu_init(); + EXPECT_TRUE(bnoHandle.functionGotCalled()); + EXPECT_TRUE(delayHandle.functionGotCalled<_delay_ms>(20)); + EXPECT_TRUE(delayHandle.functionGotCalled<_delay_ms>(20)); + EXPECT_TRUE(delayHandle.functionGotCalled<_delay_ms>(20)); + EXPECT_TRUE(delayHandle.functionGotCalled<_delay_ms>(20)); + EXPECT_TRUE(delayHandle.functionGotCalled<_delay_ms>(20)); + EXPECT_TRUE(delayHandle.functionGotCalled<_delay_ms>(20)); - EXPECT_FALSE(imu_data_available()); + /* + * Test: + * * Call start sampling, expect call to euler-angles + * * Call the provided callback with read-error as argument + * * Call start sampling, expect warning and call to euler-angles + */ - EXPECT_EQ(data.roll_mul_16, 103); - EXPECT_EQ(data.pitch_mul_16, 102); - EXPECT_EQ(data.heading_mul_16, 101); - EXPECT_EQ(data.d_roll_mul_16, 106); - EXPECT_EQ(data.d_pitch_mul_16, 105); - EXPECT_EQ(data.d_heading_mul_16, 104); - EXPECT_EQ(data.acc_x_mul_100, 107); - EXPECT_EQ(data.acc_y_mul_100, 108); - EXPECT_EQ(data.acc_z_mul_100, 109); - EXPECT_EQ(data.imu_ok, true); + EXPECT_TRUE(false); } -TEST(TEST_NAME, read__status_error) { - GTEST_SKIP_("Non requirement compliant workaround for non datasheet compliant sensor!"); +TEST(TEST_NAME, start_sampling__full_cycle) { + /* + * Init + */ auto bnoHandle = mock::bno055.getHandle(); - auto errorHandlerHandle = mock::error_handler.getHandle(); - bool alreadyCalled = false; + auto delayHandle = mock::delay.getHandle(); - bnoHandle.overrideFunc( - [&alreadyCalled](int16_t * /*out*/, bno055_callback_t callback) { - if (not alreadyCalled) { - alreadyCalled = true; - callback(read_success); - } - // Break the sampling process - }); + bnoHandle.overrideFunc( + [](auto /*op_mode*/, bno055_callback_t callback) { callback(write_success); }); + bnoHandle.overrideFunc([](bno055_self_test_result_t *out, bno055_callback_t callback) { + out->mcu_passed = true; + out->acc_passed = true; + out->gyr_passed = true; + out->mag_passed = true; + callback(read_success); + }); + bnoHandle.overrideFunc( + [](bno055_unit_sel_acc /*acc_unit*/, bno055_unit_sel_angular_rate /*angular_rate_unit*/, + bno055_unit_sel_euler_angles /*euler_angles_unit*/, bno055_unit_sel_temperature /*temperature_unit*/, + bno055_unit_sel_orientation_def /*orientation_def*/, + bno055_callback_t callback) { callback(write_success); }); + bnoHandle.overrideFunc( + [](bno055_axis_remap_axis_t /*new_x*/, bno055_axis_remap_axis_t /*new_y*/, + bno055_axis_remap_axis_t /*new_z*/, bno055_callback_t callback) { callback(write_success); }); + bnoHandle.overrideFunc( + [](bno055_axis_remap_sign_t /*new_x_sign*/, bno055_axis_remap_sign_t /*new_y_sign*/, + bno055_axis_remap_sign_t /*new_z_sign*/, bno055_callback_t callback) { callback(write_success); }); - bnoHandle.overrideFunc( - [](int16_t * /*out*/, bno055_callback_t callback) { callback(read_success); }); + imu_init(); + EXPECT_TRUE(bnoHandle.functionGotCalled()); + EXPECT_TRUE(delayHandle.functionGotCalled<_delay_ms>(20)); + EXPECT_TRUE(delayHandle.functionGotCalled<_delay_ms>(20)); + EXPECT_TRUE(delayHandle.functionGotCalled<_delay_ms>(20)); + EXPECT_TRUE(delayHandle.functionGotCalled<_delay_ms>(20)); + EXPECT_TRUE(delayHandle.functionGotCalled<_delay_ms>(20)); + EXPECT_TRUE(delayHandle.functionGotCalled<_delay_ms>(20)); - bnoHandle.overrideFunc( - [](int16_t * /*out*/, bno055_callback_t callback) { callback(read_success); }); + /* + * Test: + * * Call start sampling, expect call to read euler-angles + * * Write euler-angles to (17, 18, 19) + * * Call the provided callback with read-success as argument + * * expect no data available + * * Call start sampling, expect call to read gyro-rates + * * Write gyro-rates to (20, 21, 22) + * * Call the provided callback with read-success as argument + * * expect no data available + * * Call start sampling, expect call to read acceleration + * * Write acceleration to (23, 24, 25) + * * Call the provided callback with read-success as argument + * * expect no data available + * * Call start sampling, expect call to read system state + * * Write system state to "Sensor Fusion Algorithm Running" + * * Call the provided callback with read-success as argument + * * expect no data available + * * Call start sampling, expect call to read calibration status + * * Write calibration status to all fully calibration + * * Call the provided callback with read-success as argument + * * expect data available + * * check data + */ - bnoHandle.overrideFunc([](bno055_status_t *out, bno055_callback_t callback) { - *out = bno055_status_t::BNO055_STATUS_SYSTEM_IDLE; - callback(read_success); + EXPECT_TRUE(false); +} + +TEST(TEST_NAME, start_sampling__status_read_error) { + /* + * Init + */ + auto bnoHandle = mock::bno055.getHandle(); + auto delayHandle = mock::delay.getHandle(); + + bnoHandle.overrideFunc( + [](auto /*op_mode*/, bno055_callback_t callback) { callback(write_success); }); + bnoHandle.overrideFunc([](bno055_self_test_result_t *out, bno055_callback_t callback) { + out->mcu_passed = true; + out->acc_passed = true; + out->gyr_passed = true; + out->mag_passed = true; + callback(read_success); }); + bnoHandle.overrideFunc( + [](bno055_unit_sel_acc /*acc_unit*/, bno055_unit_sel_angular_rate /*angular_rate_unit*/, + bno055_unit_sel_euler_angles /*euler_angles_unit*/, bno055_unit_sel_temperature /*temperature_unit*/, + bno055_unit_sel_orientation_def /*orientation_def*/, + bno055_callback_t callback) { callback(write_success); }); + bnoHandle.overrideFunc( + [](bno055_axis_remap_axis_t /*new_x*/, bno055_axis_remap_axis_t /*new_y*/, + bno055_axis_remap_axis_t /*new_z*/, bno055_callback_t callback) { callback(write_success); }); + bnoHandle.overrideFunc( + [](bno055_axis_remap_sign_t /*new_x_sign*/, bno055_axis_remap_sign_t /*new_y_sign*/, + bno055_axis_remap_sign_t /*new_z_sign*/, bno055_callback_t callback) { callback(write_success); }); - bnoHandle.overrideFunc( - [](bno055_calib_status_t * /*out*/, bno055_callback_t callback) { callback(read_success); }); + imu_init(); + EXPECT_TRUE(bnoHandle.functionGotCalled()); + EXPECT_TRUE(delayHandle.functionGotCalled<_delay_ms>(20)); + EXPECT_TRUE(delayHandle.functionGotCalled<_delay_ms>(20)); + EXPECT_TRUE(delayHandle.functionGotCalled<_delay_ms>(20)); + EXPECT_TRUE(delayHandle.functionGotCalled<_delay_ms>(20)); + EXPECT_TRUE(delayHandle.functionGotCalled<_delay_ms>(20)); + EXPECT_TRUE(delayHandle.functionGotCalled<_delay_ms>(20)); - EXPECT_FALSE(imu_data_available()); - EXPECT_NO_THROW(imu_start_sampling()); - EXPECT_FALSE(imu_data_available()); + /* + * Test: + * * Call start sampling, expect call to euler-angles + * * Call the provided callback with read-error as argument + * * Call start sampling, expect warning and call to euler-angles + */ - auto data = imu_get_latest_data(); + EXPECT_TRUE(false); +} + +TEST(TEST_NAME, start_sampling__acc_not_calib) { + /* + * Init + */ + auto bnoHandle = mock::bno055.getHandle(); + auto delayHandle = mock::delay.getHandle(); + + bnoHandle.overrideFunc( + [](auto /*op_mode*/, bno055_callback_t callback) { callback(write_success); }); + bnoHandle.overrideFunc([](bno055_self_test_result_t *out, bno055_callback_t callback) { + out->mcu_passed = true; + out->acc_passed = true; + out->gyr_passed = true; + out->mag_passed = true; + callback(read_success); + }); + bnoHandle.overrideFunc( + [](bno055_unit_sel_acc /*acc_unit*/, bno055_unit_sel_angular_rate /*angular_rate_unit*/, + bno055_unit_sel_euler_angles /*euler_angles_unit*/, bno055_unit_sel_temperature /*temperature_unit*/, + bno055_unit_sel_orientation_def /*orientation_def*/, + bno055_callback_t callback) { callback(write_success); }); + bnoHandle.overrideFunc( + [](bno055_axis_remap_axis_t /*new_x*/, bno055_axis_remap_axis_t /*new_y*/, + bno055_axis_remap_axis_t /*new_z*/, bno055_callback_t callback) { callback(write_success); }); + bnoHandle.overrideFunc( + [](bno055_axis_remap_sign_t /*new_x_sign*/, bno055_axis_remap_sign_t /*new_y_sign*/, + bno055_axis_remap_sign_t /*new_z_sign*/, bno055_callback_t callback) { callback(write_success); }); + + imu_init(); + EXPECT_TRUE(bnoHandle.functionGotCalled()); + EXPECT_TRUE(delayHandle.functionGotCalled<_delay_ms>(20)); + EXPECT_TRUE(delayHandle.functionGotCalled<_delay_ms>(20)); + EXPECT_TRUE(delayHandle.functionGotCalled<_delay_ms>(20)); + EXPECT_TRUE(delayHandle.functionGotCalled<_delay_ms>(20)); + EXPECT_TRUE(delayHandle.functionGotCalled<_delay_ms>(20)); + EXPECT_TRUE(delayHandle.functionGotCalled<_delay_ms>(20)); - EXPECT_EQ(data.imu_ok, false); - EXPECT_TRUE(errorHandlerHandle.functionGotCalled(ERROR_HANDLER_GROUP_IMU, IMU_ERROR_STATUS)); + /* + * Test: + * * Call start sampling, expect call to euler-angles + * * Call the provided callback with read-error as argument + * * Call start sampling, expect warning and call to euler-angles + */ + + EXPECT_TRUE(false); } -TEST(TEST_NAME, read__error_call_error_handler) { - GTEST_SKIP_("Non requirement compliant workaround for non datasheet compliant sensor!"); +TEST(TEST_NAME, start_sampling__mag_not_calib) { + /* + * Init + */ auto bnoHandle = mock::bno055.getHandle(); - auto errorHandlerHandle = mock::error_handler.getHandle(); + auto delayHandle = mock::delay.getHandle(); - bnoHandle.overrideFunc( - [](int16_t * /*out*/, bno055_callback_t callback) { callback(read_fail); }); + bnoHandle.overrideFunc( + [](auto /*op_mode*/, bno055_callback_t callback) { callback(write_success); }); + bnoHandle.overrideFunc([](bno055_self_test_result_t *out, bno055_callback_t callback) { + out->mcu_passed = true; + out->acc_passed = true; + out->gyr_passed = true; + out->mag_passed = true; + callback(read_success); + }); + bnoHandle.overrideFunc( + [](bno055_unit_sel_acc /*acc_unit*/, bno055_unit_sel_angular_rate /*angular_rate_unit*/, + bno055_unit_sel_euler_angles /*euler_angles_unit*/, bno055_unit_sel_temperature /*temperature_unit*/, + bno055_unit_sel_orientation_def /*orientation_def*/, + bno055_callback_t callback) { callback(write_success); }); + bnoHandle.overrideFunc( + [](bno055_axis_remap_axis_t /*new_x*/, bno055_axis_remap_axis_t /*new_y*/, + bno055_axis_remap_axis_t /*new_z*/, bno055_callback_t callback) { callback(write_success); }); + bnoHandle.overrideFunc( + [](bno055_axis_remap_sign_t /*new_x_sign*/, bno055_axis_remap_sign_t /*new_y_sign*/, + bno055_axis_remap_sign_t /*new_z_sign*/, bno055_callback_t callback) { callback(write_success); }); - EXPECT_FALSE(imu_data_available()); - EXPECT_NO_THROW(imu_start_sampling()); - EXPECT_FALSE(imu_data_available()); + imu_init(); + EXPECT_TRUE(bnoHandle.functionGotCalled()); + EXPECT_TRUE(delayHandle.functionGotCalled<_delay_ms>(20)); + EXPECT_TRUE(delayHandle.functionGotCalled<_delay_ms>(20)); + EXPECT_TRUE(delayHandle.functionGotCalled<_delay_ms>(20)); + EXPECT_TRUE(delayHandle.functionGotCalled<_delay_ms>(20)); + EXPECT_TRUE(delayHandle.functionGotCalled<_delay_ms>(20)); + EXPECT_TRUE(delayHandle.functionGotCalled<_delay_ms>(20)); - auto data = imu_get_latest_data(); + /* + * Test: + * * Call start sampling, expect call to euler-angles + * * Call the provided callback with read-error as argument + * * Call start sampling, expect warning and call to euler-angles + */ - EXPECT_EQ(data.imu_ok, false); - EXPECT_TRUE(errorHandlerHandle.functionGotCalled(ERROR_HANDLER_GROUP_BNO055, read_fail + 1)); + EXPECT_TRUE(false); } -TEST(TEST_NAME, read__error_restart) { - GTEST_SKIP_("Non requirement compliant workaround for non datasheet compliant sensor!"); +TEST(TEST_NAME, start_sampling__gyr_not_calib) { + /* + * Init + */ auto bnoHandle = mock::bno055.getHandle(); - auto errorHandlerHandle = mock::error_handler.getHandle(); - int16_t *dataPtr = nullptr; + auto delayHandle = mock::delay.getHandle(); - bnoHandle.overrideFunc( - [](int16_t * /*out*/, bno055_callback_t callback) { callback(read_success); }); + bnoHandle.overrideFunc( + [](auto /*op_mode*/, bno055_callback_t callback) { callback(write_success); }); + bnoHandle.overrideFunc([](bno055_self_test_result_t *out, bno055_callback_t callback) { + out->mcu_passed = true; + out->acc_passed = true; + out->gyr_passed = true; + out->mag_passed = true; + callback(read_success); + }); + bnoHandle.overrideFunc( + [](bno055_unit_sel_acc /*acc_unit*/, bno055_unit_sel_angular_rate /*angular_rate_unit*/, + bno055_unit_sel_euler_angles /*euler_angles_unit*/, bno055_unit_sel_temperature /*temperature_unit*/, + bno055_unit_sel_orientation_def /*orientation_def*/, + bno055_callback_t callback) { callback(write_success); }); + bnoHandle.overrideFunc( + [](bno055_axis_remap_axis_t /*new_x*/, bno055_axis_remap_axis_t /*new_y*/, + bno055_axis_remap_axis_t /*new_z*/, bno055_callback_t callback) { callback(write_success); }); + bnoHandle.overrideFunc( + [](bno055_axis_remap_sign_t /*new_x_sign*/, bno055_axis_remap_sign_t /*new_y_sign*/, + bno055_axis_remap_sign_t /*new_z_sign*/, bno055_callback_t callback) { callback(write_success); }); - bnoHandle.overrideFunc([&dataPtr](int16_t *out, bno055_callback_t callback) { - EXPECT_NE(dataPtr, out); - dataPtr = out; - callback(read_fail); + imu_init(); + EXPECT_TRUE(bnoHandle.functionGotCalled()); + EXPECT_TRUE(delayHandle.functionGotCalled<_delay_ms>(20)); + EXPECT_TRUE(delayHandle.functionGotCalled<_delay_ms>(20)); + EXPECT_TRUE(delayHandle.functionGotCalled<_delay_ms>(20)); + EXPECT_TRUE(delayHandle.functionGotCalled<_delay_ms>(20)); + EXPECT_TRUE(delayHandle.functionGotCalled<_delay_ms>(20)); + EXPECT_TRUE(delayHandle.functionGotCalled<_delay_ms>(20)); + + /* + * Test: + * * Call start sampling, expect call to euler-angles + * * Call the provided callback with read-error as argument + * * Call start sampling, expect warning and call to euler-angles + */ + + EXPECT_TRUE(false); +} + +TEST(TEST_NAME, start_sampling__sys_not_calib) { + /* + * Init + */ + auto bnoHandle = mock::bno055.getHandle(); + auto delayHandle = mock::delay.getHandle(); + + bnoHandle.overrideFunc( + [](auto /*op_mode*/, bno055_callback_t callback) { callback(write_success); }); + bnoHandle.overrideFunc([](bno055_self_test_result_t *out, bno055_callback_t callback) { + out->mcu_passed = true; + out->acc_passed = true; + out->gyr_passed = true; + out->mag_passed = true; + callback(read_success); }); + bnoHandle.overrideFunc( + [](bno055_unit_sel_acc /*acc_unit*/, bno055_unit_sel_angular_rate /*angular_rate_unit*/, + bno055_unit_sel_euler_angles /*euler_angles_unit*/, bno055_unit_sel_temperature /*temperature_unit*/, + bno055_unit_sel_orientation_def /*orientation_def*/, + bno055_callback_t callback) { callback(write_success); }); + bnoHandle.overrideFunc( + [](bno055_axis_remap_axis_t /*new_x*/, bno055_axis_remap_axis_t /*new_y*/, + bno055_axis_remap_axis_t /*new_z*/, bno055_callback_t callback) { callback(write_success); }); + bnoHandle.overrideFunc( + [](bno055_axis_remap_sign_t /*new_x_sign*/, bno055_axis_remap_sign_t /*new_y_sign*/, + bno055_axis_remap_sign_t /*new_z_sign*/, bno055_callback_t callback) { callback(write_success); }); - EXPECT_FALSE(imu_data_available()); - EXPECT_NO_THROW(imu_start_sampling()); - EXPECT_TRUE(errorHandlerHandle.functionGotCalled(ERROR_HANDLER_GROUP_BNO055, read_fail + 1)); - EXPECT_NO_THROW(imu_start_sampling()); - EXPECT_TRUE(errorHandlerHandle.functionGotCalled(ERROR_HANDLER_GROUP_BNO055, read_fail + 1)); + imu_init(); + EXPECT_TRUE(bnoHandle.functionGotCalled()); + EXPECT_TRUE(delayHandle.functionGotCalled<_delay_ms>(20)); + EXPECT_TRUE(delayHandle.functionGotCalled<_delay_ms>(20)); + EXPECT_TRUE(delayHandle.functionGotCalled<_delay_ms>(20)); + EXPECT_TRUE(delayHandle.functionGotCalled<_delay_ms>(20)); + EXPECT_TRUE(delayHandle.functionGotCalled<_delay_ms>(20)); + EXPECT_TRUE(delayHandle.functionGotCalled<_delay_ms>(20)); + + /* + * Test: + * * Call start sampling, expect call to euler-angles + * * Call the provided callback with read-error as argument + * * Call start sampling, expect warning and call to euler-angles + */ + + EXPECT_TRUE(false); } From ec0056ec050082fc33d5d4ea5f449a5331ab5f8d Mon Sep 17 00:00:00 2001 From: Paul Nykiel Date: Thu, 19 Jan 2023 20:06:27 +0100 Subject: [PATCH 04/17] #41: Added second high priority timer at 250Hz --- README.md | 7 +- Src/Application/application.c | 10 +- Src/Application/mode_handler.c | 18 ++-- Src/Components/imu.c | 135 +++++++++++++-------------- Src/Components/system.c | 39 +++++--- Src/Components/system.h | 23 ++++- Tests/LowLevel/Components/system.cpp | 8 +- 7 files changed, 132 insertions(+), 108 deletions(-) diff --git a/README.md b/README.md index 90a4cf6..2ec87cf 100644 --- a/README.md +++ b/README.md @@ -54,9 +54,10 @@ The error IDs are: | Component | Exception | LED configuration | |----------------|--------------------------|-------------------| | Application | - | xxxx ◯◯◯🔴 | -| System | Timer Runtime | ◯◯◯🔴 ◯◯🔴◯ | -| | Watchdog | ◯◯🔴◯ ◯◯🔴◯ | -| | Brownout | ◯◯🔴🔴 ◯◯🔴◯ | +| System | Watchdog | ◯◯◯🔴 ◯◯🔴◯ | +| | Brownout | ◯◯🔴◯ ◯◯🔴◯ | +| | Low-Prio Timer Runtime | ◯◯🔴🔴 ◯◯🔴◯ | +| | High-Prio Timer Runtime | ◯🔴◯◯ ◯◯🔴◯ | | IMU | Init timeout error | ◯◯◯🔴 ◯◯🔴🔴 | | | Init self test error | ◯◯🔴◯ ◯◯🔴🔴 | | | Status error | ◯◯🔴🔴 ◯◯🔴🔴 | diff --git a/Src/Application/application.c b/Src/Application/application.c index 62efcce..2cf5d7b 100644 --- a/Src/Application/application.c +++ b/Src/Application/application.c @@ -19,7 +19,7 @@ /** * Frequency divider between the timer interrupt and sending new messages. */ -enum { FLIGHT_COMPUTER_SEND_PERIOD = (uint16_t) (100 / 16.383) }; +enum { FLIGHT_COMPUTER_SEND_PERIOD = (int) (100 / 16.383) }; /** * Offset from the remote data (in [0, 1000]) to the servo_motor_cmd data (in [-500, 500]) @@ -76,15 +76,10 @@ static void timer_tick(void) { flight_computer_send(&imu_data, &remote_data, &actuator_cmd); fcps_send_mux = 0; } - - /* - * Read the next IMU data - */ - //imu_start_sampling(); } void application_init(void) { - system_pre_init(timer_tick); + system_pre_init(timer_tick, imu_start_sampling); error_handler_init(); imu_init(); @@ -99,6 +94,5 @@ void application_init(void) { while (true) { system_reset_watchdog(); - imu_start_sampling(); } } diff --git a/Src/Application/mode_handler.c b/Src/Application/mode_handler.c index bb47113..b0bc39f 100644 --- a/Src/Application/mode_handler.c +++ b/Src/Application/mode_handler.c @@ -10,18 +10,18 @@ #include "error_handler.h" enum { - IMU_TIMEOUT = (uint16_t) (100 / 16.384), - FLIGHTCOMPUTER_TIMEOUT = (uint16_t) ((2 * 100) / 16.384), - REMOTE_TIMEOUT = (uint16_t) (100 / 16.384), + IMU_TIMEOUT = (int) (1000 / 16.384), + FLIGHT_COMPUTER_TIMEOUT = (int) ((2 * 100) / 16.384), + REMOTE_TIMEOUT = (int) (100 / 16.384), }; static uint8_t imu_timeout_counter; -static uint8_t flightcomputer_timeout_counter; +static uint8_t flight_computer_timeout_counter; static uint8_t remote_timeout_counter; void mode_handler_init(void) { imu_timeout_counter = IMU_TIMEOUT; - flightcomputer_timeout_counter = FLIGHTCOMPUTER_TIMEOUT; + flight_computer_timeout_counter = FLIGHT_COMPUTER_TIMEOUT; remote_timeout_counter = REMOTE_TIMEOUT; } @@ -55,13 +55,13 @@ mode_handler_mode_t mode_handler_handle(imu_data_t *imu_data, remote_data_t *rem *remote_data = remote_get_data(); if (!flight_computer_set_point_available()) { - flightcomputer_timeout_counter += 1; + flight_computer_timeout_counter += 1; } else { - flightcomputer_timeout_counter = 0; + flight_computer_timeout_counter = 0; } bool flightcomputer_active = true; - if (flightcomputer_timeout_counter >= FLIGHTCOMPUTER_TIMEOUT) { - flightcomputer_timeout_counter = FLIGHTCOMPUTER_TIMEOUT; + if (flight_computer_timeout_counter >= FLIGHT_COMPUTER_TIMEOUT) { + flight_computer_timeout_counter = FLIGHT_COMPUTER_TIMEOUT; flightcomputer_active = false; } *flightcomputer_setpoint = flight_computer_get_set_point(); diff --git a/Src/Components/imu.c b/Src/Components/imu.c index fac2813..92e2970 100644 --- a/Src/Components/imu.c +++ b/Src/Components/imu.c @@ -160,76 +160,75 @@ void imu_init(void) { } void imu_start_sampling(void) { - if (!callback_ready) { - error_handler_handle_warning(ERROR_HANDLER_GROUP_IMU, IMU_ERROR_READ_TIMEOUT); - return; - } - - imu_data_t *imu_data = (imu_data_t *) (&imu_datas[current_sample_state_id]); - callback_ready = false; - - if (response != read_success) { - if (response != bus_over_run_error) { - error_handler_handle_warning(ERROR_HANDLER_GROUP_BNO055, response + 1); - } - - switch (current_sample_state) { - case EUL: - bno055_read_eul_xyz_2_mul_16(&imu_data->heading_mul_16, bno_callback); - break; - case GYR: - bno055_read_gyr_xyz_mul_16(&imu_data->d_heading_mul_16, bno_callback); - break; - case ACC: - bno055_read_acc_xyz_mul_100(&imu_data->acc_x_mul_100, bno_callback); - break; - case STATUS: - bno055_read_system_status(&bno_status, bno_callback); - break; - case CALIB_STAT: - bno055_read_calib_status(&calib_status, bno_callback); - break; + if (callback_ready) { + imu_data_t *imu_data = (imu_data_t *) (&imu_datas[current_sample_state_id]); + callback_ready = false; + + if (response != read_success) { + if (response != bus_over_run_error) { + error_handler_handle_warning(ERROR_HANDLER_GROUP_BNO055, response + 1); + } + + switch (current_sample_state) { + case EUL: + bno055_read_eul_xyz_2_mul_16(&imu_data->heading_mul_16, bno_callback); + break; + case GYR: + bno055_read_gyr_xyz_mul_16(&imu_data->d_heading_mul_16, bno_callback); + break; + case ACC: + bno055_read_acc_xyz_mul_100(&imu_data->acc_x_mul_100, bno_callback); + break; + case STATUS: + bno055_read_system_status(&bno_status, bno_callback); + break; + case CALIB_STAT: + bno055_read_calib_status(&calib_status, bno_callback); + break; + } + } else { + switch (current_sample_state) { + case EUL: + current_sample_state = GYR; + bno055_read_gyr_xyz_mul_16(&imu_data->d_heading_mul_16, bno_callback); + break; + case GYR: + current_sample_state = ACC; + bno055_read_acc_xyz_mul_100(&imu_data->acc_x_mul_100, bno_callback); + break; + case ACC: + current_sample_state = STATUS; + bno055_read_system_status(&bno_status, bno_callback); + break; + case STATUS: + // Status post-processing + if (bno_status != BNO055_STATUS_SENSOR_FUSION_ALGORITHM_RUNNING) { + error_handler_handle_warning(ERROR_HANDLER_GROUP_IMU, IMU_ERROR_STATUS); + imu_data->imu_ok = false; + } else { + imu_data->imu_ok = true; + } + + current_sample_state = CALIB_STAT; + bno055_read_calib_status(&calib_status, bno_callback); + break; + case CALIB_STAT: + // Calib-stat post-processing + if (calib_status.sys_status < SYS_CALIB_TRESH || calib_status.gyr_status < GYR_CALIB_TRESH || + calib_status.acc_status < ACC_CALIB_TRESH || calib_status.mag_status < MAG_CALIB_TRESH) { + imu_data->imu_ok = false; + } + current_sample_state_id = 1 - current_sample_state_id; + sampling_complete = true; + imu_data = (imu_data_t *) (&imu_datas[current_sample_state_id]); + + current_sample_state = EUL; + bno055_read_eul_xyz_2_mul_16(&imu_data->heading_mul_16, bno_callback); + break; + } } } else { - switch (current_sample_state) { - case EUL: - current_sample_state = GYR; - bno055_read_gyr_xyz_mul_16(&imu_data->d_heading_mul_16, bno_callback); - break; - case GYR: - current_sample_state = ACC; - bno055_read_acc_xyz_mul_100(&imu_data->acc_x_mul_100, bno_callback); - break; - case ACC: - current_sample_state = STATUS; - bno055_read_system_status(&bno_status, bno_callback); - break; - case STATUS: - // Status post-processing - if (bno_status != BNO055_STATUS_SENSOR_FUSION_ALGORITHM_RUNNING) { - error_handler_handle_warning(ERROR_HANDLER_GROUP_IMU, IMU_ERROR_STATUS); - imu_data->imu_ok = false; - } else { - imu_data->imu_ok = true; - } - - current_sample_state = CALIB_STAT; - bno055_read_calib_status(&calib_status, bno_callback); - break; - case CALIB_STAT: - // Calib-stat post-processing - if (calib_status.sys_status < SYS_CALIB_TRESH || calib_status.gyr_status < GYR_CALIB_TRESH || - calib_status.acc_status < ACC_CALIB_TRESH || calib_status.mag_status < MAG_CALIB_TRESH) { - imu_data->imu_ok = false; - } - current_sample_state_id = 1 - current_sample_state_id; - sampling_complete = true; - imu_data = (imu_data_t *) (&imu_datas[current_sample_state_id]); - - current_sample_state = EUL; - bno055_read_eul_xyz_2_mul_16(&imu_data->heading_mul_16, bno_callback); - break; - } + //error_handler_handle_warning(ERROR_HANDLER_GROUP_IMU, IMU_ERROR_READ_TIMEOUT); } } diff --git a/Src/Components/system.c b/Src/Components/system.c index 997d663..2f3505f 100644 --- a/Src/Components/system.c +++ b/Src/Components/system.c @@ -12,23 +12,40 @@ #include #include #include +#include -enum { MAX_TIME_SLOT_USAGE = (uint8_t) (12 / 16.384 * 255) }; +enum { MAX_TIME_SLOT_USAGE_LOW_PRIO = (uint8_t) (12 / 16.384 * 255) }; +enum { MAX_TIME_SLOT_USAGE_HIGH_PRIO = (uint8_t) (1 / 4.096 * 255) }; -static system_timer_16_384ms_callback callback; +static system_timer_16_384ms_callback low_prio_callback_; +static system_timer_4_096ms_callback high_prio_callback_; -static void internal_timer_callback(void) { - callback(); - uint8_t time = timer_8bit_get_count(); - if (time > MAX_TIME_SLOT_USAGE) { - error_handler_handle_warning(ERROR_HANDLER_GROUP_SYSTEM, SYSTEM_ERROR_TIMER_RUNTIME); +static void internal_low_prio_callback(void) { + SREG |= 1U << SREG_I; // Enable nested interrupts for this interrupt + low_prio_callback_(); + + uint8_t time = timer_8bit_get_count(TIMER_ID_0); + if (time > MAX_TIME_SLOT_USAGE_LOW_PRIO) { + error_handler_handle_warning(ERROR_HANDLER_GROUP_SYSTEM, SYSTEM_ERROR_RUNTIME_LOW_PRIO); + } +} + +static void internal_high_prio_callback(void) { + high_prio_callback_(); + + uint8_t time = timer_8bit_get_count(TIMER_ID_2); + if (time > MAX_TIME_SLOT_USAGE_HIGH_PRIO) { + error_handler_handle_warning(ERROR_HANDLER_GROUP_SYSTEM, SYSTEM_ERROR_RUNTIME_HIGH_PRIO); } } -void system_pre_init(system_timer_16_384ms_callback timer_callback) { +void system_pre_init(system_timer_16_384ms_callback low_prio_callback, + system_timer_4_096ms_callback high_prio_callback) { cli(); - callback = timer_callback; + low_prio_callback_ = low_prio_callback; + high_prio_callback_ = high_prio_callback; + if (MCUSR & (1U << WDRF)) { error_handler_handle_warning(ERROR_HANDLER_GROUP_SYSTEM, SYSTEM_ERROR_WATCHDOG); } else if (MCUSR & (1U << BORF)) { @@ -41,8 +58,8 @@ void system_pre_init(system_timer_16_384ms_callback timer_callback) { } void system_post_init(void) { - // Runs at 16.384ms interval, the BNO055 provides data at 100Hz, the output can be updated at 50Hz - timer_8bit_init(prescaler_1024, &internal_timer_callback); + timer_8bit_init(TIMER_ID_0, prescaler_1024, &internal_low_prio_callback); + timer_8bit_init(TIMER_ID_2, prescaler_256, &internal_high_prio_callback); wdt_enable(WDTO_30MS); } diff --git a/Src/Components/system.h b/Src/Components/system.h index d6fc36d..adb191a 100644 --- a/Src/Components/system.h +++ b/Src/Components/system.h @@ -11,13 +11,23 @@ /** * Errors reported by the system component to the error-handler. */ -typedef enum { SYSTEM_ERROR_TIMER_RUNTIME = 1, SYSTEM_ERROR_WATCHDOG = 2, SYSTEM_ERROR_BROWNOUT = 3 } system_error_t; +typedef enum { + SYSTEM_ERROR_WATCHDOG = 1, + SYSTEM_ERROR_BROWNOUT = 2, + SYSTEM_ERROR_RUNTIME_LOW_PRIO = 3, + SYSTEM_ERROR_RUNTIME_HIGH_PRIO = 4, +} system_error_t; /** - * The type of function to be called by the system component. + * The type of function to be called by the system component every 16.384ms with low priority. */ typedef void (*system_timer_16_384ms_callback)(void); +/** + * The type of function to be called by the system component every 4.096ms with high priority. + */ +typedef void (*system_timer_4_096ms_callback)(void); + /** * @brief Run initial system setup. * @@ -27,15 +37,18 @@ typedef void (*system_timer_16_384ms_callback)(void); * * Set the watchdog to 250ms for component initialization * * Re-enable the interrupts * - * @param timer_callback the function to be called by the timer. + * @param low_prio_callback low priority function to be called every 16.384ms + * @param high_prio_callback high priority function to be called every 4.096ms */ -void system_pre_init(system_timer_16_384ms_callback timer_callback); +void system_pre_init(system_timer_16_384ms_callback low_prio_callback, + system_timer_4_096ms_callback high_prio_callback); /** * @brief Run system setup after component initialization. * * The post-initialization consists of the following task: - * * Start the timer with a period of 16.384ms + * * Start the timer 0 with a period of 16.384ms + * * Start the timer 2 with a period of 16.384ms * * Set the watchdog to 30ms */ void system_post_init(void); diff --git a/Tests/LowLevel/Components/system.cpp b/Tests/LowLevel/Components/system.cpp index f24852e..98c0eab 100644 --- a/Tests/LowLevel/Components/system.cpp +++ b/Tests/LowLevel/Components/system.cpp @@ -77,7 +77,7 @@ TEST(TEST_NAME, timer_runtime_0) { timer_8bit_callback_t internalCallback = nullptr; timerHandle.overrideFunc( - [&internalCallback](timer_8bit_clock_option_t /*clock_option*/, + [&internalCallback](timer_8bit_clock_options /*clock_option*/, system_timer_16_384ms_callback timerCallback) { internalCallback = timerCallback; }); timerHandle.overrideFunc([]() { return 0; }); @@ -103,7 +103,7 @@ TEST(TEST_NAME, timer_runtime_max) { timer_8bit_callback_t internalCallback = nullptr; timerHandle.overrideFunc( - [&internalCallback](timer_8bit_clock_option_t /*clock_option*/, + [&internalCallback](timer_8bit_clock_options /*clock_option*/, system_timer_16_384ms_callback timerCallback) { internalCallback = timerCallback; }); timerHandle.overrideFunc([]() { return 255; }); @@ -129,7 +129,7 @@ TEST(TEST_NAME, timer_runtime_under_limit) { timer_8bit_callback_t internalCallback = nullptr; timerHandle.overrideFunc( - [&internalCallback](timer_8bit_clock_option_t /*clock_option*/, + [&internalCallback](timer_8bit_clock_options /*clock_option*/, system_timer_16_384ms_callback timerCallback) { internalCallback = timerCallback; }); // 12/16,384*255=186.767578125 timerHandle.overrideFunc([]() { return 186; }); @@ -156,7 +156,7 @@ TEST(TEST_NAME, timer_runtime_over_limit) { timer_8bit_callback_t internalCallback = nullptr; timerHandle.overrideFunc( - [&internalCallback](timer_8bit_clock_option_t /*clock_option*/, + [&internalCallback](timer_8bit_clock_options /*clock_option*/, system_timer_16_384ms_callback timerCallback) { internalCallback = timerCallback; }); // 12/16,384*255=186.767578125 timerHandle.overrideFunc([]() { return 187; }); From f86922fbe0f67dd18a32132798fef6accce5e1a9 Mon Sep 17 00:00:00 2001 From: Paul Nykiel Date: Fri, 20 Jan 2023 19:58:15 +0100 Subject: [PATCH 05/17] Documentation --- Src/Application/application.h | 3 ++- Src/Components/imu.h | 14 +++++++++++--- Src/Components/system.h | 13 ++++++++++++- Src/HAL | 2 +- 4 files changed, 26 insertions(+), 6 deletions(-) diff --git a/Src/Application/application.h b/Src/Application/application.h index 83e4b39..51670bb 100644 --- a/Src/Application/application.h +++ b/Src/Application/application.h @@ -12,7 +12,8 @@ * @brief Main application that performs initialization and starts the timer. * * The initialization consists of the following tasks: - * * System Pre-Initialization (::system_pre_init) + * * System Pre-Initialization (::system_pre_init) with the internal timer callback for the low priority callback and + * ::imu_start_sampling for the high priority callback * * Error Handler initialization (::error_handler_init) * * IMU initialization (::imu_init) * * Remote initialization (::remote_init) diff --git a/Src/Components/imu.h b/Src/Components/imu.h index 01b8913..ddca320 100644 --- a/Src/Components/imu.h +++ b/Src/Components/imu.h @@ -92,8 +92,8 @@ void imu_init(void); * * Otherwise the following steps shall be performed: * * If the last response was read-success: - * * If the last datum was Status set imu_ok to false if the mode is not "Sensor Fusion Algorithm Running" - * * If the last datum was Calib-Status: + * * If the last datum was "System Status" set imu_ok to false if the mode is not "Sensor Fusion Algorithm Running" + * * If the last datum was "Calib-Status": * * set imu_ok to false if any of the fields is less than the respective threshold * * make the result available (such that ::imu_get_latest_data returns the data and ::imu_data_available * returns true) @@ -110,7 +110,11 @@ void imu_init(void); void imu_start_sampling(void); /** - * @brief Get the last, fully received, set of measurements. + * @brief Get the last measurement + * + * Returns the last fully receive set of measurements, if sampling is complete but an error was reported by the sensor + * it imu_ok is false. + * In addition ::imu_data_available will return false after this call. * * @return a struct containing all measurement data. */ @@ -118,6 +122,10 @@ imu_data_t imu_get_latest_data(void); /** * @brief Checks whether a new set of measurements was collected since the last call to get_latest_data(). + * + * Returns whether a new, complete set of measurements was received, returns true until the data is read via + * ::imu_get_latest_data + * * @return true if new data is available. */ bool imu_data_available(void); diff --git a/Src/Components/system.h b/Src/Components/system.h index adb191a..ec52f0a 100644 --- a/Src/Components/system.h +++ b/Src/Components/system.h @@ -48,8 +48,19 @@ void system_pre_init(system_timer_16_384ms_callback low_prio_callback, * * The post-initialization consists of the following task: * * Start the timer 0 with a period of 16.384ms - * * Start the timer 2 with a period of 16.384ms + * * Start the timer 2 with a period of 4.096ms * * Set the watchdog to 30ms + * + * This function will start both timers, inside the timer callbacks the following tasks are performed: + * * For the low priority timer: + * * Enable nested interrupts by setting the I bit in the SREG register + * * Call the low-priority callback + * * Check the runtime, if it exceeds 12ms call + * error_handler_handle_warning(ERROR_HANDLER_GROUP_SYSTEM, SYSTEM_ERROR_RUNTIME_LOW_PRIO) + * * For the low priority timer: + * * Call the high-priority callback + * * Check the runtime, if it exceeds 1ms call + * error_handler_handle_warning(ERROR_HANDLER_GROUP_SYSTEM, SYSTEM_ERROR_RUNTIME_HIGH_PRIO) */ void system_post_init(void); diff --git a/Src/HAL b/Src/HAL index bc63d5f..bec2574 160000 --- a/Src/HAL +++ b/Src/HAL @@ -1 +1 @@ -Subproject commit bc63d5f8909d112d7a70fa21fe68a20038b507e2 +Subproject commit bec2574463456c32d4100ba7041e43427f80fbac From 9053d7562bd25a51de56358c419eb85f63131827 Mon Sep 17 00:00:00 2001 From: Paul Nykiel Date: Sun, 22 Jan 2023 18:46:38 +0100 Subject: [PATCH 06/17] Application and system tests --- Src/Components/system.c | 1 - Src/HAL | 2 +- Tests/LowLevel/Application/application.cpp | 34 ++- Tests/LowLevel/Components/system.cpp | 299 +++++++++++++++++---- Tests/LowLevel/Mock | 2 +- 5 files changed, 264 insertions(+), 74 deletions(-) diff --git a/Src/Components/system.c b/Src/Components/system.c index 2f3505f..ec67e2b 100644 --- a/Src/Components/system.c +++ b/Src/Components/system.c @@ -12,7 +12,6 @@ #include #include #include -#include enum { MAX_TIME_SLOT_USAGE_LOW_PRIO = (uint8_t) (12 / 16.384 * 255) }; enum { MAX_TIME_SLOT_USAGE_HIGH_PRIO = (uint8_t) (1 / 4.096 * 255) }; diff --git a/Src/HAL b/Src/HAL index bec2574..d3862c9 160000 --- a/Src/HAL +++ b/Src/HAL @@ -1 +1 @@ -Subproject commit bec2574463456c32d4100ba7041e43427f80fbac +Subproject commit d3862c9c2031e6704847218fee4ae03615b74e5b diff --git a/Tests/LowLevel/Application/application.cpp b/Tests/LowLevel/Application/application.cpp index 7322893..933576e 100644 --- a/Tests/LowLevel/Application/application.cpp +++ b/Tests/LowLevel/Application/application.cpp @@ -26,7 +26,7 @@ TEST(TEST_NAME, init) { EXPECT_THROW(application_init(), std::runtime_error); - EXPECT_TRUE(systemHandle.functionGotCalled(std::ignore)); + EXPECT_TRUE(systemHandle.functionGotCalled(std::ignore, imu_start_sampling)); EXPECT_TRUE(errorHandlerHandle.functionGotCalled()); EXPECT_TRUE(imuHandle.functionGotCalled()); EXPECT_TRUE(remoteHandlerHandle.functionGotCalled()); @@ -48,13 +48,14 @@ TEST(TEST_NAME, timer_mode_fcp) { system_timer_16_384ms_callback timer_callback = nullptr; systemHandle.overrideFunc( - [&timer_callback](system_timer_16_384ms_callback callback) { timer_callback = callback; }); + [&timer_callback](system_timer_16_384ms_callback callback, system_timer_4_096ms_callback /*callback*/) { + timer_callback = callback; + }); systemHandle.overrideFunc( []() { throw std::runtime_error{"EXCEPTION TO BREAK LOOP FOR TESTS"}; }); EXPECT_THROW(application_init(), std::runtime_error); - EXPECT_TRUE(systemHandle.functionGotCalled(std::ignore)); EXPECT_TRUE(errorHandlerHandle.functionGotCalled()); EXPECT_TRUE(imuHandle.functionGotCalled()); EXPECT_TRUE(remoteHandlerHandle.functionGotCalled()); @@ -114,7 +115,6 @@ TEST(TEST_NAME, timer_mode_fcp) { EXPECT_TRUE(modeHandlerHandle.functionGotCalled()); EXPECT_TRUE(controllerHandle.functionGotCalled()); EXPECT_TRUE(actuatorsHandle.functionGotCalled()); - EXPECT_TRUE(imuHandle.functionGotCalled()); } TEST(TEST_NAME, timer_mode_remote) { @@ -129,13 +129,14 @@ TEST(TEST_NAME, timer_mode_remote) { system_timer_16_384ms_callback timer_callback = nullptr; systemHandle.overrideFunc( - [&timer_callback](system_timer_16_384ms_callback callback) { timer_callback = callback; }); + [&timer_callback](system_timer_16_384ms_callback callback, system_timer_4_096ms_callback /*callback*/) { + timer_callback = callback; + }); systemHandle.overrideFunc( []() { throw std::runtime_error{"EXCEPTION TO BREAK LOOP FOR TESTS"}; }); EXPECT_THROW(application_init(), std::runtime_error); - EXPECT_TRUE(systemHandle.functionGotCalled(std::ignore)); EXPECT_TRUE(errorHandlerHandle.functionGotCalled()); EXPECT_TRUE(imuHandle.functionGotCalled()); EXPECT_TRUE(remoteHandlerHandle.functionGotCalled()); @@ -177,7 +178,6 @@ TEST(TEST_NAME, timer_mode_remote) { timer_callback(); EXPECT_TRUE(modeHandlerHandle.functionGotCalled()); EXPECT_TRUE(actuatorsHandle.functionGotCalled()); - EXPECT_TRUE(imuHandle.functionGotCalled()); } TEST(TEST_NAME, timer_mode_stabilised_failsafe) { @@ -192,13 +192,14 @@ TEST(TEST_NAME, timer_mode_stabilised_failsafe) { system_timer_16_384ms_callback timer_callback = nullptr; systemHandle.overrideFunc( - [&timer_callback](system_timer_16_384ms_callback callback) { timer_callback = callback; }); + [&timer_callback](system_timer_16_384ms_callback callback, system_timer_4_096ms_callback /*callback*/) { + timer_callback = callback; + }); systemHandle.overrideFunc( []() { throw std::runtime_error{"EXCEPTION TO BREAK LOOP FOR TESTS"}; }); EXPECT_THROW(application_init(), std::runtime_error); - EXPECT_TRUE(systemHandle.functionGotCalled(std::ignore)); EXPECT_TRUE(errorHandlerHandle.functionGotCalled()); EXPECT_TRUE(imuHandle.functionGotCalled()); EXPECT_TRUE(remoteHandlerHandle.functionGotCalled()); @@ -258,7 +259,6 @@ TEST(TEST_NAME, timer_mode_stabilised_failsafe) { EXPECT_TRUE(modeHandlerHandle.functionGotCalled()); EXPECT_TRUE(controllerHandle.functionGotCalled()); EXPECT_TRUE(actuatorsHandle.functionGotCalled()); - EXPECT_TRUE(imuHandle.functionGotCalled()); } TEST(TEST_NAME, timer_mode_failsafe) { @@ -273,13 +273,14 @@ TEST(TEST_NAME, timer_mode_failsafe) { system_timer_16_384ms_callback timer_callback = nullptr; systemHandle.overrideFunc( - [&timer_callback](system_timer_16_384ms_callback callback) { timer_callback = callback; }); + [&timer_callback](system_timer_16_384ms_callback callback, system_timer_4_096ms_callback /*callback*/) { + timer_callback = callback; + }); systemHandle.overrideFunc( []() { throw std::runtime_error{"EXCEPTION TO BREAK LOOP FOR TESTS"}; }); EXPECT_THROW(application_init(), std::runtime_error); - EXPECT_TRUE(systemHandle.functionGotCalled(std::ignore)); EXPECT_TRUE(errorHandlerHandle.functionGotCalled()); EXPECT_TRUE(imuHandle.functionGotCalled()); EXPECT_TRUE(remoteHandlerHandle.functionGotCalled()); @@ -322,7 +323,6 @@ TEST(TEST_NAME, timer_mode_failsafe) { timer_callback(); EXPECT_TRUE(modeHandlerHandle.functionGotCalled()); EXPECT_TRUE(actuatorsHandle.functionGotCalled()); - EXPECT_TRUE(imuHandle.functionGotCalled()); } TEST(TEST_NAME, timer_send_period) { @@ -337,13 +337,14 @@ TEST(TEST_NAME, timer_send_period) { system_timer_16_384ms_callback timer_callback = nullptr; systemHandle.overrideFunc( - [&timer_callback](system_timer_16_384ms_callback callback) { timer_callback = callback; }); + [&timer_callback](system_timer_16_384ms_callback callback, system_timer_4_096ms_callback /*callback*/) { + timer_callback = callback; + }); systemHandle.overrideFunc( []() { throw std::runtime_error{"EXCEPTION TO BREAK LOOP FOR TESTS"}; }); EXPECT_THROW(application_init(), std::runtime_error); - EXPECT_TRUE(systemHandle.functionGotCalled(std::ignore)); EXPECT_TRUE(errorHandlerHandle.functionGotCalled()); EXPECT_TRUE(imuHandle.functionGotCalled()); EXPECT_TRUE(remoteHandlerHandle.functionGotCalled()); @@ -431,7 +432,6 @@ TEST(TEST_NAME, timer_send_period) { EXPECT_TRUE(controllerHandle.functionGotCalled()); EXPECT_TRUE(actuatorsHandle.functionGotCalled()); EXPECT_FALSE(flightComputerHandle.functionGotCalled()); - EXPECT_TRUE(imuHandle.functionGotCalled()); } timer_callback(); @@ -439,12 +439,10 @@ TEST(TEST_NAME, timer_send_period) { EXPECT_TRUE(controllerHandle.functionGotCalled()); EXPECT_TRUE(actuatorsHandle.functionGotCalled()); EXPECT_TRUE(flightComputerHandle.functionGotCalled()); - EXPECT_TRUE(imuHandle.functionGotCalled()); timer_callback(); EXPECT_TRUE(modeHandlerHandle.functionGotCalled()); EXPECT_TRUE(controllerHandle.functionGotCalled()); EXPECT_TRUE(actuatorsHandle.functionGotCalled()); EXPECT_FALSE(flightComputerHandle.functionGotCalled()); - EXPECT_TRUE(imuHandle.functionGotCalled()); } diff --git a/Tests/LowLevel/Components/system.cpp b/Tests/LowLevel/Components/system.cpp index 98c0eab..b095e02 100644 --- a/Tests/LowLevel/Components/system.cpp +++ b/Tests/LowLevel/Components/system.cpp @@ -9,16 +9,21 @@ extern "C" { #include } -bool timer_got_called = false; -void timer_callback(void) { - timer_got_called = true; +bool high_prio_got_called = false; +void high_prio_callback(void) { + high_prio_got_called = true; } -TEST(TEST_NAME, pre_init_ok) { +bool low_prio_got_called = false; +void low_prio_callback(void) { + low_prio_got_called = true; +} + +TEST(TEST_NAME, pre_init__ok) { auto interruptHandle = mock::interrupt.getHandle(); auto wdtHandle = mock::wdt.getHandle(); - system_pre_init(timer_callback); + system_pre_init(low_prio_callback, high_prio_callback); EXPECT_TRUE(interruptHandle.functionGotCalled()); EXPECT_TRUE(wdtHandle.functionGotCalled(WDTO_250MS)); @@ -26,32 +31,34 @@ TEST(TEST_NAME, pre_init_ok) { EXPECT_EQ(MCUSR, 0); } -TEST(TEST_NAME, pre_init_watchdog) { +TEST(TEST_NAME, pre_init__watchdog) { auto interruptHandle = mock::interrupt.getHandle(); auto wdtHandle = mock::wdt.getHandle(); auto errorHandlerHandle = mock::error_handler.getHandle(); MCUSR |= (1U << WDRF); - system_pre_init(timer_callback); + system_pre_init(low_prio_callback, high_prio_callback); EXPECT_TRUE(interruptHandle.functionGotCalled()); EXPECT_TRUE(wdtHandle.functionGotCalled(WDTO_250MS)); EXPECT_TRUE(interruptHandle.functionGotCalled()); - EXPECT_TRUE(errorHandlerHandle.functionGotCalled(ERROR_HANDLER_GROUP_SYSTEM, SYSTEM_ERROR_WATCHDOG)); + EXPECT_TRUE(errorHandlerHandle.functionGotCalled(ERROR_HANDLER_GROUP_SYSTEM, + SYSTEM_ERROR_WATCHDOG)); } -TEST(TEST_NAME, pre_init_brownout) { +TEST(TEST_NAME, pre_init__brownout) { auto interruptHandle = mock::interrupt.getHandle(); auto wdtHandle = mock::wdt.getHandle(); auto errorHandlerHandle = mock::error_handler.getHandle(); MCUSR |= (1U << BORF); - system_pre_init(timer_callback); + system_pre_init(low_prio_callback, high_prio_callback); EXPECT_TRUE(interruptHandle.functionGotCalled()); EXPECT_TRUE(wdtHandle.functionGotCalled(WDTO_250MS)); EXPECT_TRUE(interruptHandle.functionGotCalled()); - EXPECT_TRUE(errorHandlerHandle.functionGotCalled(ERROR_HANDLER_GROUP_SYSTEM, SYSTEM_ERROR_BROWNOUT)); + EXPECT_TRUE(errorHandlerHandle.functionGotCalled(ERROR_HANDLER_GROUP_SYSTEM, + SYSTEM_ERROR_BROWNOUT)); } TEST(TEST_NAME, post_init) { @@ -59,29 +66,35 @@ TEST(TEST_NAME, post_init) { auto wdtHandle = mock::wdt.getHandle(); auto timerHandle = mock::timer8bit.getHandle(); - system_pre_init(timer_callback); + system_pre_init(low_prio_callback, high_prio_callback); EXPECT_TRUE(interruptHandle.functionGotCalled()); EXPECT_TRUE(wdtHandle.functionGotCalled(WDTO_250MS)); EXPECT_TRUE(interruptHandle.functionGotCalled()); system_post_init(); - EXPECT_TRUE(timerHandle.functionGotCalled(prescaler_1024, std::ignore)); + EXPECT_TRUE(timerHandle.functionGotCalled(TIMER_ID_0, prescaler_1024, std::ignore)); + EXPECT_TRUE(timerHandle.functionGotCalled(TIMER_ID_2, prescaler_256, std::ignore)); EXPECT_TRUE(wdtHandle.functionGotCalled(WDTO_30MS)); } -TEST(TEST_NAME, timer_runtime_0) { +TEST(TEST_NAME, timer_low_prio_callback) { auto interruptHandle = mock::interrupt.getHandle(); auto wdtHandle = mock::wdt.getHandle(); auto timerHandle = mock::timer8bit.getHandle(); + auto errorHandlerHandle = mock::error_handler.getHandle(); timer_8bit_callback_t internalCallback = nullptr; - timerHandle.overrideFunc( - [&internalCallback](timer_8bit_clock_options /*clock_option*/, - system_timer_16_384ms_callback timerCallback) { internalCallback = timerCallback; }); - timerHandle.overrideFunc([]() { return 0; }); - - system_pre_init(timer_callback); + timerHandle.overrideFunc([&internalCallback](timer_8bit_id_t timerId, + timer_8bit_clock_option_t /*clock_option*/, + timer_8bit_callback_t timerCallback) { + if (timerId == TIMER_ID_0) { + internalCallback = timerCallback; + } + }); + timerHandle.overrideFunc([](timer_8bit_id_t /*timerId*/) { return 0; }); + + system_pre_init(low_prio_callback, high_prio_callback); EXPECT_TRUE(interruptHandle.functionGotCalled()); EXPECT_TRUE(wdtHandle.functionGotCalled(WDTO_250MS)); EXPECT_TRUE(interruptHandle.functionGotCalled()); @@ -89,25 +102,56 @@ TEST(TEST_NAME, timer_runtime_0) { system_post_init(); EXPECT_TRUE(wdtHandle.functionGotCalled(WDTO_30MS)); - timer_got_called = false; + low_prio_got_called = false; internalCallback(); - EXPECT_TRUE(timer_got_called); - EXPECT_TRUE(timerHandle.functionGotCalled()); + EXPECT_TRUE(low_prio_got_called); + EXPECT_TRUE(SREG & (1 << SREG_I)); } -TEST(TEST_NAME, timer_runtime_max) { +TEST(TEST_NAME, timer_low_prio_runtime_0) { auto interruptHandle = mock::interrupt.getHandle(); auto wdtHandle = mock::wdt.getHandle(); auto timerHandle = mock::timer8bit.getHandle(); auto errorHandlerHandle = mock::error_handler.getHandle(); timer_8bit_callback_t internalCallback = nullptr; - timerHandle.overrideFunc( - [&internalCallback](timer_8bit_clock_options /*clock_option*/, - system_timer_16_384ms_callback timerCallback) { internalCallback = timerCallback; }); - timerHandle.overrideFunc([]() { return 255; }); + timerHandle.overrideFunc([&internalCallback](timer_8bit_id_t timerId, + timer_8bit_clock_option_t /*clock_option*/, + timer_8bit_callback_t timerCallback) { + if (timerId == TIMER_ID_0) { + internalCallback = timerCallback; + } + }); + timerHandle.overrideFunc([](timer_8bit_id_t /*timerId*/) { return 0; }); + + system_pre_init(low_prio_callback, high_prio_callback); + EXPECT_TRUE(interruptHandle.functionGotCalled()); + EXPECT_TRUE(wdtHandle.functionGotCalled(WDTO_250MS)); + EXPECT_TRUE(interruptHandle.functionGotCalled()); + + system_post_init(); + EXPECT_TRUE(wdtHandle.functionGotCalled(WDTO_30MS)); + + internalCallback(); +} - system_pre_init(timer_callback); +TEST(TEST_NAME, timer_low_prio_runtime_max) { + auto interruptHandle = mock::interrupt.getHandle(); + auto wdtHandle = mock::wdt.getHandle(); + auto timerHandle = mock::timer8bit.getHandle(); + auto errorHandlerHandle = mock::error_handler.getHandle(); + + timer_8bit_callback_t internalCallback = nullptr; + timerHandle.overrideFunc([&internalCallback](timer_8bit_id_t timerId, + timer_8bit_clock_option_t /*clock_option*/, + timer_8bit_callback_t timerCallback) { + if (timerId == TIMER_ID_0) { + internalCallback = timerCallback; + } + }); + timerHandle.overrideFunc([](timer_8bit_id_t /*timerId*/) { return 255; }); + + system_pre_init(low_prio_callback, high_prio_callback); EXPECT_TRUE(interruptHandle.functionGotCalled()); EXPECT_TRUE(wdtHandle.functionGotCalled(WDTO_250MS)); EXPECT_TRUE(interruptHandle.functionGotCalled()); @@ -115,26 +159,30 @@ TEST(TEST_NAME, timer_runtime_max) { system_post_init(); EXPECT_TRUE(wdtHandle.functionGotCalled(WDTO_30MS)); - timer_got_called = false; internalCallback(); - EXPECT_TRUE(timer_got_called); - EXPECT_TRUE(timerHandle.functionGotCalled()); - EXPECT_TRUE(errorHandlerHandle.functionGotCalled(ERROR_HANDLER_GROUP_SYSTEM, SYSTEM_ERROR_TIMER_RUNTIME)); + EXPECT_TRUE(errorHandlerHandle.functionGotCalled(ERROR_HANDLER_GROUP_SYSTEM, + SYSTEM_ERROR_RUNTIME_LOW_PRIO)); } -TEST(TEST_NAME, timer_runtime_under_limit) { +TEST(TEST_NAME, timer_low_prio_runtime_under_limit) { auto interruptHandle = mock::interrupt.getHandle(); auto wdtHandle = mock::wdt.getHandle(); auto timerHandle = mock::timer8bit.getHandle(); + auto errorHandlerHandle = mock::error_handler.getHandle(); timer_8bit_callback_t internalCallback = nullptr; - timerHandle.overrideFunc( - [&internalCallback](timer_8bit_clock_options /*clock_option*/, - system_timer_16_384ms_callback timerCallback) { internalCallback = timerCallback; }); + timerHandle.overrideFunc([&internalCallback](timer_8bit_id_t timerId, + timer_8bit_clock_option_t /*clock_option*/, + timer_8bit_callback_t timerCallback) { + if (timerId == TIMER_ID_0) { + internalCallback = timerCallback; + } + }); + // 12/16,384*255=186.767578125 - timerHandle.overrideFunc([]() { return 186; }); + timerHandle.overrideFunc([](timer_8bit_id_t /*timerId*/) { return 186; }); - system_pre_init(timer_callback); + system_pre_init(low_prio_callback, high_prio_callback); EXPECT_TRUE(interruptHandle.functionGotCalled()); EXPECT_TRUE(wdtHandle.functionGotCalled(WDTO_250MS)); EXPECT_TRUE(interruptHandle.functionGotCalled()); @@ -142,26 +190,173 @@ TEST(TEST_NAME, timer_runtime_under_limit) { system_post_init(); EXPECT_TRUE(wdtHandle.functionGotCalled(WDTO_30MS)); - timer_got_called = false; internalCallback(); - EXPECT_TRUE(timer_got_called); - EXPECT_TRUE(timerHandle.functionGotCalled()); } -TEST(TEST_NAME, timer_runtime_over_limit) { +TEST(TEST_NAME, timer_low_prio_runtime_over_limit) { auto interruptHandle = mock::interrupt.getHandle(); auto wdtHandle = mock::wdt.getHandle(); auto timerHandle = mock::timer8bit.getHandle(); auto errorHandlerHandle = mock::error_handler.getHandle(); timer_8bit_callback_t internalCallback = nullptr; - timerHandle.overrideFunc( - [&internalCallback](timer_8bit_clock_options /*clock_option*/, - system_timer_16_384ms_callback timerCallback) { internalCallback = timerCallback; }); + timerHandle.overrideFunc([&internalCallback](timer_8bit_id_t timerId, + timer_8bit_clock_option_t /*clock_option*/, + timer_8bit_callback_t timerCallback) { + if (timerId == TIMER_ID_0) { + internalCallback = timerCallback; + } + }); + // 12/16,384*255=186.767578125 - timerHandle.overrideFunc([]() { return 187; }); + timerHandle.overrideFunc([](timer_8bit_id_t /*timerId*/) { return 187; }); + + system_pre_init(low_prio_callback, high_prio_callback); + EXPECT_TRUE(interruptHandle.functionGotCalled()); + EXPECT_TRUE(wdtHandle.functionGotCalled(WDTO_250MS)); + EXPECT_TRUE(interruptHandle.functionGotCalled()); + + system_post_init(); + EXPECT_TRUE(wdtHandle.functionGotCalled(WDTO_30MS)); + + internalCallback(); + EXPECT_TRUE(errorHandlerHandle.functionGotCalled(ERROR_HANDLER_GROUP_SYSTEM, + SYSTEM_ERROR_RUNTIME_LOW_PRIO)); +} + +TEST(TEST_NAME, timer_high_prio_callback) { + auto interruptHandle = mock::interrupt.getHandle(); + auto wdtHandle = mock::wdt.getHandle(); + auto timerHandle = mock::timer8bit.getHandle(); + auto errorHandlerHandle = mock::error_handler.getHandle(); + + timer_8bit_callback_t internalCallback = nullptr; + timerHandle.overrideFunc([&internalCallback](timer_8bit_id_t timerId, + timer_8bit_clock_option_t /*clock_option*/, + timer_8bit_callback_t timerCallback) { + if (timerId == TIMER_ID_2) { + internalCallback = timerCallback; + } + }); + timerHandle.overrideFunc([](timer_8bit_id_t /*timerId*/) { return 0; }); + + system_pre_init(high_prio_callback, high_prio_callback); + EXPECT_TRUE(interruptHandle.functionGotCalled()); + EXPECT_TRUE(wdtHandle.functionGotCalled(WDTO_250MS)); + EXPECT_TRUE(interruptHandle.functionGotCalled()); + + system_post_init(); + EXPECT_TRUE(wdtHandle.functionGotCalled(WDTO_30MS)); + + high_prio_got_called = false; + internalCallback(); + EXPECT_TRUE(high_prio_got_called); +} + +TEST(TEST_NAME, timer_high_prio_runtime_0) { + auto interruptHandle = mock::interrupt.getHandle(); + auto wdtHandle = mock::wdt.getHandle(); + auto timerHandle = mock::timer8bit.getHandle(); + auto errorHandlerHandle = mock::error_handler.getHandle(); + + timer_8bit_callback_t internalCallback = nullptr; + timerHandle.overrideFunc([&internalCallback](timer_8bit_id_t timerId, + timer_8bit_clock_option_t /*clock_option*/, + timer_8bit_callback_t timerCallback) { + if (timerId == TIMER_ID_2) { + internalCallback = timerCallback; + } + }); + timerHandle.overrideFunc([](timer_8bit_id_t /*timerId*/) { return 0; }); + + system_pre_init(high_prio_callback, high_prio_callback); + EXPECT_TRUE(interruptHandle.functionGotCalled()); + EXPECT_TRUE(wdtHandle.functionGotCalled(WDTO_250MS)); + EXPECT_TRUE(interruptHandle.functionGotCalled()); + + system_post_init(); + EXPECT_TRUE(wdtHandle.functionGotCalled(WDTO_30MS)); + + internalCallback(); +} + +TEST(TEST_NAME, timer_high_prio_runtime_max) { + auto interruptHandle = mock::interrupt.getHandle(); + auto wdtHandle = mock::wdt.getHandle(); + auto timerHandle = mock::timer8bit.getHandle(); + auto errorHandlerHandle = mock::error_handler.getHandle(); + + timer_8bit_callback_t internalCallback = nullptr; + timerHandle.overrideFunc([&internalCallback](timer_8bit_id_t timerId, + timer_8bit_clock_option_t /*clock_option*/, + timer_8bit_callback_t timerCallback) { + if (timerId == TIMER_ID_2) { + internalCallback = timerCallback; + } + }); + timerHandle.overrideFunc([](timer_8bit_id_t /*timerId*/) { return 255; }); + + system_pre_init(high_prio_callback, high_prio_callback); + EXPECT_TRUE(interruptHandle.functionGotCalled()); + EXPECT_TRUE(wdtHandle.functionGotCalled(WDTO_250MS)); + EXPECT_TRUE(interruptHandle.functionGotCalled()); + + system_post_init(); + EXPECT_TRUE(wdtHandle.functionGotCalled(WDTO_30MS)); - system_pre_init(timer_callback); + internalCallback(); + EXPECT_TRUE(errorHandlerHandle.functionGotCalled(ERROR_HANDLER_GROUP_SYSTEM, + SYSTEM_ERROR_RUNTIME_HIGH_PRIO)); +} + +TEST(TEST_NAME, timer_high_prio_runtime_under_limit) { + auto interruptHandle = mock::interrupt.getHandle(); + auto wdtHandle = mock::wdt.getHandle(); + auto timerHandle = mock::timer8bit.getHandle(); + auto errorHandlerHandle = mock::error_handler.getHandle(); + + timer_8bit_callback_t internalCallback = nullptr; + timerHandle.overrideFunc([&internalCallback](timer_8bit_id_t timerId, + timer_8bit_clock_option_t /*clock_option*/, + timer_8bit_callback_t timerCallback) { + if (timerId == TIMER_ID_2) { + internalCallback = timerCallback; + } + }); + + // 1/4.096*255=62.255 + timerHandle.overrideFunc([](timer_8bit_id_t /*timerId*/) { return 62; }); + + system_pre_init(high_prio_callback, high_prio_callback); + EXPECT_TRUE(interruptHandle.functionGotCalled()); + EXPECT_TRUE(wdtHandle.functionGotCalled(WDTO_250MS)); + EXPECT_TRUE(interruptHandle.functionGotCalled()); + + system_post_init(); + EXPECT_TRUE(wdtHandle.functionGotCalled(WDTO_30MS)); + + internalCallback(); +} + +TEST(TEST_NAME, timer_high_prio_runtime_over_limit) { + auto interruptHandle = mock::interrupt.getHandle(); + auto wdtHandle = mock::wdt.getHandle(); + auto timerHandle = mock::timer8bit.getHandle(); + auto errorHandlerHandle = mock::error_handler.getHandle(); + + timer_8bit_callback_t internalCallback = nullptr; + timerHandle.overrideFunc([&internalCallback](timer_8bit_id_t timerId, + timer_8bit_clock_option_t /*clock_option*/, + timer_8bit_callback_t timerCallback) { + if (timerId == TIMER_ID_2) { + internalCallback = timerCallback; + } + }); + + // 1/4.096*255=62.255 + timerHandle.overrideFunc([](timer_8bit_id_t /*timerId*/) { return 63; }); + + system_pre_init(high_prio_callback, high_prio_callback); EXPECT_TRUE(interruptHandle.functionGotCalled()); EXPECT_TRUE(wdtHandle.functionGotCalled(WDTO_250MS)); EXPECT_TRUE(interruptHandle.functionGotCalled()); @@ -169,14 +364,12 @@ TEST(TEST_NAME, timer_runtime_over_limit) { system_post_init(); EXPECT_TRUE(wdtHandle.functionGotCalled(WDTO_30MS)); - timer_got_called = false; internalCallback(); - EXPECT_TRUE(timer_got_called); - EXPECT_TRUE(timerHandle.functionGotCalled()); - EXPECT_TRUE(errorHandlerHandle.functionGotCalled(ERROR_HANDLER_GROUP_SYSTEM, SYSTEM_ERROR_TIMER_RUNTIME)); + EXPECT_TRUE(errorHandlerHandle.functionGotCalled(ERROR_HANDLER_GROUP_SYSTEM, + SYSTEM_ERROR_RUNTIME_HIGH_PRIO)); } -TEST(TEST_NAME, timer_reset_watchdog) { +TEST(TEST_NAME, reset_watchdog) { auto wdtHandle = mock::wdt.getHandle(); system_reset_watchdog(); EXPECT_TRUE(wdtHandle.functionGotCalled()); diff --git a/Tests/LowLevel/Mock b/Tests/LowLevel/Mock index 5809143..fdd4f01 160000 --- a/Tests/LowLevel/Mock +++ b/Tests/LowLevel/Mock @@ -1 +1 @@ -Subproject commit 5809143a7e813810a63278e4b1b4a56aabe182e0 +Subproject commit fdd4f0176d4c4aa7718e31d1d3f89a2d2dfe486a From 36a2c3e275cf0cbbc3febbd7488b89be026bf37f Mon Sep 17 00:00:00 2001 From: Paul Nykiel Date: Sun, 22 Jan 2023 18:50:03 +0100 Subject: [PATCH 07/17] Format + doc --- Src/Application/application.h | 2 +- Src/Components/flight_computer.c | 10 +-- Src/Components/imu.c | 3 +- Tests/LowLevel/Components/imu.cpp | 137 +++++++++++++++++------------- 4 files changed, 84 insertions(+), 68 deletions(-) diff --git a/Src/Application/application.h b/Src/Application/application.h index 51670bb..5e37717 100644 --- a/Src/Application/application.h +++ b/Src/Application/application.h @@ -38,7 +38,7 @@ * and use the result as elevon commands, the motor command is always 0 * * In failsafe mode: set all three commands to 0 * * Pass the actuator commands to the actuators (::actuators_set) - * * Every FLIGHT_COMPUTER_SEND_PERIOD frame: pass the current data to ::flight-computer_send + * * Every FLIGHT_COMPUTER_SEND_PERIOD frame: pass the current data to ::flight_computer_send * * Call ::imu_start_sampling * */ diff --git a/Src/Components/flight_computer.c b/Src/Components/flight_computer.c index ee11776..93c98d8 100644 --- a/Src/Components/flight_computer.c +++ b/Src/Components/flight_computer.c @@ -5,16 +5,16 @@ * @brief Implementation of the Flight-Computer interface component. * @ingroup Components */ -#include - #include "flight_computer.h" +#include + void flight_computer_init(void) { protobuf_init(); } void flight_computer_send(const imu_data_t *imu_data, const remote_data_t *remote_data, - const actuator_cmd_t *actuator_cmd) { + const actuator_cmd_t *actuator_cmd) { fc_message_t message = {.has_imu = true, .imu = { @@ -50,8 +50,8 @@ bool flight_computer_set_point_available(void) { flight_computer_set_point_t flight_computer_get_set_point(void) { set_point_message_t message = protobuf_get(); flight_computer_set_point_t setpoint = {.motor = message.motor, - .pitch = (int16_t) message.pitch, - .roll = (int16_t) message.roll}; + .pitch = (int16_t) message.pitch, + .roll = (int16_t) message.roll}; return setpoint; } diff --git a/Src/Components/imu.c b/Src/Components/imu.c index 92e2970..c4c5a0d 100644 --- a/Src/Components/imu.c +++ b/Src/Components/imu.c @@ -156,7 +156,6 @@ void imu_init(void) { response = read_success; current_sample_state_id = 1; current_sample_state = CALIB_STAT; - } void imu_start_sampling(void) { @@ -228,7 +227,7 @@ void imu_start_sampling(void) { } } } else { - //error_handler_handle_warning(ERROR_HANDLER_GROUP_IMU, IMU_ERROR_READ_TIMEOUT); + // error_handler_handle_warning(ERROR_HANDLER_GROUP_IMU, IMU_ERROR_READ_TIMEOUT); } } diff --git a/Tests/LowLevel/Components/imu.cpp b/Tests/LowLevel/Components/imu.cpp index 84db3b1..b3472d1 100644 --- a/Tests/LowLevel/Components/imu.cpp +++ b/Tests/LowLevel/Components/imu.cpp @@ -18,7 +18,8 @@ TEST(TEST_NAME, init__config_mode_error) { imu_init(); EXPECT_TRUE(bnoHandle.functionGotCalled()); EXPECT_TRUE(delayHandle.functionGotCalled<_delay_ms>(20)); - EXPECT_TRUE(errorHandlerHandle.functionGotCalled(ERROR_HANDLER_GROUP_BNO055, write_fail + 1)); + EXPECT_TRUE(errorHandlerHandle.functionGotCalled(ERROR_HANDLER_GROUP_BNO055, + write_fail + 1)); EXPECT_FALSE(imu_get_latest_data().imu_ok); EXPECT_FALSE(imu_data_available()); } @@ -32,7 +33,8 @@ TEST(TEST_NAME, init__config_mode_timeout) { EXPECT_TRUE(bnoHandle.functionGotCalled()); EXPECT_TRUE(bnoHandle.functionGotCalled(BNO055_OPR_MODE_CONFIG_MODE, std::ignore)); EXPECT_TRUE(delayHandle.functionGotCalled<_delay_ms>(20)); - EXPECT_TRUE(errorHandlerHandle.functionGotCalled(ERROR_HANDLER_GROUP_IMU, IMU_ERROR_INIT_TIMEOUT)); + EXPECT_TRUE(errorHandlerHandle.functionGotCalled(ERROR_HANDLER_GROUP_IMU, + IMU_ERROR_INIT_TIMEOUT)); EXPECT_FALSE(imu_get_latest_data().imu_ok); EXPECT_FALSE(imu_data_available()); } @@ -56,7 +58,8 @@ TEST(TEST_NAME, init__self_test_error) { EXPECT_TRUE(bnoHandle.functionGotCalled()); EXPECT_TRUE(delayHandle.functionGotCalled<_delay_ms>(20)); EXPECT_TRUE(delayHandle.functionGotCalled<_delay_ms>(20)); - EXPECT_TRUE(errorHandlerHandle.functionGotCalled(ERROR_HANDLER_GROUP_BNO055, read_fail + 1)); + EXPECT_TRUE(errorHandlerHandle.functionGotCalled(ERROR_HANDLER_GROUP_BNO055, + read_fail + 1)); EXPECT_FALSE(imu_get_latest_data().imu_ok); EXPECT_FALSE(imu_data_available()); } @@ -80,7 +83,8 @@ TEST(TEST_NAME, init__self_test_mcu_fail) { EXPECT_TRUE(bnoHandle.functionGotCalled()); EXPECT_TRUE(delayHandle.functionGotCalled<_delay_ms>(20)); EXPECT_TRUE(delayHandle.functionGotCalled<_delay_ms>(20)); - EXPECT_TRUE(errorHandlerHandle.functionGotCalled(ERROR_HANDLER_GROUP_IMU, IMU_ERROR_INIT_SELF_TEST)); + EXPECT_TRUE(errorHandlerHandle.functionGotCalled(ERROR_HANDLER_GROUP_IMU, + IMU_ERROR_INIT_SELF_TEST)); EXPECT_FALSE(imu_get_latest_data().imu_ok); EXPECT_FALSE(imu_data_available()); } @@ -104,7 +108,8 @@ TEST(TEST_NAME, init__self_test_acc_fail) { EXPECT_TRUE(bnoHandle.functionGotCalled()); EXPECT_TRUE(delayHandle.functionGotCalled<_delay_ms>(20)); EXPECT_TRUE(delayHandle.functionGotCalled<_delay_ms>(20)); - EXPECT_TRUE(errorHandlerHandle.functionGotCalled(ERROR_HANDLER_GROUP_IMU, IMU_ERROR_INIT_SELF_TEST)); + EXPECT_TRUE(errorHandlerHandle.functionGotCalled(ERROR_HANDLER_GROUP_IMU, + IMU_ERROR_INIT_SELF_TEST)); EXPECT_FALSE(imu_get_latest_data().imu_ok); EXPECT_FALSE(imu_data_available()); } @@ -128,7 +133,8 @@ TEST(TEST_NAME, init__self_test_gyr_fail) { EXPECT_TRUE(bnoHandle.functionGotCalled()); EXPECT_TRUE(delayHandle.functionGotCalled<_delay_ms>(20)); EXPECT_TRUE(delayHandle.functionGotCalled<_delay_ms>(20)); - EXPECT_TRUE(errorHandlerHandle.functionGotCalled(ERROR_HANDLER_GROUP_IMU, IMU_ERROR_INIT_SELF_TEST)); + EXPECT_TRUE(errorHandlerHandle.functionGotCalled(ERROR_HANDLER_GROUP_IMU, + IMU_ERROR_INIT_SELF_TEST)); EXPECT_FALSE(imu_get_latest_data().imu_ok); EXPECT_FALSE(imu_data_available()); } @@ -152,7 +158,8 @@ TEST(TEST_NAME, init__self_test_mag_fail) { EXPECT_TRUE(bnoHandle.functionGotCalled()); EXPECT_TRUE(delayHandle.functionGotCalled<_delay_ms>(20)); EXPECT_TRUE(delayHandle.functionGotCalled<_delay_ms>(20)); - EXPECT_TRUE(errorHandlerHandle.functionGotCalled(ERROR_HANDLER_GROUP_IMU, IMU_ERROR_INIT_SELF_TEST)); + EXPECT_TRUE(errorHandlerHandle.functionGotCalled(ERROR_HANDLER_GROUP_IMU, + IMU_ERROR_INIT_SELF_TEST)); EXPECT_FALSE(imu_get_latest_data().imu_ok); EXPECT_FALSE(imu_data_available()); } @@ -170,7 +177,8 @@ TEST(TEST_NAME, init__self_test_timeout) { EXPECT_TRUE(delayHandle.functionGotCalled<_delay_ms>(20)); EXPECT_TRUE(bnoHandle.functionGotCalled()); EXPECT_TRUE(delayHandle.functionGotCalled<_delay_ms>(20)); - EXPECT_TRUE(errorHandlerHandle.functionGotCalled(ERROR_HANDLER_GROUP_IMU, IMU_ERROR_INIT_TIMEOUT)); + EXPECT_TRUE(errorHandlerHandle.functionGotCalled(ERROR_HANDLER_GROUP_IMU, + IMU_ERROR_INIT_TIMEOUT)); EXPECT_FALSE(imu_get_latest_data().imu_ok); EXPECT_FALSE(imu_data_available()); } @@ -200,7 +208,8 @@ TEST(TEST_NAME, init__unit_sel_error) { EXPECT_TRUE(delayHandle.functionGotCalled<_delay_ms>(20)); EXPECT_TRUE(delayHandle.functionGotCalled<_delay_ms>(20)); EXPECT_TRUE(delayHandle.functionGotCalled<_delay_ms>(20)); - EXPECT_TRUE(errorHandlerHandle.functionGotCalled(ERROR_HANDLER_GROUP_BNO055, write_fail + 1)); + EXPECT_TRUE(errorHandlerHandle.functionGotCalled(ERROR_HANDLER_GROUP_BNO055, + write_fail + 1)); EXPECT_FALSE(imu_get_latest_data().imu_ok); EXPECT_FALSE(imu_data_available()); } @@ -226,7 +235,8 @@ TEST(TEST_NAME, init__unit_sel_timeout) { EXPECT_TRUE(delayHandle.functionGotCalled<_delay_ms>(20)); EXPECT_TRUE(bnoHandle.functionGotCalled()); EXPECT_TRUE(delayHandle.functionGotCalled<_delay_ms>(20)); - EXPECT_TRUE(errorHandlerHandle.functionGotCalled(ERROR_HANDLER_GROUP_IMU, IMU_ERROR_INIT_TIMEOUT)); + EXPECT_TRUE(errorHandlerHandle.functionGotCalled(ERROR_HANDLER_GROUP_IMU, + IMU_ERROR_INIT_TIMEOUT)); EXPECT_FALSE(imu_get_latest_data().imu_ok); EXPECT_FALSE(imu_data_available()); } @@ -260,7 +270,8 @@ TEST(TEST_NAME, init__remap_error) { EXPECT_TRUE(delayHandle.functionGotCalled<_delay_ms>(20)); EXPECT_TRUE(delayHandle.functionGotCalled<_delay_ms>(20)); EXPECT_TRUE(delayHandle.functionGotCalled<_delay_ms>(20)); - EXPECT_TRUE(errorHandlerHandle.functionGotCalled(ERROR_HANDLER_GROUP_BNO055, write_fail + 1)); + EXPECT_TRUE(errorHandlerHandle.functionGotCalled(ERROR_HANDLER_GROUP_BNO055, + write_fail + 1)); EXPECT_FALSE(imu_get_latest_data().imu_ok); EXPECT_FALSE(imu_data_available()); } @@ -292,7 +303,8 @@ TEST(TEST_NAME, init__remap_timeout) { EXPECT_TRUE(delayHandle.functionGotCalled<_delay_ms>(20)); EXPECT_TRUE(delayHandle.functionGotCalled<_delay_ms>(20)); EXPECT_TRUE(bnoHandle.functionGotCalled()); - EXPECT_TRUE(errorHandlerHandle.functionGotCalled(ERROR_HANDLER_GROUP_IMU, IMU_ERROR_INIT_TIMEOUT)); + EXPECT_TRUE(errorHandlerHandle.functionGotCalled(ERROR_HANDLER_GROUP_IMU, + IMU_ERROR_INIT_TIMEOUT)); EXPECT_FALSE(imu_get_latest_data().imu_ok); EXPECT_FALSE(imu_data_available()); } @@ -330,7 +342,8 @@ TEST(TEST_NAME, init__remap_sign_error) { EXPECT_TRUE(delayHandle.functionGotCalled<_delay_ms>(20)); EXPECT_TRUE(delayHandle.functionGotCalled<_delay_ms>(20)); EXPECT_TRUE(delayHandle.functionGotCalled<_delay_ms>(20)); - EXPECT_TRUE(errorHandlerHandle.functionGotCalled(ERROR_HANDLER_GROUP_BNO055, write_fail + 1)); + EXPECT_TRUE(errorHandlerHandle.functionGotCalled(ERROR_HANDLER_GROUP_BNO055, + write_fail + 1)); EXPECT_FALSE(imu_get_latest_data().imu_ok); } @@ -365,7 +378,8 @@ TEST(TEST_NAME, init__remap_sign_timeout) { EXPECT_TRUE(delayHandle.functionGotCalled<_delay_ms>(20)); EXPECT_TRUE(delayHandle.functionGotCalled<_delay_ms>(20)); EXPECT_TRUE(bnoHandle.functionGotCalled()); - EXPECT_TRUE(errorHandlerHandle.functionGotCalled(ERROR_HANDLER_GROUP_IMU, IMU_ERROR_INIT_TIMEOUT)); + EXPECT_TRUE(errorHandlerHandle.functionGotCalled(ERROR_HANDLER_GROUP_IMU, + IMU_ERROR_INIT_TIMEOUT)); EXPECT_FALSE(imu_get_latest_data().imu_ok); } @@ -408,7 +422,8 @@ TEST(TEST_NAME, init__ndof_error) { EXPECT_TRUE(delayHandle.functionGotCalled<_delay_ms>(20)); EXPECT_TRUE(delayHandle.functionGotCalled<_delay_ms>(20)); EXPECT_TRUE(delayHandle.functionGotCalled<_delay_ms>(20)); - EXPECT_TRUE(errorHandlerHandle.functionGotCalled(ERROR_HANDLER_GROUP_BNO055, write_fail + 1)); + EXPECT_TRUE(errorHandlerHandle.functionGotCalled(ERROR_HANDLER_GROUP_BNO055, + write_fail + 1)); EXPECT_FALSE(imu_get_latest_data().imu_ok); EXPECT_FALSE(imu_data_available()); } @@ -450,7 +465,8 @@ TEST(TEST_NAME, init__ndof_timeout) { EXPECT_TRUE(delayHandle.functionGotCalled<_delay_ms>(20)); EXPECT_TRUE(delayHandle.functionGotCalled<_delay_ms>(20)); EXPECT_TRUE(delayHandle.functionGotCalled<_delay_ms>(20)); - EXPECT_TRUE(errorHandlerHandle.functionGotCalled(ERROR_HANDLER_GROUP_IMU, IMU_ERROR_INIT_TIMEOUT)); + EXPECT_TRUE(errorHandlerHandle.functionGotCalled(ERROR_HANDLER_GROUP_IMU, + IMU_ERROR_INIT_TIMEOUT)); EXPECT_FALSE(imu_get_latest_data().imu_ok); EXPECT_FALSE(imu_data_available()); } @@ -488,15 +504,16 @@ TEST(TEST_NAME, init__success) { EXPECT_TRUE(delayHandle.functionGotCalled<_delay_ms>(20)); EXPECT_TRUE(bnoHandle.functionGotCalled(std::ignore, std::ignore)); EXPECT_TRUE(delayHandle.functionGotCalled<_delay_ms>(20)); - EXPECT_TRUE(bnoHandle.functionGotCalled(BNO055_UNIT_SEL_ACC_MPS2, BNO055_UNIT_SEL_ANGULAR_RATE_DPS, BNO055_UNIT_SEL_EULER_ANGLES_DEGREES, - BNO055_UNIT_SEL_TEMPERATURE_CELSIUS, BNO055_UNIT_SEL_ORIENTATION_DEF_WINDOWS, - std::ignore)); + EXPECT_TRUE(bnoHandle.functionGotCalled( + BNO055_UNIT_SEL_ACC_MPS2, BNO055_UNIT_SEL_ANGULAR_RATE_DPS, BNO055_UNIT_SEL_EULER_ANGLES_DEGREES, + BNO055_UNIT_SEL_TEMPERATURE_CELSIUS, BNO055_UNIT_SEL_ORIENTATION_DEF_WINDOWS, std::ignore)); EXPECT_TRUE(delayHandle.functionGotCalled<_delay_ms>(20)); EXPECT_TRUE(bnoHandle.functionGotCalled(BNO055_AXIS_REMAP_Y_AXIS, BNO055_AXIS_REMAP_X_AXIS, BNO055_AXIS_REMAP_Z_AXIS, std::ignore)); EXPECT_TRUE(delayHandle.functionGotCalled<_delay_ms>(20)); EXPECT_TRUE(bnoHandle.functionGotCalled( - BNO055_AXIS_REMAP_SIGN_POSITIVE, BNO055_AXIS_REMAP_SIGN_POSITIVE, BNO055_AXIS_REMAP_SIGN_NEGATIVE, std::ignore)); + BNO055_AXIS_REMAP_SIGN_POSITIVE, BNO055_AXIS_REMAP_SIGN_POSITIVE, BNO055_AXIS_REMAP_SIGN_NEGATIVE, + std::ignore)); EXPECT_TRUE(delayHandle.functionGotCalled<_delay_ms>(20)); EXPECT_TRUE(bnoHandle.functionGotCalled(BNO055_OPR_MODE_NDOF_FMC_OFF, std::ignore)); EXPECT_TRUE(delayHandle.functionGotCalled<_delay_ms>(20)); @@ -512,11 +529,11 @@ TEST(TEST_NAME, start_sampling__no_response) { bnoHandle.overrideFunc( [](auto /*op_mode*/, bno055_callback_t callback) { callback(write_success); }); bnoHandle.overrideFunc([](bno055_self_test_result_t *out, bno055_callback_t callback) { - out->mcu_passed = true; - out->acc_passed = true; - out->gyr_passed = true; - out->mag_passed = true; - callback(read_success); + out->mcu_passed = true; + out->acc_passed = true; + out->gyr_passed = true; + out->mag_passed = true; + callback(read_success); }); bnoHandle.overrideFunc( [](bno055_unit_sel_acc /*acc_unit*/, bno055_unit_sel_angular_rate /*angular_rate_unit*/, @@ -558,11 +575,11 @@ TEST(TEST_NAME, start_sampling__read_error) { bnoHandle.overrideFunc( [](auto /*op_mode*/, bno055_callback_t callback) { callback(write_success); }); bnoHandle.overrideFunc([](bno055_self_test_result_t *out, bno055_callback_t callback) { - out->mcu_passed = true; - out->acc_passed = true; - out->gyr_passed = true; - out->mag_passed = true; - callback(read_success); + out->mcu_passed = true; + out->acc_passed = true; + out->gyr_passed = true; + out->mag_passed = true; + callback(read_success); }); bnoHandle.overrideFunc( [](bno055_unit_sel_acc /*acc_unit*/, bno055_unit_sel_angular_rate /*angular_rate_unit*/, @@ -605,11 +622,11 @@ TEST(TEST_NAME, start_sampling__full_cycle) { bnoHandle.overrideFunc( [](auto /*op_mode*/, bno055_callback_t callback) { callback(write_success); }); bnoHandle.overrideFunc([](bno055_self_test_result_t *out, bno055_callback_t callback) { - out->mcu_passed = true; - out->acc_passed = true; - out->gyr_passed = true; - out->mag_passed = true; - callback(read_success); + out->mcu_passed = true; + out->acc_passed = true; + out->gyr_passed = true; + out->mag_passed = true; + callback(read_success); }); bnoHandle.overrideFunc( [](bno055_unit_sel_acc /*acc_unit*/, bno055_unit_sel_angular_rate /*angular_rate_unit*/, @@ -670,11 +687,11 @@ TEST(TEST_NAME, start_sampling__status_read_error) { bnoHandle.overrideFunc( [](auto /*op_mode*/, bno055_callback_t callback) { callback(write_success); }); bnoHandle.overrideFunc([](bno055_self_test_result_t *out, bno055_callback_t callback) { - out->mcu_passed = true; - out->acc_passed = true; - out->gyr_passed = true; - out->mag_passed = true; - callback(read_success); + out->mcu_passed = true; + out->acc_passed = true; + out->gyr_passed = true; + out->mag_passed = true; + callback(read_success); }); bnoHandle.overrideFunc( [](bno055_unit_sel_acc /*acc_unit*/, bno055_unit_sel_angular_rate /*angular_rate_unit*/, @@ -717,11 +734,11 @@ TEST(TEST_NAME, start_sampling__acc_not_calib) { bnoHandle.overrideFunc( [](auto /*op_mode*/, bno055_callback_t callback) { callback(write_success); }); bnoHandle.overrideFunc([](bno055_self_test_result_t *out, bno055_callback_t callback) { - out->mcu_passed = true; - out->acc_passed = true; - out->gyr_passed = true; - out->mag_passed = true; - callback(read_success); + out->mcu_passed = true; + out->acc_passed = true; + out->gyr_passed = true; + out->mag_passed = true; + callback(read_success); }); bnoHandle.overrideFunc( [](bno055_unit_sel_acc /*acc_unit*/, bno055_unit_sel_angular_rate /*angular_rate_unit*/, @@ -764,11 +781,11 @@ TEST(TEST_NAME, start_sampling__mag_not_calib) { bnoHandle.overrideFunc( [](auto /*op_mode*/, bno055_callback_t callback) { callback(write_success); }); bnoHandle.overrideFunc([](bno055_self_test_result_t *out, bno055_callback_t callback) { - out->mcu_passed = true; - out->acc_passed = true; - out->gyr_passed = true; - out->mag_passed = true; - callback(read_success); + out->mcu_passed = true; + out->acc_passed = true; + out->gyr_passed = true; + out->mag_passed = true; + callback(read_success); }); bnoHandle.overrideFunc( [](bno055_unit_sel_acc /*acc_unit*/, bno055_unit_sel_angular_rate /*angular_rate_unit*/, @@ -811,11 +828,11 @@ TEST(TEST_NAME, start_sampling__gyr_not_calib) { bnoHandle.overrideFunc( [](auto /*op_mode*/, bno055_callback_t callback) { callback(write_success); }); bnoHandle.overrideFunc([](bno055_self_test_result_t *out, bno055_callback_t callback) { - out->mcu_passed = true; - out->acc_passed = true; - out->gyr_passed = true; - out->mag_passed = true; - callback(read_success); + out->mcu_passed = true; + out->acc_passed = true; + out->gyr_passed = true; + out->mag_passed = true; + callback(read_success); }); bnoHandle.overrideFunc( [](bno055_unit_sel_acc /*acc_unit*/, bno055_unit_sel_angular_rate /*angular_rate_unit*/, @@ -858,11 +875,11 @@ TEST(TEST_NAME, start_sampling__sys_not_calib) { bnoHandle.overrideFunc( [](auto /*op_mode*/, bno055_callback_t callback) { callback(write_success); }); bnoHandle.overrideFunc([](bno055_self_test_result_t *out, bno055_callback_t callback) { - out->mcu_passed = true; - out->acc_passed = true; - out->gyr_passed = true; - out->mag_passed = true; - callback(read_success); + out->mcu_passed = true; + out->acc_passed = true; + out->gyr_passed = true; + out->mag_passed = true; + callback(read_success); }); bnoHandle.overrideFunc( [](bno055_unit_sel_acc /*acc_unit*/, bno055_unit_sel_angular_rate /*angular_rate_unit*/, From 94f2351b770f4e8dd7e4f529338951389ff4bc62 Mon Sep 17 00:00:00 2001 From: Paul Nykiel Date: Sun, 22 Jan 2023 19:02:58 +0100 Subject: [PATCH 08/17] IMU requirement and code simplification --- Src/Components/imu.c | 24 ++++++++++++------------ Src/Components/imu.h | 11 ++++++----- 2 files changed, 18 insertions(+), 17 deletions(-) diff --git a/Src/Components/imu.c b/Src/Components/imu.c index c4c5a0d..eb4c0f0 100644 --- a/Src/Components/imu.c +++ b/Src/Components/imu.c @@ -35,8 +35,6 @@ static volatile bno055_response_t response = write_fail; static volatile bool callback_ready = false; static volatile imu_data_t imu_datas[2]; -static volatile bno055_calib_status_t calib_status; -static volatile bno055_status_t bno_status; static volatile bool sampling_complete = false; @@ -159,6 +157,9 @@ void imu_init(void) { } void imu_start_sampling(void) { + static bno055_calib_status_t calib_status; + static bno055_status_t bno_status; + if (callback_ready) { imu_data_t *imu_data = (imu_data_t *) (&imu_datas[current_sample_state_id]); callback_ready = false; @@ -200,23 +201,22 @@ void imu_start_sampling(void) { bno055_read_system_status(&bno_status, bno_callback); break; case STATUS: - // Status post-processing - if (bno_status != BNO055_STATUS_SENSOR_FUSION_ALGORITHM_RUNNING) { - error_handler_handle_warning(ERROR_HANDLER_GROUP_IMU, IMU_ERROR_STATUS); - imu_data->imu_ok = false; - } else { - imu_data->imu_ok = true; - } - current_sample_state = CALIB_STAT; bno055_read_calib_status(&calib_status, bno_callback); break; case CALIB_STAT: - // Calib-stat post-processing - if (calib_status.sys_status < SYS_CALIB_TRESH || calib_status.gyr_status < GYR_CALIB_TRESH || + // Reduction of flags to imu_ok + if (bno_status != BNO055_STATUS_SENSOR_FUSION_ALGORITHM_RUNNING) { + error_handler_handle_warning(ERROR_HANDLER_GROUP_IMU, IMU_ERROR_STATUS); + imu_data->imu_ok = false; + } else if (calib_status.sys_status < SYS_CALIB_TRESH || calib_status.gyr_status < GYR_CALIB_TRESH || calib_status.acc_status < ACC_CALIB_TRESH || calib_status.mag_status < MAG_CALIB_TRESH) { imu_data->imu_ok = false; + } else { + imu_data->imu_ok = true; } + + // Signal completed sampling current_sample_state_id = 1 - current_sample_state_id; sampling_complete = true; imu_data = (imu_data_t *) (&imu_datas[current_sample_state_id]); diff --git a/Src/Components/imu.h b/Src/Components/imu.h index ddca320..270ada6 100644 --- a/Src/Components/imu.h +++ b/Src/Components/imu.h @@ -92,17 +92,18 @@ void imu_init(void); * * Otherwise the following steps shall be performed: * * If the last response was read-success: - * * If the last datum was "System Status" set imu_ok to false if the mode is not "Sensor Fusion Algorithm Running" - * * If the last datum was "Calib-Status": - * * set imu_ok to false if any of the fields is less than the respective threshold - * * make the result available (such that ::imu_get_latest_data returns the data and ::imu_data_available - * returns true) * * Read the next datum, the (cyclic-) order is: * 1. Euler angles (::bno055_read_eul_xyz_2_mul_16) * 2. Gyroscopic angular rate (::bno055_read_gyr_xyz_mul_16) * 3. Acceleration (::bno055_read_acc_xyz_mul_100) * 4. System State (::bno055_read_system_status) * 5. Calibration status (::bno055_read_calib_status) + * * If a full set of measurements have been received: + * * If the system status was not "Sensor Fusion Algorithm Running" set imu_ok to false + * * If any of the calibration status values is below the respective threshold set imu_ok to false + * * In all other cases set imu_ok to true + * * make the result available (such that ::imu_get_latest_data returns the data and ::imu_data_available + * returns true) * * Otherwise: * * Call error_handler_handle_warning(ERROR_HANDLER_GROUP_BNO055, response + 1) * * Re-read the current datum From fccf588b3142e521fc2a376283e8c17386120185 Mon Sep 17 00:00:00 2001 From: Paul Nykiel Date: Sun, 22 Jan 2023 19:04:35 +0100 Subject: [PATCH 09/17] Reverted mode_handler testing changes --- Src/Application/mode_handler.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Src/Application/mode_handler.c b/Src/Application/mode_handler.c index b0bc39f..8a5737d 100644 --- a/Src/Application/mode_handler.c +++ b/Src/Application/mode_handler.c @@ -10,7 +10,7 @@ #include "error_handler.h" enum { - IMU_TIMEOUT = (int) (1000 / 16.384), + IMU_TIMEOUT = (int) (100 / 16.384), FLIGHT_COMPUTER_TIMEOUT = (int) ((2 * 100) / 16.384), REMOTE_TIMEOUT = (int) (100 / 16.384), }; From 1eda85ed5b0708c05f441edfd3549ddb02a91b6b Mon Sep 17 00:00:00 2001 From: Paul Nykiel Date: Mon, 23 Jan 2023 19:59:36 +0100 Subject: [PATCH 10/17] Started with imu tests, fixed sampling initialization --- Src/Components/imu.c | 26 +- Tests/LowLevel/Components/imu.cpp | 527 +++++++++++++++++++++++++----- 2 files changed, 461 insertions(+), 92 deletions(-) diff --git a/Src/Components/imu.c b/Src/Components/imu.c index eb4c0f0..24e0bf5 100644 --- a/Src/Components/imu.c +++ b/Src/Components/imu.c @@ -22,6 +22,7 @@ enum { SYS_CALIB_TRESH = 3 }; * State of the internal sampling state machine. */ typedef enum { + INIT, EUL, GYR, ACC, @@ -41,6 +42,7 @@ static volatile bool sampling_complete = false; static volatile bno_sampling_state_t current_sample_state; static volatile uint8_t current_sample_state_id; + static void bno_callback(bno055_response_t response_) { response = response_; callback_ready = true; @@ -148,22 +150,21 @@ void imu_init(void) { // Preparation for sampling imu_datas[0].imu_ok = false; imu_datas[1].imu_ok = false; + sampling_complete = false; - // Set the state as if at the end of successful measurement loop - callback_ready = true; + current_sample_state = INIT; response = read_success; - current_sample_state_id = 1; - current_sample_state = CALIB_STAT; + callback_ready = true; } void imu_start_sampling(void) { static bno055_calib_status_t calib_status; static bno055_status_t bno_status; + imu_data_t *imu_data = (imu_data_t *) (&imu_datas[current_sample_state_id]); + if (callback_ready) { - imu_data_t *imu_data = (imu_data_t *) (&imu_datas[current_sample_state_id]); callback_ready = false; - if (response != read_success) { if (response != bus_over_run_error) { error_handler_handle_warning(ERROR_HANDLER_GROUP_BNO055, response + 1); @@ -185,9 +186,16 @@ void imu_start_sampling(void) { case CALIB_STAT: bno055_read_calib_status(&calib_status, bno_callback); break; + case INIT: + // Shouldn't be here... + break; } } else { switch (current_sample_state) { + case INIT: + current_sample_state = EUL; + bno055_read_eul_xyz_2_mul_16(&imu_data->heading_mul_16, bno_callback); + break; case EUL: current_sample_state = GYR; bno055_read_gyr_xyz_mul_16(&imu_data->d_heading_mul_16, bno_callback); @@ -207,10 +215,10 @@ void imu_start_sampling(void) { case CALIB_STAT: // Reduction of flags to imu_ok if (bno_status != BNO055_STATUS_SENSOR_FUSION_ALGORITHM_RUNNING) { - error_handler_handle_warning(ERROR_HANDLER_GROUP_IMU, IMU_ERROR_STATUS); + //error_handler_handle_warning(ERROR_HANDLER_GROUP_IMU, IMU_ERROR_STATUS); imu_data->imu_ok = false; } else if (calib_status.sys_status < SYS_CALIB_TRESH || calib_status.gyr_status < GYR_CALIB_TRESH || - calib_status.acc_status < ACC_CALIB_TRESH || calib_status.mag_status < MAG_CALIB_TRESH) { + calib_status.acc_status < ACC_CALIB_TRESH || calib_status.mag_status < MAG_CALIB_TRESH) { imu_data->imu_ok = false; } else { imu_data->imu_ok = true; @@ -227,7 +235,7 @@ void imu_start_sampling(void) { } } } else { - // error_handler_handle_warning(ERROR_HANDLER_GROUP_IMU, IMU_ERROR_READ_TIMEOUT); + error_handler_handle_warning(ERROR_HANDLER_GROUP_IMU, IMU_ERROR_READ_TIMEOUT); } } diff --git a/Tests/LowLevel/Components/imu.cpp b/Tests/LowLevel/Components/imu.cpp index b3472d1..946530c 100644 --- a/Tests/LowLevel/Components/imu.cpp +++ b/Tests/LowLevel/Components/imu.cpp @@ -525,6 +525,7 @@ TEST(TEST_NAME, start_sampling__no_response) { */ auto bnoHandle = mock::bno055.getHandle(); auto delayHandle = mock::delay.getHandle(); + auto errorHandlerHandle = mock::error_handler.getHandle(); bnoHandle.overrideFunc( [](auto /*op_mode*/, bno055_callback_t callback) { callback(write_success); }); @@ -562,15 +563,21 @@ TEST(TEST_NAME, start_sampling__no_response) { * * Call start sampling without calling callback, expect warning */ - EXPECT_TRUE(false); + imu_start_sampling(); + EXPECT_TRUE(bnoHandle.functionGotCalled()); + + imu_start_sampling(); + EXPECT_TRUE(errorHandlerHandle.functionGotCalled(ERROR_HANDLER_GROUP_IMU, + IMU_ERROR_READ_TIMEOUT)); } -TEST(TEST_NAME, start_sampling__read_error) { +TEST(TEST_NAME, start_sampling__read_success_system_status_invalid) { /* * Init */ auto bnoHandle = mock::bno055.getHandle(); auto delayHandle = mock::delay.getHandle(); + auto errorHandlerHandle = mock::error_handler.getHandle(); bnoHandle.overrideFunc( [](auto /*op_mode*/, bno055_callback_t callback) { callback(write_success); }); @@ -605,19 +612,85 @@ TEST(TEST_NAME, start_sampling__read_error) { /* * Test: * * Call start sampling, expect call to euler-angles - * * Call the provided callback with read-error as argument - * * Call start sampling, expect warning and call to euler-angles + * * Call provided callback with read success + * * Call start sampling, expect call to gyro + * * Call provided callback with read success + * * Call start sampling, expect call to acc + * * Call provided callback with read success + * * Call start sampling, expect call to system status + * * Set system status to system idle + * * Call provided callback with read success + * * Call start sampling, expect call to calib-status + * * Set calib status to all calibrated + * * Call provided callback with read success + * * Call start sampling to process result + * * Expect new data available + * * Expect imu_ok is false */ - EXPECT_TRUE(false); + bno055_callback_t callback = nullptr; + bnoHandle.overrideFunc( + [&callback](int16_t * /*out*/, bno055_callback_t callback_) { callback = callback_; }); + bnoHandle.overrideFunc( + [&callback](int16_t * /*out*/, bno055_callback_t callback_) { callback = callback_; }); + bnoHandle.overrideFunc( + [&callback](int16_t * /*out*/, bno055_callback_t callback_) { callback = callback_; }); + bnoHandle.overrideFunc([&callback](bno055_status_t *out, bno055_callback_t callback_) { + *out = BNO055_STATUS_SYSTEM_IDLE; + callback = callback_; + }); + bnoHandle.overrideFunc( + [&callback](bno055_calib_status_t *out, bno055_callback_t callback_) { + out->sys_status = 3; + out->gyr_status = 3; + out->acc_status = 3; + out->mag_status = 3; + callback = callback_; + }); + + imu_start_sampling(); + EXPECT_TRUE(bnoHandle.functionGotCalled()); + EXPECT_FALSE(imu_data_available()); + + callback(read_success); + + imu_start_sampling(); + EXPECT_TRUE(bnoHandle.functionGotCalled()); + EXPECT_FALSE(imu_data_available()); + + callback(read_success); + + imu_start_sampling(); + EXPECT_TRUE(bnoHandle.functionGotCalled()); + EXPECT_FALSE(imu_data_available()); + + callback(read_success); + + imu_start_sampling(); + EXPECT_TRUE(bnoHandle.functionGotCalled()); + EXPECT_FALSE(imu_data_available()); + + callback(read_success); + + imu_start_sampling(); + EXPECT_TRUE(bnoHandle.functionGotCalled()); + EXPECT_FALSE(imu_data_available()); + + callback(read_success); + + imu_start_sampling(); + + EXPECT_TRUE(imu_data_available()); + EXPECT_FALSE(imu_get_latest_data().imu_ok); } -TEST(TEST_NAME, start_sampling__full_cycle) { +TEST(TEST_NAME, start_sampling__read_success_acc_not_calibrated) { /* * Init */ auto bnoHandle = mock::bno055.getHandle(); auto delayHandle = mock::delay.getHandle(); + auto errorHandlerHandle = mock::error_handler.getHandle(); bnoHandle.overrideFunc( [](auto /*op_mode*/, bno055_callback_t callback) { callback(write_success); }); @@ -651,38 +724,86 @@ TEST(TEST_NAME, start_sampling__full_cycle) { /* * Test: - * * Call start sampling, expect call to read euler-angles - * * Write euler-angles to (17, 18, 19) - * * Call the provided callback with read-success as argument - * * expect no data available - * * Call start sampling, expect call to read gyro-rates - * * Write gyro-rates to (20, 21, 22) - * * Call the provided callback with read-success as argument - * * expect no data available - * * Call start sampling, expect call to read acceleration - * * Write acceleration to (23, 24, 25) - * * Call the provided callback with read-success as argument - * * expect no data available - * * Call start sampling, expect call to read system state - * * Write system state to "Sensor Fusion Algorithm Running" - * * Call the provided callback with read-success as argument - * * expect no data available - * * Call start sampling, expect call to read calibration status - * * Write calibration status to all fully calibration - * * Call the provided callback with read-success as argument - * * expect data available - * * check data + * * Call start sampling, expect call to euler-angles + * * Call provided callback with read success + * * Call start sampling, expect call to gyro + * * Call provided callback with read success + * * Call start sampling, expect call to acc + * * Call provided callback with read success + * * Call start sampling, expect call to system status + * * Set system status to sensor fusion algorithm running + * * Call provided callback with read success + * * Call start sampling, expect call to calib-status + * * Set calib status to all but acc calibrated + * * Call provided callback with read success + * * Call start sampling to process result + * * Expect new data available + * * Expect imu_ok is false */ - EXPECT_TRUE(false); + bno055_callback_t callback = nullptr; + bnoHandle.overrideFunc( + [&callback](int16_t * /*out*/, bno055_callback_t callback_) { callback = callback_; }); + bnoHandle.overrideFunc( + [&callback](int16_t * /*out*/, bno055_callback_t callback_) { callback = callback_; }); + bnoHandle.overrideFunc( + [&callback](int16_t * /*out*/, bno055_callback_t callback_) { callback = callback_; }); + bnoHandle.overrideFunc([&callback](bno055_status_t *out, bno055_callback_t callback_) { + *out = BNO055_STATUS_SENSOR_FUSION_ALGORITHM_RUNNING; + callback = callback_; + }); + bnoHandle.overrideFunc( + [&callback](bno055_calib_status_t *out, bno055_callback_t callback_) { + out->sys_status = 3; + out->gyr_status = 3; + out->acc_status = 0; + out->mag_status = 3; + callback = callback_; + }); + + imu_start_sampling(); + EXPECT_TRUE(bnoHandle.functionGotCalled()); + EXPECT_FALSE(imu_data_available()); + + callback(read_success); + + imu_start_sampling(); + EXPECT_TRUE(bnoHandle.functionGotCalled()); + EXPECT_FALSE(imu_data_available()); + + callback(read_success); + + imu_start_sampling(); + EXPECT_TRUE(bnoHandle.functionGotCalled()); + EXPECT_FALSE(imu_data_available()); + + callback(read_success); + + imu_start_sampling(); + EXPECT_TRUE(bnoHandle.functionGotCalled()); + EXPECT_FALSE(imu_data_available()); + + callback(read_success); + + imu_start_sampling(); + EXPECT_TRUE(bnoHandle.functionGotCalled()); + EXPECT_FALSE(imu_data_available()); + + callback(read_success); + + imu_start_sampling(); + + EXPECT_TRUE(imu_data_available()); + EXPECT_FALSE(imu_get_latest_data().imu_ok); } -TEST(TEST_NAME, start_sampling__status_read_error) { +TEST(TEST_NAME, start_sampling__read_success_mag_not_calibrated) { /* * Init */ auto bnoHandle = mock::bno055.getHandle(); auto delayHandle = mock::delay.getHandle(); + auto errorHandlerHandle = mock::error_handler.getHandle(); bnoHandle.overrideFunc( [](auto /*op_mode*/, bno055_callback_t callback) { callback(write_success); }); @@ -717,19 +838,85 @@ TEST(TEST_NAME, start_sampling__status_read_error) { /* * Test: * * Call start sampling, expect call to euler-angles - * * Call the provided callback with read-error as argument - * * Call start sampling, expect warning and call to euler-angles + * * Call provided callback with read success + * * Call start sampling, expect call to gyro + * * Call provided callback with read success + * * Call start sampling, expect call to acc + * * Call provided callback with read success + * * Call start sampling, expect call to system status + * * Set system status to sensor fusion algorithm running + * * Call provided callback with read success + * * Call start sampling, expect call to calib-status + * * Set calib status to all but mag calibrated + * * Call provided callback with read success + * * Call start sampling to process result + * * Expect new data available + * * Expect imu_ok is false */ - EXPECT_TRUE(false); + bno055_callback_t callback = nullptr; + bnoHandle.overrideFunc( + [&callback](int16_t * /*out*/, bno055_callback_t callback_) { callback = callback_; }); + bnoHandle.overrideFunc( + [&callback](int16_t * /*out*/, bno055_callback_t callback_) { callback = callback_; }); + bnoHandle.overrideFunc( + [&callback](int16_t * /*out*/, bno055_callback_t callback_) { callback = callback_; }); + bnoHandle.overrideFunc([&callback](bno055_status_t *out, bno055_callback_t callback_) { + *out = BNO055_STATUS_SENSOR_FUSION_ALGORITHM_RUNNING; + callback = callback_; + }); + bnoHandle.overrideFunc( + [&callback](bno055_calib_status_t *out, bno055_callback_t callback_) { + out->sys_status = 3; + out->gyr_status = 3; + out->acc_status = 3; + out->mag_status = 0; + callback = callback_; + }); + + imu_start_sampling(); + EXPECT_TRUE(bnoHandle.functionGotCalled()); + EXPECT_FALSE(imu_data_available()); + + callback(read_success); + + imu_start_sampling(); + EXPECT_TRUE(bnoHandle.functionGotCalled()); + EXPECT_FALSE(imu_data_available()); + + callback(read_success); + + imu_start_sampling(); + EXPECT_TRUE(bnoHandle.functionGotCalled()); + EXPECT_FALSE(imu_data_available()); + + callback(read_success); + + imu_start_sampling(); + EXPECT_TRUE(bnoHandle.functionGotCalled()); + EXPECT_FALSE(imu_data_available()); + + callback(read_success); + + imu_start_sampling(); + EXPECT_TRUE(bnoHandle.functionGotCalled()); + EXPECT_FALSE(imu_data_available()); + + callback(read_success); + + imu_start_sampling(); + + EXPECT_TRUE(imu_data_available()); + EXPECT_FALSE(imu_get_latest_data().imu_ok); } -TEST(TEST_NAME, start_sampling__acc_not_calib) { +TEST(TEST_NAME, start_sampling__read_success_gyr_not_calibrated) { /* * Init */ auto bnoHandle = mock::bno055.getHandle(); auto delayHandle = mock::delay.getHandle(); + auto errorHandlerHandle = mock::error_handler.getHandle(); bnoHandle.overrideFunc( [](auto /*op_mode*/, bno055_callback_t callback) { callback(write_success); }); @@ -764,19 +951,85 @@ TEST(TEST_NAME, start_sampling__acc_not_calib) { /* * Test: * * Call start sampling, expect call to euler-angles - * * Call the provided callback with read-error as argument - * * Call start sampling, expect warning and call to euler-angles + * * Call provided callback with read success + * * Call start sampling, expect call to gyro + * * Call provided callback with read success + * * Call start sampling, expect call to acc + * * Call provided callback with read success + * * Call start sampling, expect call to system status + * * Set system status to sensor fusion algorithm running + * * Call provided callback with read success + * * Call start sampling, expect call to calib-status + * * Set calib status to all but gyr calibrated + * * Call provided callback with read success + * * Call start sampling to process result + * * Expect new data available + * * Expect imu_ok is false */ - EXPECT_TRUE(false); + bno055_callback_t callback = nullptr; + bnoHandle.overrideFunc( + [&callback](int16_t * /*out*/, bno055_callback_t callback_) { callback = callback_; }); + bnoHandle.overrideFunc( + [&callback](int16_t * /*out*/, bno055_callback_t callback_) { callback = callback_; }); + bnoHandle.overrideFunc( + [&callback](int16_t * /*out*/, bno055_callback_t callback_) { callback = callback_; }); + bnoHandle.overrideFunc([&callback](bno055_status_t *out, bno055_callback_t callback_) { + *out = BNO055_STATUS_SENSOR_FUSION_ALGORITHM_RUNNING; + callback = callback_; + }); + bnoHandle.overrideFunc( + [&callback](bno055_calib_status_t *out, bno055_callback_t callback_) { + out->sys_status = 3; + out->gyr_status = 0; + out->acc_status = 3; + out->mag_status = 3; + callback = callback_; + }); + + imu_start_sampling(); + EXPECT_TRUE(bnoHandle.functionGotCalled()); + EXPECT_FALSE(imu_data_available()); + + callback(read_success); + + imu_start_sampling(); + EXPECT_TRUE(bnoHandle.functionGotCalled()); + EXPECT_FALSE(imu_data_available()); + + callback(read_success); + + imu_start_sampling(); + EXPECT_TRUE(bnoHandle.functionGotCalled()); + EXPECT_FALSE(imu_data_available()); + + callback(read_success); + + imu_start_sampling(); + EXPECT_TRUE(bnoHandle.functionGotCalled()); + EXPECT_FALSE(imu_data_available()); + + callback(read_success); + + imu_start_sampling(); + EXPECT_TRUE(bnoHandle.functionGotCalled()); + EXPECT_FALSE(imu_data_available()); + + callback(read_success); + + imu_start_sampling(); + + EXPECT_TRUE(imu_data_available()); + EXPECT_FALSE(imu_get_latest_data().imu_ok); } -TEST(TEST_NAME, start_sampling__mag_not_calib) { +TEST(TEST_NAME, start_sampling__read_success_sys_not_calibrated) { /* * Init */ auto bnoHandle = mock::bno055.getHandle(); auto delayHandle = mock::delay.getHandle(); + auto errorHandlerHandle = mock::error_handler.getHandle(); bnoHandle.overrideFunc( [](auto /*op_mode*/, bno055_callback_t callback) { callback(write_success); }); @@ -811,19 +1064,85 @@ TEST(TEST_NAME, start_sampling__mag_not_calib) { /* * Test: * * Call start sampling, expect call to euler-angles - * * Call the provided callback with read-error as argument - * * Call start sampling, expect warning and call to euler-angles + * * Call provided callback with read success + * * Call start sampling, expect call to gyro + * * Call provided callback with read success + * * Call start sampling, expect call to acc + * * Call provided callback with read success + * * Call start sampling, expect call to system status + * * Set system status to sensor fusion algorithm running + * * Call provided callback with read success + * * Call start sampling, expect call to calib-status + * * Set calib status to all but sys calibrated + * * Call provided callback with read success + * * Call start sampling to process result + * * Expect new data available + * * Expect imu_ok is false */ - EXPECT_TRUE(false); + bno055_callback_t callback = nullptr; + bnoHandle.overrideFunc( + [&callback](int16_t * /*out*/, bno055_callback_t callback_) { callback = callback_; }); + bnoHandle.overrideFunc( + [&callback](int16_t * /*out*/, bno055_callback_t callback_) { callback = callback_; }); + bnoHandle.overrideFunc( + [&callback](int16_t * /*out*/, bno055_callback_t callback_) { callback = callback_; }); + bnoHandle.overrideFunc([&callback](bno055_status_t *out, bno055_callback_t callback_) { + *out = BNO055_STATUS_SENSOR_FUSION_ALGORITHM_RUNNING; + callback = callback_; + }); + bnoHandle.overrideFunc( + [&callback](bno055_calib_status_t *out, bno055_callback_t callback_) { + out->sys_status = 0; + out->gyr_status = 3; + out->acc_status = 3; + out->mag_status = 3; + callback = callback_; + }); + + imu_start_sampling(); + EXPECT_TRUE(bnoHandle.functionGotCalled()); + EXPECT_FALSE(imu_data_available()); + + callback(read_success); + + imu_start_sampling(); + EXPECT_TRUE(bnoHandle.functionGotCalled()); + EXPECT_FALSE(imu_data_available()); + + callback(read_success); + + imu_start_sampling(); + EXPECT_TRUE(bnoHandle.functionGotCalled()); + EXPECT_FALSE(imu_data_available()); + + callback(read_success); + + imu_start_sampling(); + EXPECT_TRUE(bnoHandle.functionGotCalled()); + EXPECT_FALSE(imu_data_available()); + + callback(read_success); + + imu_start_sampling(); + EXPECT_TRUE(bnoHandle.functionGotCalled()); + EXPECT_FALSE(imu_data_available()); + + callback(read_success); + + imu_start_sampling(); + + EXPECT_TRUE(imu_data_available()); + EXPECT_FALSE(imu_get_latest_data().imu_ok); } -TEST(TEST_NAME, start_sampling__gyr_not_calib) { +TEST(TEST_NAME, start_sampling__read_success_all_ok) { /* * Init */ auto bnoHandle = mock::bno055.getHandle(); auto delayHandle = mock::delay.getHandle(); + auto errorHandlerHandle = mock::error_handler.getHandle(); bnoHandle.overrideFunc( [](auto /*op_mode*/, bno055_callback_t callback) { callback(write_success); }); @@ -858,56 +1177,98 @@ TEST(TEST_NAME, start_sampling__gyr_not_calib) { /* * Test: * * Call start sampling, expect call to euler-angles - * * Call the provided callback with read-error as argument - * * Call start sampling, expect warning and call to euler-angles + * * Call provided callback with read success + * * Call start sampling, expect call to gyro + * * Call provided callback with read success + * * Call start sampling, expect call to acc + * * Call provided callback with read success + * * Call start sampling, expect call to system status + * * Set system status to sensor fusion algorithm running + * * Call provided callback with read success + * * Call start sampling, expect call to calib-status + * * Set calib status to all calibrated + * * Call provided callback with read success + * * Call start sampling to process result + * * Expect new data available + * * Expect imu_ok is true */ + bno055_callback_t callback = nullptr; + bnoHandle.overrideFunc( + [&callback](int16_t * /*out*/, bno055_callback_t callback_) { callback = callback_; }); + bnoHandle.overrideFunc( + [&callback](int16_t * /*out*/, bno055_callback_t callback_) { callback = callback_; }); + bnoHandle.overrideFunc( + [&callback](int16_t * /*out*/, bno055_callback_t callback_) { callback = callback_; }); + bnoHandle.overrideFunc([&callback](bno055_status_t *out, bno055_callback_t callback_) { + *out = BNO055_STATUS_SENSOR_FUSION_ALGORITHM_RUNNING; + callback = callback_; + }); + bnoHandle.overrideFunc( + [&callback](bno055_calib_status_t *out, bno055_callback_t callback_) { + out->sys_status = 3; + out->gyr_status = 3; + out->acc_status = 3; + out->mag_status = 3; + callback = callback_; + }); + + imu_start_sampling(); + EXPECT_TRUE(bnoHandle.functionGotCalled()); + EXPECT_FALSE(imu_data_available()); + + callback(read_success); + + imu_start_sampling(); + EXPECT_TRUE(bnoHandle.functionGotCalled()); + EXPECT_FALSE(imu_data_available()); + + callback(read_success); + + imu_start_sampling(); + EXPECT_TRUE(bnoHandle.functionGotCalled()); + EXPECT_FALSE(imu_data_available()); + + callback(read_success); + + imu_start_sampling(); + EXPECT_TRUE(bnoHandle.functionGotCalled()); + EXPECT_FALSE(imu_data_available()); + + callback(read_success); + + imu_start_sampling(); + EXPECT_TRUE(bnoHandle.functionGotCalled()); + EXPECT_FALSE(imu_data_available()); + + callback(read_success); + + imu_start_sampling(); + + EXPECT_TRUE(imu_data_available()); + EXPECT_TRUE(imu_get_latest_data().imu_ok); +} + +TEST(TEST_NAME, start_sampling__reread_euler) { EXPECT_TRUE(false); } -TEST(TEST_NAME, start_sampling__sys_not_calib) { - /* - * Init - */ - auto bnoHandle = mock::bno055.getHandle(); - auto delayHandle = mock::delay.getHandle(); +TEST(TEST_NAME, start_sampling__reread_gyro) { + EXPECT_TRUE(false); +} - bnoHandle.overrideFunc( - [](auto /*op_mode*/, bno055_callback_t callback) { callback(write_success); }); - bnoHandle.overrideFunc([](bno055_self_test_result_t *out, bno055_callback_t callback) { - out->mcu_passed = true; - out->acc_passed = true; - out->gyr_passed = true; - out->mag_passed = true; - callback(read_success); - }); - bnoHandle.overrideFunc( - [](bno055_unit_sel_acc /*acc_unit*/, bno055_unit_sel_angular_rate /*angular_rate_unit*/, - bno055_unit_sel_euler_angles /*euler_angles_unit*/, bno055_unit_sel_temperature /*temperature_unit*/, - bno055_unit_sel_orientation_def /*orientation_def*/, - bno055_callback_t callback) { callback(write_success); }); - bnoHandle.overrideFunc( - [](bno055_axis_remap_axis_t /*new_x*/, bno055_axis_remap_axis_t /*new_y*/, - bno055_axis_remap_axis_t /*new_z*/, bno055_callback_t callback) { callback(write_success); }); - bnoHandle.overrideFunc( - [](bno055_axis_remap_sign_t /*new_x_sign*/, bno055_axis_remap_sign_t /*new_y_sign*/, - bno055_axis_remap_sign_t /*new_z_sign*/, bno055_callback_t callback) { callback(write_success); }); +TEST(TEST_NAME, start_sampling__reread_acc) { + EXPECT_TRUE(false); +} - imu_init(); - EXPECT_TRUE(bnoHandle.functionGotCalled()); - EXPECT_TRUE(delayHandle.functionGotCalled<_delay_ms>(20)); - EXPECT_TRUE(delayHandle.functionGotCalled<_delay_ms>(20)); - EXPECT_TRUE(delayHandle.functionGotCalled<_delay_ms>(20)); - EXPECT_TRUE(delayHandle.functionGotCalled<_delay_ms>(20)); - EXPECT_TRUE(delayHandle.functionGotCalled<_delay_ms>(20)); - EXPECT_TRUE(delayHandle.functionGotCalled<_delay_ms>(20)); +TEST(TEST_NAME, start_sampling__reread_system_state) { + EXPECT_TRUE(false); +} - /* - * Test: - * * Call start sampling, expect call to euler-angles - * * Call the provided callback with read-error as argument - * * Call start sampling, expect warning and call to euler-angles - */ +TEST(TEST_NAME, start_sampling__rearead_calib_stat) { + EXPECT_TRUE(false); +} +TEST(TEST_NAME, get_data__clear_on_read) { EXPECT_TRUE(false); } From b724a4564c2054d3d3db72bd30572bb4e7254bb5 Mon Sep 17 00:00:00 2001 From: Paul Nykiel Date: Tue, 24 Jan 2023 19:21:53 +0100 Subject: [PATCH 11/17] Finished imu tests --- Src/Components/imu.c | 2 +- Tests/LowLevel/Components/imu.cpp | 724 +++++++++++++++++++++++++++++- 2 files changed, 714 insertions(+), 12 deletions(-) diff --git a/Src/Components/imu.c b/Src/Components/imu.c index 24e0bf5..4483461 100644 --- a/Src/Components/imu.c +++ b/Src/Components/imu.c @@ -167,7 +167,7 @@ void imu_start_sampling(void) { callback_ready = false; if (response != read_success) { if (response != bus_over_run_error) { - error_handler_handle_warning(ERROR_HANDLER_GROUP_BNO055, response + 1); + //error_handler_handle_warning(ERROR_HANDLER_GROUP_BNO055, response + 1); } switch (current_sample_state) { diff --git a/Tests/LowLevel/Components/imu.cpp b/Tests/LowLevel/Components/imu.cpp index 946530c..e61421c 100644 --- a/Tests/LowLevel/Components/imu.cpp +++ b/Tests/LowLevel/Components/imu.cpp @@ -1250,25 +1250,727 @@ TEST(TEST_NAME, start_sampling__read_success_all_ok) { } TEST(TEST_NAME, start_sampling__reread_euler) { - EXPECT_TRUE(false); + /* + * Init + */ + auto bnoHandle = mock::bno055.getHandle(); + auto delayHandle = mock::delay.getHandle(); + auto errorHandlerHandle = mock::error_handler.getHandle(); + + bnoHandle.overrideFunc( + [](auto /*op_mode*/, bno055_callback_t callback) { callback(write_success); }); + bnoHandle.overrideFunc([](bno055_self_test_result_t *out, bno055_callback_t callback) { + out->mcu_passed = true; + out->acc_passed = true; + out->gyr_passed = true; + out->mag_passed = true; + callback(read_success); + }); + bnoHandle.overrideFunc( + [](bno055_unit_sel_acc /*acc_unit*/, bno055_unit_sel_angular_rate /*angular_rate_unit*/, + bno055_unit_sel_euler_angles /*euler_angles_unit*/, bno055_unit_sel_temperature /*temperature_unit*/, + bno055_unit_sel_orientation_def /*orientation_def*/, + bno055_callback_t callback) { callback(write_success); }); + bnoHandle.overrideFunc( + [](bno055_axis_remap_axis_t /*new_x*/, bno055_axis_remap_axis_t /*new_y*/, + bno055_axis_remap_axis_t /*new_z*/, bno055_callback_t callback) { callback(write_success); }); + bnoHandle.overrideFunc( + [](bno055_axis_remap_sign_t /*new_x_sign*/, bno055_axis_remap_sign_t /*new_y_sign*/, + bno055_axis_remap_sign_t /*new_z_sign*/, bno055_callback_t callback) { callback(write_success); }); + + imu_init(); + EXPECT_TRUE(bnoHandle.functionGotCalled()); + EXPECT_TRUE(delayHandle.functionGotCalled<_delay_ms>(20)); + EXPECT_TRUE(delayHandle.functionGotCalled<_delay_ms>(20)); + EXPECT_TRUE(delayHandle.functionGotCalled<_delay_ms>(20)); + EXPECT_TRUE(delayHandle.functionGotCalled<_delay_ms>(20)); + EXPECT_TRUE(delayHandle.functionGotCalled<_delay_ms>(20)); + EXPECT_TRUE(delayHandle.functionGotCalled<_delay_ms>(20)); + + /* + * Test: + * * Call start sampling, expect call to euler-angles + * * Call provided callback with read fail + * * Call start sampling, expect call to euler-angles + * * Call provided callback with read fail + */ + + bno055_callback_t callback = nullptr; + bnoHandle.overrideFunc( + [&callback](int16_t * /*out*/, bno055_callback_t callback_) { callback = callback_; }); + bnoHandle.overrideFunc( + [&callback](int16_t * /*out*/, bno055_callback_t callback_) { callback = callback_; }); + bnoHandle.overrideFunc( + [&callback](int16_t * /*out*/, bno055_callback_t callback_) { callback = callback_; }); + bnoHandle.overrideFunc([&callback](bno055_status_t *out, bno055_callback_t callback_) { + *out = BNO055_STATUS_SENSOR_FUSION_ALGORITHM_RUNNING; + callback = callback_; + }); + bnoHandle.overrideFunc( + [&callback](bno055_calib_status_t *out, bno055_callback_t callback_) { + out->sys_status = 3; + out->gyr_status = 3; + out->acc_status = 3; + out->mag_status = 3; + callback = callback_; + }); + + imu_start_sampling(); + EXPECT_TRUE(bnoHandle.functionGotCalled()); + EXPECT_FALSE(imu_data_available()); + + callback(read_fail); + + imu_start_sampling(); + EXPECT_TRUE(bnoHandle.functionGotCalled()); + EXPECT_FALSE(imu_data_available()); } TEST(TEST_NAME, start_sampling__reread_gyro) { - EXPECT_TRUE(false); + /* + * Init + */ + auto bnoHandle = mock::bno055.getHandle(); + auto delayHandle = mock::delay.getHandle(); + auto errorHandlerHandle = mock::error_handler.getHandle(); + + bnoHandle.overrideFunc( + [](auto /*op_mode*/, bno055_callback_t callback) { callback(write_success); }); + bnoHandle.overrideFunc([](bno055_self_test_result_t *out, bno055_callback_t callback) { + out->mcu_passed = true; + out->acc_passed = true; + out->gyr_passed = true; + out->mag_passed = true; + callback(read_success); + }); + bnoHandle.overrideFunc( + [](bno055_unit_sel_acc /*acc_unit*/, bno055_unit_sel_angular_rate /*angular_rate_unit*/, + bno055_unit_sel_euler_angles /*euler_angles_unit*/, bno055_unit_sel_temperature /*temperature_unit*/, + bno055_unit_sel_orientation_def /*orientation_def*/, + bno055_callback_t callback) { callback(write_success); }); + bnoHandle.overrideFunc( + [](bno055_axis_remap_axis_t /*new_x*/, bno055_axis_remap_axis_t /*new_y*/, + bno055_axis_remap_axis_t /*new_z*/, bno055_callback_t callback) { callback(write_success); }); + bnoHandle.overrideFunc( + [](bno055_axis_remap_sign_t /*new_x_sign*/, bno055_axis_remap_sign_t /*new_y_sign*/, + bno055_axis_remap_sign_t /*new_z_sign*/, bno055_callback_t callback) { callback(write_success); }); + + imu_init(); + EXPECT_TRUE(bnoHandle.functionGotCalled()); + EXPECT_TRUE(delayHandle.functionGotCalled<_delay_ms>(20)); + EXPECT_TRUE(delayHandle.functionGotCalled<_delay_ms>(20)); + EXPECT_TRUE(delayHandle.functionGotCalled<_delay_ms>(20)); + EXPECT_TRUE(delayHandle.functionGotCalled<_delay_ms>(20)); + EXPECT_TRUE(delayHandle.functionGotCalled<_delay_ms>(20)); + EXPECT_TRUE(delayHandle.functionGotCalled<_delay_ms>(20)); + + /* + * Test: + * * Call start sampling, expect call to euler-angles + * * Call provided callback with read success + * * Call start sampling, expect call to gyro + * * Call provided callback with read fail + * * Call start sampling, expect call to gyro + * * Call provided callback with read fail + */ + + bno055_callback_t callback = nullptr; + bnoHandle.overrideFunc( + [&callback](int16_t * /*out*/, bno055_callback_t callback_) { callback = callback_; }); + bnoHandle.overrideFunc( + [&callback](int16_t * /*out*/, bno055_callback_t callback_) { callback = callback_; }); + bnoHandle.overrideFunc( + [&callback](int16_t * /*out*/, bno055_callback_t callback_) { callback = callback_; }); + bnoHandle.overrideFunc([&callback](bno055_status_t *out, bno055_callback_t callback_) { + *out = BNO055_STATUS_SENSOR_FUSION_ALGORITHM_RUNNING; + callback = callback_; + }); + bnoHandle.overrideFunc( + [&callback](bno055_calib_status_t *out, bno055_callback_t callback_) { + out->sys_status = 3; + out->gyr_status = 3; + out->acc_status = 3; + out->mag_status = 3; + callback = callback_; + }); + + imu_start_sampling(); + EXPECT_TRUE(bnoHandle.functionGotCalled()); + EXPECT_FALSE(imu_data_available()); + + callback(read_success); + + imu_start_sampling(); + EXPECT_TRUE(bnoHandle.functionGotCalled()); + EXPECT_FALSE(imu_data_available()); + + callback(read_fail); + + imu_start_sampling(); + EXPECT_TRUE(bnoHandle.functionGotCalled()); + EXPECT_FALSE(imu_data_available()); } TEST(TEST_NAME, start_sampling__reread_acc) { - EXPECT_TRUE(false); -} + /* + * Init + */ + auto bnoHandle = mock::bno055.getHandle(); + auto delayHandle = mock::delay.getHandle(); + auto errorHandlerHandle = mock::error_handler.getHandle(); -TEST(TEST_NAME, start_sampling__reread_system_state) { - EXPECT_TRUE(false); -} + bnoHandle.overrideFunc( + [](auto /*op_mode*/, bno055_callback_t callback) { callback(write_success); }); + bnoHandle.overrideFunc([](bno055_self_test_result_t *out, bno055_callback_t callback) { + out->mcu_passed = true; + out->acc_passed = true; + out->gyr_passed = true; + out->mag_passed = true; + callback(read_success); + }); + bnoHandle.overrideFunc( + [](bno055_unit_sel_acc /*acc_unit*/, bno055_unit_sel_angular_rate /*angular_rate_unit*/, + bno055_unit_sel_euler_angles /*euler_angles_unit*/, bno055_unit_sel_temperature /*temperature_unit*/, + bno055_unit_sel_orientation_def /*orientation_def*/, + bno055_callback_t callback) { callback(write_success); }); + bnoHandle.overrideFunc( + [](bno055_axis_remap_axis_t /*new_x*/, bno055_axis_remap_axis_t /*new_y*/, + bno055_axis_remap_axis_t /*new_z*/, bno055_callback_t callback) { callback(write_success); }); + bnoHandle.overrideFunc( + [](bno055_axis_remap_sign_t /*new_x_sign*/, bno055_axis_remap_sign_t /*new_y_sign*/, + bno055_axis_remap_sign_t /*new_z_sign*/, bno055_callback_t callback) { callback(write_success); }); -TEST(TEST_NAME, start_sampling__rearead_calib_stat) { - EXPECT_TRUE(false); + imu_init(); + EXPECT_TRUE(bnoHandle.functionGotCalled()); + EXPECT_TRUE(delayHandle.functionGotCalled<_delay_ms>(20)); + EXPECT_TRUE(delayHandle.functionGotCalled<_delay_ms>(20)); + EXPECT_TRUE(delayHandle.functionGotCalled<_delay_ms>(20)); + EXPECT_TRUE(delayHandle.functionGotCalled<_delay_ms>(20)); + EXPECT_TRUE(delayHandle.functionGotCalled<_delay_ms>(20)); + EXPECT_TRUE(delayHandle.functionGotCalled<_delay_ms>(20)); + + /* + * Test: + * * Call start sampling, expect call to euler-angles + * * Call provided callback with read success + * * Call start sampling, expect call to gyro + * * Call provided callback with read success + * * Call start sampling, expect call to acc + * * Call provided callback with read fail + * * Call start sampling, expect call to acc + */ + + bno055_callback_t callback = nullptr; + bnoHandle.overrideFunc( + [&callback](int16_t * /*out*/, bno055_callback_t callback_) { callback = callback_; }); + bnoHandle.overrideFunc( + [&callback](int16_t * /*out*/, bno055_callback_t callback_) { callback = callback_; }); + bnoHandle.overrideFunc( + [&callback](int16_t * /*out*/, bno055_callback_t callback_) { callback = callback_; }); + bnoHandle.overrideFunc([&callback](bno055_status_t *out, bno055_callback_t callback_) { + *out = BNO055_STATUS_SENSOR_FUSION_ALGORITHM_RUNNING; + callback = callback_; + }); + bnoHandle.overrideFunc( + [&callback](bno055_calib_status_t *out, bno055_callback_t callback_) { + out->sys_status = 3; + out->gyr_status = 3; + out->acc_status = 3; + out->mag_status = 3; + callback = callback_; + }); + + imu_start_sampling(); + EXPECT_TRUE(bnoHandle.functionGotCalled()); + EXPECT_FALSE(imu_data_available()); + + callback(read_success); + + imu_start_sampling(); + EXPECT_TRUE(bnoHandle.functionGotCalled()); + EXPECT_FALSE(imu_data_available()); + + callback(read_success); + + imu_start_sampling(); + EXPECT_TRUE(bnoHandle.functionGotCalled()); + EXPECT_FALSE(imu_data_available()); + + callback(read_fail); + + imu_start_sampling(); + EXPECT_TRUE(bnoHandle.functionGotCalled()); + EXPECT_FALSE(imu_data_available()); } -TEST(TEST_NAME, get_data__clear_on_read) { - EXPECT_TRUE(false); +TEST(TEST_NAME, start_sampling__reread_system_state) { + /* + * Init + */ + auto bnoHandle = mock::bno055.getHandle(); + auto delayHandle = mock::delay.getHandle(); + auto errorHandlerHandle = mock::error_handler.getHandle(); + + bnoHandle.overrideFunc( + [](auto /*op_mode*/, bno055_callback_t callback) { callback(write_success); }); + bnoHandle.overrideFunc([](bno055_self_test_result_t *out, bno055_callback_t callback) { + out->mcu_passed = true; + out->acc_passed = true; + out->gyr_passed = true; + out->mag_passed = true; + callback(read_success); + }); + bnoHandle.overrideFunc( + [](bno055_unit_sel_acc /*acc_unit*/, bno055_unit_sel_angular_rate /*angular_rate_unit*/, + bno055_unit_sel_euler_angles /*euler_angles_unit*/, bno055_unit_sel_temperature /*temperature_unit*/, + bno055_unit_sel_orientation_def /*orientation_def*/, + bno055_callback_t callback) { callback(write_success); }); + bnoHandle.overrideFunc( + [](bno055_axis_remap_axis_t /*new_x*/, bno055_axis_remap_axis_t /*new_y*/, + bno055_axis_remap_axis_t /*new_z*/, bno055_callback_t callback) { callback(write_success); }); + bnoHandle.overrideFunc( + [](bno055_axis_remap_sign_t /*new_x_sign*/, bno055_axis_remap_sign_t /*new_y_sign*/, + bno055_axis_remap_sign_t /*new_z_sign*/, bno055_callback_t callback) { callback(write_success); }); + + imu_init(); + EXPECT_TRUE(bnoHandle.functionGotCalled()); + EXPECT_TRUE(delayHandle.functionGotCalled<_delay_ms>(20)); + EXPECT_TRUE(delayHandle.functionGotCalled<_delay_ms>(20)); + EXPECT_TRUE(delayHandle.functionGotCalled<_delay_ms>(20)); + EXPECT_TRUE(delayHandle.functionGotCalled<_delay_ms>(20)); + EXPECT_TRUE(delayHandle.functionGotCalled<_delay_ms>(20)); + EXPECT_TRUE(delayHandle.functionGotCalled<_delay_ms>(20)); + + /* + * Test: + * * Call start sampling, expect call to euler-angles + * * Call provided callback with read success + * * Call start sampling, expect call to gyro + * * Call provided callback with read success + * * Call start sampling, expect call to acc + * * Call provided callback with read success + * * Call start sampling, expect call to system status + * * Call provided callback with read fail + * * Call start sampling, expect call to system status + */ + + bno055_callback_t callback = nullptr; + bnoHandle.overrideFunc( + [&callback](int16_t * /*out*/, bno055_callback_t callback_) { callback = callback_; }); + bnoHandle.overrideFunc( + [&callback](int16_t * /*out*/, bno055_callback_t callback_) { callback = callback_; }); + bnoHandle.overrideFunc( + [&callback](int16_t * /*out*/, bno055_callback_t callback_) { callback = callback_; }); + bnoHandle.overrideFunc([&callback](bno055_status_t *out, bno055_callback_t callback_) { + *out = BNO055_STATUS_SENSOR_FUSION_ALGORITHM_RUNNING; + callback = callback_; + }); + bnoHandle.overrideFunc( + [&callback](bno055_calib_status_t *out, bno055_callback_t callback_) { + out->sys_status = 3; + out->gyr_status = 3; + out->acc_status = 3; + out->mag_status = 3; + callback = callback_; + }); + + imu_start_sampling(); + EXPECT_TRUE(bnoHandle.functionGotCalled()); + EXPECT_FALSE(imu_data_available()); + + callback(read_success); + + imu_start_sampling(); + EXPECT_TRUE(bnoHandle.functionGotCalled()); + EXPECT_FALSE(imu_data_available()); + + callback(read_success); + + imu_start_sampling(); + EXPECT_TRUE(bnoHandle.functionGotCalled()); + EXPECT_FALSE(imu_data_available()); + + callback(read_success); + + imu_start_sampling(); + EXPECT_TRUE(bnoHandle.functionGotCalled()); + EXPECT_FALSE(imu_data_available()); + + callback(read_fail); + + imu_start_sampling(); + EXPECT_TRUE(bnoHandle.functionGotCalled()); + EXPECT_FALSE(imu_data_available()); +} + +TEST(TEST_NAME, start_sampling__rearead_calib_stat) { + /* + * Init + */ + auto bnoHandle = mock::bno055.getHandle(); + auto delayHandle = mock::delay.getHandle(); + auto errorHandlerHandle = mock::error_handler.getHandle(); + + bnoHandle.overrideFunc( + [](auto /*op_mode*/, bno055_callback_t callback) { callback(write_success); }); + bnoHandle.overrideFunc([](bno055_self_test_result_t *out, bno055_callback_t callback) { + out->mcu_passed = true; + out->acc_passed = true; + out->gyr_passed = true; + out->mag_passed = true; + callback(read_success); + }); + bnoHandle.overrideFunc( + [](bno055_unit_sel_acc /*acc_unit*/, bno055_unit_sel_angular_rate /*angular_rate_unit*/, + bno055_unit_sel_euler_angles /*euler_angles_unit*/, bno055_unit_sel_temperature /*temperature_unit*/, + bno055_unit_sel_orientation_def /*orientation_def*/, + bno055_callback_t callback) { callback(write_success); }); + bnoHandle.overrideFunc( + [](bno055_axis_remap_axis_t /*new_x*/, bno055_axis_remap_axis_t /*new_y*/, + bno055_axis_remap_axis_t /*new_z*/, bno055_callback_t callback) { callback(write_success); }); + bnoHandle.overrideFunc( + [](bno055_axis_remap_sign_t /*new_x_sign*/, bno055_axis_remap_sign_t /*new_y_sign*/, + bno055_axis_remap_sign_t /*new_z_sign*/, bno055_callback_t callback) { callback(write_success); }); + + imu_init(); + EXPECT_TRUE(bnoHandle.functionGotCalled()); + EXPECT_TRUE(delayHandle.functionGotCalled<_delay_ms>(20)); + EXPECT_TRUE(delayHandle.functionGotCalled<_delay_ms>(20)); + EXPECT_TRUE(delayHandle.functionGotCalled<_delay_ms>(20)); + EXPECT_TRUE(delayHandle.functionGotCalled<_delay_ms>(20)); + EXPECT_TRUE(delayHandle.functionGotCalled<_delay_ms>(20)); + EXPECT_TRUE(delayHandle.functionGotCalled<_delay_ms>(20)); + + /* + * Test: + * * Call start sampling, expect call to euler-angles + * * Call provided callback with read success + * * Call start sampling, expect call to gyro + * * Call provided callback with read success + * * Call start sampling, expect call to acc + * * Call provided callback with read success + * * Call start sampling, expect call to system status + * * Set system status to sensor fusion algorithm running + * * Call provided callback with read success + * * Call start sampling, expect call to calib-status + * * Set calib status to all calibrated + * * Call provided callback with read fail + * * Call start sampling, expect call to calib-status + */ + + bno055_callback_t callback = nullptr; + bnoHandle.overrideFunc( + [&callback](int16_t * /*out*/, bno055_callback_t callback_) { callback = callback_; }); + bnoHandle.overrideFunc( + [&callback](int16_t * /*out*/, bno055_callback_t callback_) { callback = callback_; }); + bnoHandle.overrideFunc( + [&callback](int16_t * /*out*/, bno055_callback_t callback_) { callback = callback_; }); + bnoHandle.overrideFunc([&callback](bno055_status_t *out, bno055_callback_t callback_) { + *out = BNO055_STATUS_SENSOR_FUSION_ALGORITHM_RUNNING; + callback = callback_; + }); + bnoHandle.overrideFunc( + [&callback](bno055_calib_status_t *out, bno055_callback_t callback_) { + out->sys_status = 3; + out->gyr_status = 3; + out->acc_status = 3; + out->mag_status = 3; + callback = callback_; + }); + + imu_start_sampling(); + EXPECT_TRUE(bnoHandle.functionGotCalled()); + EXPECT_FALSE(imu_data_available()); + + callback(read_success); + + imu_start_sampling(); + EXPECT_TRUE(bnoHandle.functionGotCalled()); + EXPECT_FALSE(imu_data_available()); + + callback(read_success); + + imu_start_sampling(); + EXPECT_TRUE(bnoHandle.functionGotCalled()); + EXPECT_FALSE(imu_data_available()); + + callback(read_success); + + imu_start_sampling(); + EXPECT_TRUE(bnoHandle.functionGotCalled()); + EXPECT_FALSE(imu_data_available()); + + callback(read_success); + + imu_start_sampling(); + EXPECT_TRUE(bnoHandle.functionGotCalled()); + EXPECT_FALSE(imu_data_available()); + + callback(read_fail); + + imu_start_sampling(); + EXPECT_TRUE(bnoHandle.functionGotCalled()); + EXPECT_FALSE(imu_data_available()); +} + +TEST(TEST_NAME, get_data__fields) { + /* + * Init + */ + auto bnoHandle = mock::bno055.getHandle(); + auto delayHandle = mock::delay.getHandle(); + auto errorHandlerHandle = mock::error_handler.getHandle(); + + bnoHandle.overrideFunc( + [](auto /*op_mode*/, bno055_callback_t callback) { callback(write_success); }); + bnoHandle.overrideFunc([](bno055_self_test_result_t *out, bno055_callback_t callback) { + out->mcu_passed = true; + out->acc_passed = true; + out->gyr_passed = true; + out->mag_passed = true; + callback(read_success); + }); + bnoHandle.overrideFunc( + [](bno055_unit_sel_acc /*acc_unit*/, bno055_unit_sel_angular_rate /*angular_rate_unit*/, + bno055_unit_sel_euler_angles /*euler_angles_unit*/, bno055_unit_sel_temperature /*temperature_unit*/, + bno055_unit_sel_orientation_def /*orientation_def*/, + bno055_callback_t callback) { callback(write_success); }); + bnoHandle.overrideFunc( + [](bno055_axis_remap_axis_t /*new_x*/, bno055_axis_remap_axis_t /*new_y*/, + bno055_axis_remap_axis_t /*new_z*/, bno055_callback_t callback) { callback(write_success); }); + bnoHandle.overrideFunc( + [](bno055_axis_remap_sign_t /*new_x_sign*/, bno055_axis_remap_sign_t /*new_y_sign*/, + bno055_axis_remap_sign_t /*new_z_sign*/, bno055_callback_t callback) { callback(write_success); }); + + imu_init(); + EXPECT_TRUE(bnoHandle.functionGotCalled()); + EXPECT_TRUE(delayHandle.functionGotCalled<_delay_ms>(20)); + EXPECT_TRUE(delayHandle.functionGotCalled<_delay_ms>(20)); + EXPECT_TRUE(delayHandle.functionGotCalled<_delay_ms>(20)); + EXPECT_TRUE(delayHandle.functionGotCalled<_delay_ms>(20)); + EXPECT_TRUE(delayHandle.functionGotCalled<_delay_ms>(20)); + EXPECT_TRUE(delayHandle.functionGotCalled<_delay_ms>(20)); + + /* + * Test: + * * Call start sampling + * * Set euler values to 17, 18, 19 + * * Call provided callback with read success + * * Call start sampling + * * Set gyro values to 20, 21, 22 + * * Call provided callback with read success + * * Call start sampling + * * Set acc values to 23, 24, 25 + * * Call provided callback with read success + * * Call start sampling + * * Set system status to sensor fusion algorithm running + * * Call provided callback with read success + * * Call start sampling + * * Set calib status to all calibrated + * * Call provided callback with read success + * * Call start sampling to process result + * * Expect new data available + * * Expect imu_ok is true + * * Check values in new data + */ + + bno055_callback_t callback = nullptr; + bnoHandle.overrideFunc([&callback](int16_t *out, bno055_callback_t callback_) { + out[0] = 17; + out[1] = 18; + out[2] = 19; + callback = callback_; + }); + bnoHandle.overrideFunc([&callback](int16_t *out, bno055_callback_t callback_) { + out[0] = 20; + out[1] = 21; + out[2] = 22; + callback = callback_; + }); + bnoHandle.overrideFunc([&callback](int16_t *out, bno055_callback_t callback_) { + out[0] = 23; + out[1] = 24; + out[2] = 25; + callback = callback_; + }); + bnoHandle.overrideFunc([&callback](bno055_status_t *out, bno055_callback_t callback_) { + *out = BNO055_STATUS_SENSOR_FUSION_ALGORITHM_RUNNING; + callback = callback_; + }); + bnoHandle.overrideFunc( + [&callback](bno055_calib_status_t *out, bno055_callback_t callback_) { + out->sys_status = 3; + out->gyr_status = 3; + out->acc_status = 3; + out->mag_status = 3; + callback = callback_; + }); + + imu_start_sampling(); + EXPECT_FALSE(imu_data_available()); + + callback(read_success); + + imu_start_sampling(); + EXPECT_FALSE(imu_data_available()); + + callback(read_success); + + imu_start_sampling(); + EXPECT_FALSE(imu_data_available()); + + callback(read_success); + + imu_start_sampling(); + EXPECT_FALSE(imu_data_available()); + + callback(read_success); + + imu_start_sampling(); + EXPECT_FALSE(imu_data_available()); + + callback(read_success); + + imu_start_sampling(); + + EXPECT_TRUE(imu_data_available()); + + auto data = imu_get_latest_data(); + EXPECT_TRUE(data.imu_ok); + EXPECT_EQ(data.heading_mul_16, 17); + EXPECT_EQ(data.pitch_mul_16, 18); + EXPECT_EQ(data.roll_mul_16, 19); + EXPECT_EQ(data.d_heading_mul_16, 20); + EXPECT_EQ(data.d_pitch_mul_16, 21); + EXPECT_EQ(data.d_roll_mul_16, 22); + EXPECT_EQ(data.acc_x_mul_100, 23); + EXPECT_EQ(data.acc_y_mul_100, 24); + EXPECT_EQ(data.acc_z_mul_100, 25); +} + +TEST(TEST_NAME, get_data__clear_on_read) { + /* + * Init + */ + auto bnoHandle = mock::bno055.getHandle(); + auto delayHandle = mock::delay.getHandle(); + auto errorHandlerHandle = mock::error_handler.getHandle(); + + bnoHandle.overrideFunc( + [](auto /*op_mode*/, bno055_callback_t callback) { callback(write_success); }); + bnoHandle.overrideFunc([](bno055_self_test_result_t *out, bno055_callback_t callback) { + out->mcu_passed = true; + out->acc_passed = true; + out->gyr_passed = true; + out->mag_passed = true; + callback(read_success); + }); + bnoHandle.overrideFunc( + [](bno055_unit_sel_acc /*acc_unit*/, bno055_unit_sel_angular_rate /*angular_rate_unit*/, + bno055_unit_sel_euler_angles /*euler_angles_unit*/, bno055_unit_sel_temperature /*temperature_unit*/, + bno055_unit_sel_orientation_def /*orientation_def*/, + bno055_callback_t callback) { callback(write_success); }); + bnoHandle.overrideFunc( + [](bno055_axis_remap_axis_t /*new_x*/, bno055_axis_remap_axis_t /*new_y*/, + bno055_axis_remap_axis_t /*new_z*/, bno055_callback_t callback) { callback(write_success); }); + bnoHandle.overrideFunc( + [](bno055_axis_remap_sign_t /*new_x_sign*/, bno055_axis_remap_sign_t /*new_y_sign*/, + bno055_axis_remap_sign_t /*new_z_sign*/, bno055_callback_t callback) { callback(write_success); }); + + imu_init(); + EXPECT_TRUE(bnoHandle.functionGotCalled()); + EXPECT_TRUE(delayHandle.functionGotCalled<_delay_ms>(20)); + EXPECT_TRUE(delayHandle.functionGotCalled<_delay_ms>(20)); + EXPECT_TRUE(delayHandle.functionGotCalled<_delay_ms>(20)); + EXPECT_TRUE(delayHandle.functionGotCalled<_delay_ms>(20)); + EXPECT_TRUE(delayHandle.functionGotCalled<_delay_ms>(20)); + EXPECT_TRUE(delayHandle.functionGotCalled<_delay_ms>(20)); + + /* + * Test: + * * Call start sampling + * * Set euler values to 17, 18, 19 + * * Call provided callback with read success + * * Call start sampling + * * Set gyro values to 20, 21, 22 + * * Call provided callback with read success + * * Call start sampling + * * Set acc values to 23, 24, 25 + * * Call provided callback with read success + * * Call start sampling + * * Set system status to sensor fusion algorithm running + * * Call provided callback with read success + * * Call start sampling + * * Set calib status to all calibrated + * * Call provided callback with read success + * * Call start sampling to process result + * * Expect new data available + * * Read data + * * Expect no new data + */ + + bno055_callback_t callback = nullptr; + bnoHandle.overrideFunc([&callback](int16_t *out, bno055_callback_t callback_) { + out[0] = 17; + out[1] = 18; + out[2] = 19; + callback = callback_; + }); + bnoHandle.overrideFunc([&callback](int16_t *out, bno055_callback_t callback_) { + out[0] = 20; + out[1] = 21; + out[2] = 22; + callback = callback_; + }); + bnoHandle.overrideFunc([&callback](int16_t *out, bno055_callback_t callback_) { + out[0] = 23; + out[1] = 24; + out[2] = 25; + callback = callback_; + }); + bnoHandle.overrideFunc([&callback](bno055_status_t *out, bno055_callback_t callback_) { + *out = BNO055_STATUS_SENSOR_FUSION_ALGORITHM_RUNNING; + callback = callback_; + }); + bnoHandle.overrideFunc( + [&callback](bno055_calib_status_t *out, bno055_callback_t callback_) { + out->sys_status = 3; + out->gyr_status = 3; + out->acc_status = 3; + out->mag_status = 3; + callback = callback_; + }); + + imu_start_sampling(); + EXPECT_FALSE(imu_data_available()); + + callback(read_success); + + imu_start_sampling(); + EXPECT_FALSE(imu_data_available()); + + callback(read_success); + + imu_start_sampling(); + EXPECT_FALSE(imu_data_available()); + + callback(read_success); + + imu_start_sampling(); + EXPECT_FALSE(imu_data_available()); + + callback(read_success); + + imu_start_sampling(); + EXPECT_FALSE(imu_data_available()); + + callback(read_success); + + imu_start_sampling(); + + EXPECT_TRUE(imu_data_available()); + + imu_get_latest_data(); + + EXPECT_FALSE(imu_data_available()); } From 280f646303bf7d5b0c181c1ca545367c7c03b87b Mon Sep 17 00:00:00 2001 From: Paul Nykiel Date: Tue, 24 Jan 2023 19:25:16 +0100 Subject: [PATCH 12/17] Format --- Src/Components/imu.c | 4 +-- Src/HAL | 2 +- Tests/LowLevel/Components/imu.cpp | 48 +++++++++++++++---------------- 3 files changed, 27 insertions(+), 27 deletions(-) diff --git a/Src/Components/imu.c b/Src/Components/imu.c index 4483461..0a2daaa 100644 --- a/Src/Components/imu.c +++ b/Src/Components/imu.c @@ -167,7 +167,7 @@ void imu_start_sampling(void) { callback_ready = false; if (response != read_success) { if (response != bus_over_run_error) { - //error_handler_handle_warning(ERROR_HANDLER_GROUP_BNO055, response + 1); + // error_handler_handle_warning(ERROR_HANDLER_GROUP_BNO055, response + 1); } switch (current_sample_state) { @@ -215,7 +215,7 @@ void imu_start_sampling(void) { case CALIB_STAT: // Reduction of flags to imu_ok if (bno_status != BNO055_STATUS_SENSOR_FUSION_ALGORITHM_RUNNING) { - //error_handler_handle_warning(ERROR_HANDLER_GROUP_IMU, IMU_ERROR_STATUS); + // error_handler_handle_warning(ERROR_HANDLER_GROUP_IMU, IMU_ERROR_STATUS); imu_data->imu_ok = false; } else if (calib_status.sys_status < SYS_CALIB_TRESH || calib_status.gyr_status < GYR_CALIB_TRESH || calib_status.acc_status < ACC_CALIB_TRESH || calib_status.mag_status < MAG_CALIB_TRESH) { diff --git a/Src/HAL b/Src/HAL index d3862c9..8472324 160000 --- a/Src/HAL +++ b/Src/HAL @@ -1 +1 @@ -Subproject commit d3862c9c2031e6704847218fee4ae03615b74e5b +Subproject commit 84723244ae261f433f541e668b2f5e838480307d diff --git a/Tests/LowLevel/Components/imu.cpp b/Tests/LowLevel/Components/imu.cpp index e61421c..5c16635 100644 --- a/Tests/LowLevel/Components/imu.cpp +++ b/Tests/LowLevel/Components/imu.cpp @@ -1859,11 +1859,11 @@ TEST(TEST_NAME, get_data__clear_on_read) { bnoHandle.overrideFunc( [](auto /*op_mode*/, bno055_callback_t callback) { callback(write_success); }); bnoHandle.overrideFunc([](bno055_self_test_result_t *out, bno055_callback_t callback) { - out->mcu_passed = true; - out->acc_passed = true; - out->gyr_passed = true; - out->mag_passed = true; - callback(read_success); + out->mcu_passed = true; + out->acc_passed = true; + out->gyr_passed = true; + out->mag_passed = true; + callback(read_success); }); bnoHandle.overrideFunc( [](bno055_unit_sel_acc /*acc_unit*/, bno055_unit_sel_angular_rate /*angular_rate_unit*/, @@ -1911,34 +1911,34 @@ TEST(TEST_NAME, get_data__clear_on_read) { bno055_callback_t callback = nullptr; bnoHandle.overrideFunc([&callback](int16_t *out, bno055_callback_t callback_) { - out[0] = 17; - out[1] = 18; - out[2] = 19; - callback = callback_; + out[0] = 17; + out[1] = 18; + out[2] = 19; + callback = callback_; }); bnoHandle.overrideFunc([&callback](int16_t *out, bno055_callback_t callback_) { - out[0] = 20; - out[1] = 21; - out[2] = 22; - callback = callback_; + out[0] = 20; + out[1] = 21; + out[2] = 22; + callback = callback_; }); bnoHandle.overrideFunc([&callback](int16_t *out, bno055_callback_t callback_) { - out[0] = 23; - out[1] = 24; - out[2] = 25; - callback = callback_; + out[0] = 23; + out[1] = 24; + out[2] = 25; + callback = callback_; }); bnoHandle.overrideFunc([&callback](bno055_status_t *out, bno055_callback_t callback_) { - *out = BNO055_STATUS_SENSOR_FUSION_ALGORITHM_RUNNING; - callback = callback_; + *out = BNO055_STATUS_SENSOR_FUSION_ALGORITHM_RUNNING; + callback = callback_; }); bnoHandle.overrideFunc( [&callback](bno055_calib_status_t *out, bno055_callback_t callback_) { - out->sys_status = 3; - out->gyr_status = 3; - out->acc_status = 3; - out->mag_status = 3; - callback = callback_; + out->sys_status = 3; + out->gyr_status = 3; + out->acc_status = 3; + out->mag_status = 3; + callback = callback_; }); imu_start_sampling(); From d061cf6127a174379f6476a9d7469c08a13a6c1e Mon Sep 17 00:00:00 2001 From: Paul Nykiel Date: Tue, 24 Jan 2023 19:27:54 +0100 Subject: [PATCH 13/17] Fixed documentation --- Src/Components/actuators.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Src/Components/actuators.h b/Src/Components/actuators.h index 4fa8d9d..0973047 100644 --- a/Src/Components/actuators.h +++ b/Src/Components/actuators.h @@ -30,7 +30,7 @@ void actuators_init(void); /** * @brief Set the actuators. * - * This function sets the actuators, using ::ppm_set, according to the following mapping: + * This function sets the actuators, using ::ppm_channel_set, according to the following mapping: * * elevon-left mapped from [-500, 500] to [0, 1000] to channel 1 * * throttle to channel 2 * * elevon-right mapped from [-500, 500] to [0, 1000] to channel 3 From 5282b2cd47675dc6b217f55b37fc21a3c9408837 Mon Sep 17 00:00:00 2001 From: Paul Nykiel Date: Tue, 24 Jan 2023 19:34:02 +0100 Subject: [PATCH 14/17] Fixed test initialization --- Src/Components/imu.c | 20 +++++++++++--------- 1 file changed, 11 insertions(+), 9 deletions(-) diff --git a/Src/Components/imu.c b/Src/Components/imu.c index 0a2daaa..e87e6f2 100644 --- a/Src/Components/imu.c +++ b/Src/Components/imu.c @@ -40,7 +40,7 @@ static volatile imu_data_t imu_datas[2]; static volatile bool sampling_complete = false; static volatile bno_sampling_state_t current_sample_state; -static volatile uint8_t current_sample_state_id; +static volatile uint8_t current_sample_idx; static void bno_callback(bno055_response_t response_) { @@ -49,6 +49,11 @@ static void bno_callback(bno055_response_t response_) { } void imu_init(void) { + // Signal everything as invalid + imu_datas[0].imu_ok = false; + imu_datas[1].imu_ok = false; + sampling_complete = false; + // Initialize physical connection bno055_init(); @@ -148,10 +153,7 @@ void imu_init(void) { } // Preparation for sampling - imu_datas[0].imu_ok = false; - imu_datas[1].imu_ok = false; - sampling_complete = false; - + current_sample_idx = 0; current_sample_state = INIT; response = read_success; callback_ready = true; @@ -161,7 +163,7 @@ void imu_start_sampling(void) { static bno055_calib_status_t calib_status; static bno055_status_t bno_status; - imu_data_t *imu_data = (imu_data_t *) (&imu_datas[current_sample_state_id]); + imu_data_t *imu_data = (imu_data_t *) (&imu_datas[current_sample_idx]); if (callback_ready) { callback_ready = false; @@ -225,9 +227,9 @@ void imu_start_sampling(void) { } // Signal completed sampling - current_sample_state_id = 1 - current_sample_state_id; + current_sample_idx = 1 - current_sample_idx; sampling_complete = true; - imu_data = (imu_data_t *) (&imu_datas[current_sample_state_id]); + imu_data = (imu_data_t *) (&imu_datas[current_sample_idx]); current_sample_state = EUL; bno055_read_eul_xyz_2_mul_16(&imu_data->heading_mul_16, bno_callback); @@ -241,7 +243,7 @@ void imu_start_sampling(void) { imu_data_t imu_get_latest_data(void) { sampling_complete = false; - return imu_datas[1 - current_sample_state_id]; + return imu_datas[1 - current_sample_idx]; } bool imu_data_available(void) { From b49d9fc00c0970d56d742f72760ce27dafd8ab85 Mon Sep 17 00:00:00 2001 From: Paul Nykiel Date: Tue, 24 Jan 2023 19:41:05 +0100 Subject: [PATCH 15/17] Updated avr/io mock --- Tests/LowLevel/Mock | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Tests/LowLevel/Mock b/Tests/LowLevel/Mock index fdd4f01..0351958 160000 --- a/Tests/LowLevel/Mock +++ b/Tests/LowLevel/Mock @@ -1 +1 @@ -Subproject commit fdd4f0176d4c4aa7718e31d1d3f89a2d2dfe486a +Subproject commit 035195821dbaf42353318c11e041f6739d6bed09 From 4ff68e7bd1d20f69176ea451680191b6853e37e5 Mon Sep 17 00:00:00 2001 From: Paul Nykiel Date: Tue, 24 Jan 2023 19:55:00 +0100 Subject: [PATCH 16/17] Fixed coverage generation --- .github/workflows/low_level_tests.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/low_level_tests.yml b/.github/workflows/low_level_tests.yml index 2c92fc0..7bca4a3 100644 --- a/.github/workflows/low_level_tests.yml +++ b/.github/workflows/low_level_tests.yml @@ -87,7 +87,7 @@ jobs: - name: Set up Build run: cmake -B build - name: Run LCOV - run: lcov --no-external -r "build/*" --capture --directory . --output-file coverage.info + run: lcov --no-external --capture --directory . --output-file coverage.info - name: Gen HTML run: genhtml coverage.info --output-directory coverage - name: Move HTML From bf468201d0efee7db741c27c0628a4bdf00c4572 Mon Sep 17 00:00:00 2001 From: Paul Nykiel Date: Tue, 24 Jan 2023 19:58:32 +0100 Subject: [PATCH 17/17] Removed dead code to increase coverage --- Src/Components/imu.c | 3 -- Tests/LowLevel/Components/imu.cpp | 50 ------------------------------- 2 files changed, 53 deletions(-) diff --git a/Src/Components/imu.c b/Src/Components/imu.c index e87e6f2..2eff36d 100644 --- a/Src/Components/imu.c +++ b/Src/Components/imu.c @@ -188,9 +188,6 @@ void imu_start_sampling(void) { case CALIB_STAT: bno055_read_calib_status(&calib_status, bno_callback); break; - case INIT: - // Shouldn't be here... - break; } } else { switch (current_sample_state) { diff --git a/Tests/LowLevel/Components/imu.cpp b/Tests/LowLevel/Components/imu.cpp index 5c16635..665d104 100644 --- a/Tests/LowLevel/Components/imu.cpp +++ b/Tests/LowLevel/Components/imu.cpp @@ -1298,22 +1298,6 @@ TEST(TEST_NAME, start_sampling__reread_euler) { bno055_callback_t callback = nullptr; bnoHandle.overrideFunc( [&callback](int16_t * /*out*/, bno055_callback_t callback_) { callback = callback_; }); - bnoHandle.overrideFunc( - [&callback](int16_t * /*out*/, bno055_callback_t callback_) { callback = callback_; }); - bnoHandle.overrideFunc( - [&callback](int16_t * /*out*/, bno055_callback_t callback_) { callback = callback_; }); - bnoHandle.overrideFunc([&callback](bno055_status_t *out, bno055_callback_t callback_) { - *out = BNO055_STATUS_SENSOR_FUSION_ALGORITHM_RUNNING; - callback = callback_; - }); - bnoHandle.overrideFunc( - [&callback](bno055_calib_status_t *out, bno055_callback_t callback_) { - out->sys_status = 3; - out->gyr_status = 3; - out->acc_status = 3; - out->mag_status = 3; - callback = callback_; - }); imu_start_sampling(); EXPECT_TRUE(bnoHandle.functionGotCalled()); @@ -1379,20 +1363,6 @@ TEST(TEST_NAME, start_sampling__reread_gyro) { [&callback](int16_t * /*out*/, bno055_callback_t callback_) { callback = callback_; }); bnoHandle.overrideFunc( [&callback](int16_t * /*out*/, bno055_callback_t callback_) { callback = callback_; }); - bnoHandle.overrideFunc( - [&callback](int16_t * /*out*/, bno055_callback_t callback_) { callback = callback_; }); - bnoHandle.overrideFunc([&callback](bno055_status_t *out, bno055_callback_t callback_) { - *out = BNO055_STATUS_SENSOR_FUSION_ALGORITHM_RUNNING; - callback = callback_; - }); - bnoHandle.overrideFunc( - [&callback](bno055_calib_status_t *out, bno055_callback_t callback_) { - out->sys_status = 3; - out->gyr_status = 3; - out->acc_status = 3; - out->mag_status = 3; - callback = callback_; - }); imu_start_sampling(); EXPECT_TRUE(bnoHandle.functionGotCalled()); @@ -1467,18 +1437,6 @@ TEST(TEST_NAME, start_sampling__reread_acc) { [&callback](int16_t * /*out*/, bno055_callback_t callback_) { callback = callback_; }); bnoHandle.overrideFunc( [&callback](int16_t * /*out*/, bno055_callback_t callback_) { callback = callback_; }); - bnoHandle.overrideFunc([&callback](bno055_status_t *out, bno055_callback_t callback_) { - *out = BNO055_STATUS_SENSOR_FUSION_ALGORITHM_RUNNING; - callback = callback_; - }); - bnoHandle.overrideFunc( - [&callback](bno055_calib_status_t *out, bno055_callback_t callback_) { - out->sys_status = 3; - out->gyr_status = 3; - out->acc_status = 3; - out->mag_status = 3; - callback = callback_; - }); imu_start_sampling(); EXPECT_TRUE(bnoHandle.functionGotCalled()); @@ -1565,14 +1523,6 @@ TEST(TEST_NAME, start_sampling__reread_system_state) { *out = BNO055_STATUS_SENSOR_FUSION_ALGORITHM_RUNNING; callback = callback_; }); - bnoHandle.overrideFunc( - [&callback](bno055_calib_status_t *out, bno055_callback_t callback_) { - out->sys_status = 3; - out->gyr_status = 3; - out->acc_status = 3; - out->mag_status = 3; - callback = callback_; - }); imu_start_sampling(); EXPECT_TRUE(bnoHandle.functionGotCalled());