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 diff --git a/README.md b/README.md index 6168908..2ec87cf 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: @@ -54,18 +54,20 @@ The error IDs are: | Component | Exception | LED configuration | |----------------|--------------------------|-------------------| | Application | - | xxxx ◯◯◯🔴 | -| System | Timer Runtime | ◯◯◯🔴 ◯◯🔴◯ | -| | Watchdog | ◯◯🔴◯ ◯◯🔴◯ | -| | Brownout | ◯◯🔴🔴 ◯◯🔴◯ | -| ERROR_HANDLER_GROUP_IMU | Init timeout error | ◯◯◯🔴 ◯◯🔴🔴 | +| System | Watchdog | ◯◯◯🔴 ◯◯🔴◯ | +| | Brownout | ◯◯🔴◯ ◯◯🔴◯ | +| | Low-Prio Timer Runtime | ◯◯🔴🔴 ◯◯🔴◯ | +| | High-Prio Timer Runtime | ◯🔴◯◯ ◯◯🔴◯ | +| 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..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(); diff --git a/Src/Application/application.h b/Src/Application/application.h index 83e4b39..5e37717 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) @@ -37,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/Application/mode_handler.c b/Src/Application/mode_handler.c index bb47113..8a5737d 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) (100 / 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/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 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 27564d5..2eff36d 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, @@ -30,98 +31,56 @@ 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 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_idx; -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) { + // Signal everything as invalid imu_datas[0].imu_ok = false; imu_datas[1].imu_ok = false; + sampling_complete = 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 +93,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,70 +115,132 @@ 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 + current_sample_idx = 0; + current_sample_state = INIT; + response = read_success; + callback_ready = true; } void imu_start_sampling(void) { - 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; + 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_idx]); + + if (callback_ready) { + 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 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); + 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: + current_sample_state = CALIB_STAT; + bno055_read_calib_status(&calib_status, bno_callback); + break; + 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); + 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_idx = 1 - current_sample_idx; + sampling_complete = true; + 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); + break; + } + } + } else { + error_handler_handle_warning(ERROR_HANDLER_GROUP_IMU, IMU_ERROR_READ_TIMEOUT); } } 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) { diff --git a/Src/Components/imu.h b/Src/Components/imu.h index bdfb671..270ada6 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; /** @@ -84,24 +85,48 @@ 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: + * * 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 */ void imu_start_sampling(void); /** - * 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. */ 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(). + * + * 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.c b/Src/Components/system.c index 997d663..ec67e2b 100644 --- a/Src/Components/system.c +++ b/Src/Components/system.c @@ -13,22 +13,38 @@ #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 +57,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..ec52f0a 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,16 +37,30 @@ 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 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..8472324 160000 --- a/Src/HAL +++ b/Src/HAL @@ -1 +1 @@ -Subproject commit bc63d5f8909d112d7a70fa21fe68a20038b507e2 +Subproject commit 84723244ae261f433f541e668b2f5e838480307d 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/imu.cpp b/Tests/LowLevel/Components/imu.cpp index 173abb9..665d104 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,160 +504,1423 @@ 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)); } - -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(); + auto errorHandlerHandle = mock::error_handler.getHandle(); - bnoHandle.overrideFunc([](int16_t *out, bno055_callback_t callback) { - out[0] = 104; - out[1] = 105; - out[2] = 106; + 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)); + + /* + * Test: + * * Call start sampling, expect call to euler-angles + * * Call start sampling without calling callback, expect warning + */ + + 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_success_system_status_invalid) { + /* + * Init + */ + auto bnoHandle = mock::bno055.getHandle(); + auto delayHandle = mock::delay.getHandle(); + auto errorHandlerHandle = mock::error_handler.getHandle(); - bnoHandle.overrideFunc([](bno055_status_t *out, bno055_callback_t callback) { - *out = bno055_status_t::BNO055_STATUS_SENSOR_FUSION_ALGORITHM_RUNNING; + 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) { - out->acc_status = 0; - out->gyr_status = 0; - out->mag_status = 0; - out->sys_status = 0; - 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)); + + /* + * 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 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 + */ + + 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()); - EXPECT_NO_THROW(imu_start_sampling()); - EXPECT_TRUE(imu_data_available()); - auto data = imu_get_latest_data(); + 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()); - 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); + callback(read_success); + + imu_start_sampling(); + + EXPECT_TRUE(imu_data_available()); + EXPECT_FALSE(imu_get_latest_data().imu_ok); } -TEST(TEST_NAME, read__status_error) { - GTEST_SKIP_("Non requirement compliant workaround for non datasheet compliant sensor!"); +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(); - bool alreadyCalled = 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); }); + + 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 but acc calibrated + * * Call provided callback with read success + * * Call start sampling to process result + * * Expect new data available + * * Expect imu_ok is false + */ + + bno055_callback_t callback = nullptr; bnoHandle.overrideFunc( - [&alreadyCalled](int16_t * /*out*/, bno055_callback_t callback) { - if (not alreadyCalled) { - alreadyCalled = true; - callback(read_success); - } - // Break the sampling process + [&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_; }); - bnoHandle.overrideFunc( - [](int16_t * /*out*/, bno055_callback_t callback) { callback(read_success); }); + imu_start_sampling(); + EXPECT_TRUE(bnoHandle.functionGotCalled()); + EXPECT_FALSE(imu_data_available()); - bnoHandle.overrideFunc( - [](int16_t * /*out*/, bno055_callback_t callback) { callback(read_success); }); + callback(read_success); + + imu_start_sampling(); + EXPECT_TRUE(bnoHandle.functionGotCalled()); + EXPECT_FALSE(imu_data_available()); - bnoHandle.overrideFunc([](bno055_status_t *out, bno055_callback_t callback) { - *out = bno055_status_t::BNO055_STATUS_SYSTEM_IDLE; + 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__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); }); + 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 but mag calibrated + * * Call provided callback with read success + * * Call start sampling to process result + * * Expect new data available + * * Expect imu_ok is 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( - [](bno055_calib_status_t * /*out*/, bno055_callback_t callback) { callback(read_success); }); + [&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()); - EXPECT_NO_THROW(imu_start_sampling()); + + callback(read_success); + + imu_start_sampling(); + EXPECT_TRUE(bnoHandle.functionGotCalled()); EXPECT_FALSE(imu_data_available()); - auto data = imu_get_latest_data(); + 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_EQ(data.imu_ok, false); - EXPECT_TRUE(errorHandlerHandle.functionGotCalled(ERROR_HANDLER_GROUP_IMU, IMU_ERROR_STATUS)); + EXPECT_TRUE(imu_data_available()); + EXPECT_FALSE(imu_get_latest_data().imu_ok); } -TEST(TEST_NAME, read__error_call_error_handler) { - GTEST_SKIP_("Non requirement compliant workaround for non datasheet compliant sensor!"); +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); }); + 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 but gyr calibrated + * * Call provided callback with read success + * * Call start sampling to process result + * * Expect new data available + * * Expect imu_ok is false + */ + + bno055_callback_t callback = nullptr; bnoHandle.overrideFunc( - [](int16_t * /*out*/, bno055_callback_t callback) { callback(read_fail); }); + [&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()); - EXPECT_NO_THROW(imu_start_sampling()); + + callback(read_success); + + imu_start_sampling(); + EXPECT_TRUE(bnoHandle.functionGotCalled()); EXPECT_FALSE(imu_data_available()); - auto data = imu_get_latest_data(); + 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); - EXPECT_EQ(data.imu_ok, false); - EXPECT_TRUE(errorHandlerHandle.functionGotCalled(ERROR_HANDLER_GROUP_BNO055, read_fail + 1)); + imu_start_sampling(); + + EXPECT_TRUE(imu_data_available()); + EXPECT_FALSE(imu_get_latest_data().imu_ok); } -TEST(TEST_NAME, read__error_restart) { - GTEST_SKIP_("Non requirement compliant workaround for non datasheet compliant sensor!"); +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(); - int16_t *dataPtr = nullptr; - - bnoHandle.overrideFunc( - [](int16_t * /*out*/, bno055_callback_t callback) { callback(read_success); }); - bnoHandle.overrideFunc([&dataPtr](int16_t *out, bno055_callback_t callback) { - EXPECT_NE(dataPtr, out); - dataPtr = out; - 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); }); + + 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 but sys calibrated + * * Call provided callback with read success + * * Call start sampling to process result + * * Expect new data available + * * Expect imu_ok is 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__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); }); + 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 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) { + /* + * 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_; }); + + 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) { + /* + * 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_; }); + + 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) { + /* + * 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 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_; }); + + 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__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_; + }); + + 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()); - 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)); } diff --git a/Tests/LowLevel/Components/system.cpp b/Tests/LowLevel/Components/system.cpp index f24852e..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_option_t /*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_option_t /*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_option_t /*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_option_t /*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..0351958 160000 --- a/Tests/LowLevel/Mock +++ b/Tests/LowLevel/Mock @@ -1 +1 @@ -Subproject commit 5809143a7e813810a63278e4b1b4a56aabe182e0 +Subproject commit 035195821dbaf42353318c11e041f6739d6bed09