From 0f4e13f3a935bfeb104be5f096021113d83949ca Mon Sep 17 00:00:00 2001 From: myles-parfeniuk Date: Thu, 14 Nov 2024 17:48:02 -0800 Subject: [PATCH] Report tests --- include/BNO08x.hpp | 17 ++- include/BNO08xTestSuite.hpp | 16 ++- source/BNO08x.cpp | 129 ++++++++++++++---- test/InitDeinitTests.cpp | 4 +- test/ReportTests.cpp | 254 ++++++++++++++++++++++++++++++++++++ 5 files changed, 389 insertions(+), 31 deletions(-) diff --git a/include/BNO08x.hpp b/include/BNO08x.hpp index 813def6..f2e716f 100644 --- a/include/BNO08x.hpp +++ b/include/BNO08x.hpp @@ -40,7 +40,8 @@ enum class IMUAccuracy { LOW = 1, MED, - HIGH + HIGH, + UNDEFINED }; /// @brief Reason for previous IMU reset (returned by get_reset_reason()) @@ -171,9 +172,11 @@ class BNO08x void save_tare(); void clear_tare(); - bool data_available(); + bool data_available(bool ignore_no_reports_enabled = false); void register_cb(std::function cb_fxn); + void reset_all_data(); + uint32_t get_time_stamp(); void get_magf(float& x, float& y, float& z, uint8_t& accuracy); @@ -201,6 +204,7 @@ class BNO08x float get_quat_J(); float get_quat_K(); float get_quat_real(); + uint8_t get_raw_quat_radian_accuracy(); float get_quat_radian_accuracy(); uint8_t get_quat_accuracy(); @@ -469,9 +473,12 @@ class BNO08x 200UL / portTICK_PERIOD_MS; ///(IMUAccuracy::LOW); float quat_I = 0; float quat_J = 0; float quat_K = 0; float quat_real = 0; - uint8_t quat_accuracy = (uint8_t) IMUAccuracy::LOW; + uint8_t quat_accuracy = static_cast(IMUAccuracy::LOW); uint16_t high_accuracy = 0; uint16_t save_calibration_attempt = 0; @@ -1084,7 +1086,7 @@ bool BNO08x::run_full_calibration_routine() vTaskDelay(5 / portTICK_PERIOD_MS); - if ((magnetometer_accuracy >= (uint8_t) IMUAccuracy::MED) && (quat_accuracy == (uint8_t) IMUAccuracy::HIGH)) + if ((magnetometer_accuracy >= static_cast(IMUAccuracy::MED)) && (quat_accuracy == static_cast(IMUAccuracy::HIGH))) high_accuracy++; else high_accuracy = 0; @@ -1130,15 +1132,17 @@ bool BNO08x::run_full_calibration_routine() /** * @brief Checks if BNO08x has asserted interrupt and sent data. * + * @param ignore_no_reports_enabled Forces a wait for data even if no reports are enabled (default is false), used for unit tests. * @return true if new data has been parsed and saved */ -bool BNO08x::data_available() +bool BNO08x::data_available(bool ignore_no_reports_enabled) { - if (xEventGroupGetBits(evt_grp_report_en) == 0) - { - ESP_LOGE(TAG, "No reports enabled."); - return false; - } + if (!ignore_no_reports_enabled) + if (xEventGroupGetBits(evt_grp_report_en) == 0) + { + ESP_LOGE(TAG, "No reports enabled."); + return false; + } return wait_for_data(); } @@ -1869,7 +1873,7 @@ void BNO08x::disable_raw_magnetometer() void BNO08x::tare_now(uint8_t axis_sel, uint8_t rotation_vector_basis) { queue_tare_command(TARE_NOW, axis_sel, rotation_vector_basis); - wait_for_tx_done(); // wait for transmit operation to complete + wait_for_tx_done(); // wait for transmit operation to complete vTaskDelay(CMD_EXECUTION_DELAY_MS); // allow some time for command to be executed } @@ -1881,7 +1885,7 @@ void BNO08x::tare_now(uint8_t axis_sel, uint8_t rotation_vector_basis) void BNO08x::save_tare() { queue_tare_command(TARE_PERSIST); - wait_for_tx_done(); // wait for transmit operation to complete + wait_for_tx_done(); // wait for transmit operation to complete vTaskDelay(CMD_EXECUTION_DELAY_MS); // allow some time for command to be executed } @@ -1893,7 +1897,7 @@ void BNO08x::save_tare() void BNO08x::clear_tare() { queue_tare_command(TARE_SET_REORIENTATION); - wait_for_tx_done(); // wait for transmit operation to complete + wait_for_tx_done(); // wait for transmit operation to complete vTaskDelay(CMD_EXECUTION_DELAY_MS); // allow some time for command to be executed } @@ -1916,13 +1920,84 @@ float BNO08x::q_to_float(int16_t fixed_point_value, uint8_t q_point) /** * @brief Return timestamp of most recent report. * - * @return void, nothing to return + * @return uint32_t containing the timestamp of the most recent report in microseconds. */ uint32_t BNO08x::get_time_stamp() { return time_stamp; } +/** + * @brief Resets all data returned by public getter APIs to initial values of 0 and low accuracy. + * + * @return void, nothing to return + */ +void BNO08x::reset_all_data() +{ + time_stamp = 0UL; + + raw_accel_X = 0U; + raw_accel_Y = 0U; + raw_accel_Z = 0U; + accel_accuracy = static_cast(IMUAccuracy::UNDEFINED); + + raw_lin_accel_X = 0U; + raw_lin_accel_Y = 0U; + raw_lin_accel_Z = 0U; + accel_lin_accuracy = static_cast(IMUAccuracy::UNDEFINED); + + raw_gyro_X = 0U; + raw_gyro_Y = 0U; + raw_gyro_Z = 0U; + gyro_accuracy = static_cast(IMUAccuracy::UNDEFINED); + + raw_quat_I = 0U; + raw_quat_J = 0U; + raw_quat_K = 0U; + raw_quat_real = 1U; + raw_quat_radian_accuracy = static_cast(IMUAccuracy::UNDEFINED); + quat_accuracy = static_cast(IMUAccuracy::UNDEFINED); + + raw_velocity_gyro_X = 0U; + raw_velocity_gyro_Y = 0U; + raw_velocity_gyro_Z = 0U; + + gravity_X = 0U; + gravity_Y = 0U; + gravity_Z = 0U; + gravity_accuracy = static_cast(IMUAccuracy::UNDEFINED); + + raw_uncalib_gyro_X = 0U; + raw_uncalib_gyro_Y = 0U; + raw_uncalib_gyro_Z = 0U; + raw_bias_X = 0U; + raw_bias_Y = 0U; + raw_bias_Z = 0U; + uncalib_gyro_accuracy = static_cast(IMUAccuracy::UNDEFINED); + + raw_magf_X = 0U; + raw_magf_Y = 0U; + raw_magf_Z = 0U; + magf_accuracy = static_cast(IMUAccuracy::UNDEFINED); + + tap_detector = 0U; + step_count = 0U; + stability_classifier = 0U; + activity_classifier = 0U; + + mems_raw_accel_X = 0U; + mems_raw_accel_Y = 0U; + mems_raw_accel_Z = 0U; + + mems_raw_gyro_X = 0U; + mems_raw_gyro_Y = 0U; + mems_raw_gyro_Z = 0U; + + mems_raw_magf_X = 0U; + mems_raw_magf_Y = 0U; + mems_raw_magf_Z = 0U; +} + /** * @brief Get the full magnetic field vector. * @@ -2221,6 +2296,16 @@ float BNO08x::get_quat_real() return quat; } +/** + * @brief Get the raw radian accuracy of reported quaternion before it is converted to float. + * + * @return The raw radian accuracy of reported quaternion. + */ +uint8_t BNO08x::get_raw_quat_radian_accuracy() +{ + return raw_quat_radian_accuracy; +} + /** * @brief Get radian accuracy of reported quaternion. * @@ -3193,7 +3278,7 @@ esp_err_t BNO08x::kill_all_tasks() bno08x_rx_packet_t dummy_packet; sem_kill_tasks = xSemaphoreCreateCounting(init_status.task_count, 0); - memset(&dummy_packet, 0, sizeof(dummy_packet)); + memset(&dummy_packet, 0, sizeof(bno08x_rx_packet_t)); xEventGroupClearBits( evt_grp_task_flow, EVT_GRP_TSK_FLW_RUNNING_BIT); // clear task running bit in task flow event group to request deletion of tasks diff --git a/test/InitDeinitTests.cpp b/test/InitDeinitTests.cpp index 8a64435..d81d497 100644 --- a/test/InitDeinitTests.cpp +++ b/test/InitDeinitTests.cpp @@ -94,9 +94,9 @@ TEST_CASE("Finish Init", "[Init]") BNO08xTestHelper::print_test_end_banner(TEST_TAG); } -TEST_CASE("Full Init & Deinit", "[FullInitDenit]") +TEST_CASE("Init & Deinit", "[InitDenit]") { - const constexpr char* TEST_TAG = "Full Init & Deinit"; + const constexpr char* TEST_TAG = "Init & Deinit"; BNO08x* imu = nullptr; BNO08xTestHelper::print_test_start_banner(TEST_TAG); diff --git a/test/ReportTests.cpp b/test/ReportTests.cpp index 7f9578b..8f93b92 100644 --- a/test/ReportTests.cpp +++ b/test/ReportTests.cpp @@ -1,2 +1,256 @@ #include "unity.h" #include "../include/BNO08xTestHelper.hpp" + +static const constexpr uint8_t RX_REPORT_TRIAL_CNT = 5; + +typedef struct imu_report_data_t +{ + uint32_t time_stamp; + + float quat_I; + float quat_J; + float quat_K; + float quat_real; + IMUAccuracy raw_quat_radian_accuracy; + IMUAccuracy quat_accuracy; + +} imu_report_data_t; + +void update_report_data(imu_report_data_t* report_data, BNO08x* imu) +{ + report_data->quat_I = imu->get_quat_I(); + report_data->quat_J = imu->get_quat_J(); + report_data->quat_K = imu->get_quat_K(); + report_data->quat_real = imu->get_quat_real(); + report_data->raw_quat_radian_accuracy = static_cast(imu->get_raw_quat_radian_accuracy()); + report_data->quat_accuracy = static_cast(imu->get_quat_accuracy()); +} + +TEST_CASE("Enable/Disable Rotation Vector", "[ReportEnableDisable]") +{ + const constexpr char* TEST_TAG = "Enable/Disable Rotation Vector"; + BNO08x* imu = nullptr; + imu_report_data_t report_data; + bool new_data = false; + char msg_buff[200] = {}; + + BNO08xTestHelper::print_test_start_banner(TEST_TAG); + + BNO08xTestHelper::print_test_msg(TEST_TAG, "Creating & initializing IMU for Enable/Disable report tests."); + BNO08xTestHelper::create_test_imu(); + imu = BNO08xTestHelper::get_test_imu(); + + // ensure IMU initialized successfully + TEST_ASSERT_EQUAL(true, imu->initialize()); + + // reset all data used in report test + imu->reset_all_data(); + update_report_data(&report_data, imu); + + BNO08xTestHelper::print_test_msg(TEST_TAG, "Report enabled testing phase started."); + /*enable respective report to test and ensure it reports new data */ + imu->enable_rotation_vector(100000UL); + + for (int i = 0; i < RX_REPORT_TRIAL_CNT; i++) + { + new_data = false; + + if (imu->data_available()) + { + update_report_data(&report_data, imu); + + // check if any default values have been overwritten, implying new data from respective report + if (report_data.quat_I != 0.0f) + new_data = true; + + if (report_data.quat_J != 0.0f) + new_data = true; + + if (report_data.quat_K != 0.0f) + new_data = true; + + if (report_data.quat_real != 1.0f) + new_data = true; + + // if the accuracy still contains its default value, something has gone wrong, a defined accuracy should be received with every report + if (report_data.quat_accuracy == IMUAccuracy::UNDEFINED) + new_data = false; + + if (report_data.raw_quat_radian_accuracy == IMUAccuracy::UNDEFINED) + new_data = false; + } + + // assert whether new data was received or not + TEST_ASSERT_EQUAL(true, new_data); + + sprintf(msg_buff, "Enabled Report Rx Data Trial %d Success: I: %.2lf J: %.2lf K: %.2lf real: %.2lf", (i + 1), report_data.quat_I, + report_data.quat_J, report_data.quat_K, report_data.quat_real); + BNO08xTestHelper::print_test_msg(TEST_TAG, msg_buff); + + // reset all data used in report test + imu->reset_all_data(); + update_report_data(&report_data, imu); + } + + BNO08xTestHelper::print_test_msg(TEST_TAG, "Report enabled testing phase completed."); + + BNO08xTestHelper::print_test_msg(TEST_TAG, "Report disabled testing phase started."); + /*disable respective report to test and ensure it stops reporting new data */ + imu->disable_rotation_vector(); + for (int i = 0; i < RX_REPORT_TRIAL_CNT; i++) + { + new_data = false; + + // use "true" argument to force wait for data even if no reports are enabled + if (imu->data_available(true)) + { + update_report_data(&report_data, imu); + + // check if any default values have been overwritten, implying new data from respective report + if (report_data.quat_I != 0.0f) + new_data = true; + + if (report_data.quat_J != 0.0f) + new_data = true; + + if (report_data.quat_K != 0.0f) + new_data = true; + + if (report_data.quat_real != 1.0f) + new_data = true; + + // if the accuracy does not contain its default value, something has gone wrong, respective report should be disabled + if (report_data.quat_accuracy != IMUAccuracy::UNDEFINED) + new_data = true; + + if (report_data.raw_quat_radian_accuracy != IMUAccuracy::UNDEFINED) + new_data = true; + } + + // assert that no new data from this report has been received + TEST_ASSERT_NOT_EQUAL(true, new_data); + + sprintf(msg_buff, "Disabled Report No Rx Data Trial %d Success", (i + 1)); + BNO08xTestHelper::print_test_msg(TEST_TAG, msg_buff); + + // reset all data used in report test + imu->reset_all_data(); + update_report_data(&report_data, imu); + } + + BNO08xTestHelper::print_test_msg(TEST_TAG, "Report disabled testing phase completed."); + + BNO08xTestHelper::print_test_end_banner(TEST_TAG); +} + +TEST_CASE("Enable/Disable Game Rotation Vector", "[ReportEnableDisable]") +{ + const constexpr char* TEST_TAG = "Enable/Disable Game Rotation Vector"; + BNO08x* imu = nullptr; + imu_report_data_t report_data; + bool new_data = false; + char msg_buff[200] = {}; + + BNO08xTestHelper::print_test_start_banner(TEST_TAG); + + imu = BNO08xTestHelper::get_test_imu(); + + // reset all data used in report test + imu->reset_all_data(); + update_report_data(&report_data, imu); + + BNO08xTestHelper::print_test_msg(TEST_TAG, "Report enabled testing phase started."); + /*enable respective report to test and ensure it reports new data */ + imu->enable_game_rotation_vector(100000UL); + + for (int i = 0; i < RX_REPORT_TRIAL_CNT; i++) + { + new_data = false; + + if (imu->data_available()) + { + update_report_data(&report_data, imu); + + // check if any default values have been overwritten, implying new data from respective report + if (report_data.quat_I != 0.0f) + new_data = true; + + if (report_data.quat_J != 0.0f) + new_data = true; + + if (report_data.quat_K != 0.0f) + new_data = true; + + if (report_data.quat_real != 1.0f) + new_data = true; + + // if the accuracy still contains its default value, something has gone wrong, a defined accuracy should be received with every report + if (report_data.quat_accuracy == IMUAccuracy::UNDEFINED) + new_data = false; + + if (report_data.raw_quat_radian_accuracy == IMUAccuracy::UNDEFINED) + new_data = false; + } + + // assert whether new data was received or not + TEST_ASSERT_EQUAL(true, new_data); + + sprintf(msg_buff, "Enabled Report Rx Data Trial %d Success: I: %.2lf J: %.2lf K: %.2lf real: %.2lf", (i + 1), report_data.quat_I, + report_data.quat_J, report_data.quat_K, report_data.quat_real); + BNO08xTestHelper::print_test_msg(TEST_TAG, msg_buff); + + // reset all data used in report test + imu->reset_all_data(); + update_report_data(&report_data, imu); + } + + BNO08xTestHelper::print_test_msg(TEST_TAG, "Report enabled testing phase completed."); + + BNO08xTestHelper::print_test_msg(TEST_TAG, "Report disabled testing phase started."); + /*disable respective report to test and ensure it stops reporting new data */ + imu->disable_game_rotation_vector(); + for (int i = 0; i < RX_REPORT_TRIAL_CNT; i++) + { + new_data = false; + + // use "true" argument to force wait for data even if no reports are enabled + if (imu->data_available(true)) + { + update_report_data(&report_data, imu); + + // check if any default values have been overwritten, implying new data from respective report + if (report_data.quat_I != 0.0f) + new_data = true; + + if (report_data.quat_J != 0.0f) + new_data = true; + + if (report_data.quat_K != 0.0f) + new_data = true; + + if (report_data.quat_real != 1.0f) + new_data = true; + + // if the accuracy does not contain its default value, something has gone wrong, respective report should be disabled + if (report_data.quat_accuracy != IMUAccuracy::UNDEFINED) + new_data = true; + + if (report_data.raw_quat_radian_accuracy != IMUAccuracy::UNDEFINED) + new_data = true; + } + + // assert that no new data from this report has been received + TEST_ASSERT_NOT_EQUAL(true, new_data); + + sprintf(msg_buff, "Disabled Report No Rx Data Trial %d Success", (i + 1)); + BNO08xTestHelper::print_test_msg(TEST_TAG, msg_buff); + + // reset all data used in report test + imu->reset_all_data(); + update_report_data(&report_data, imu); + } + + BNO08xTestHelper::print_test_msg(TEST_TAG, "Report disabled testing phase completed."); + + BNO08xTestHelper::print_test_end_banner(TEST_TAG); +} \ No newline at end of file