Cansat
/
esp32_BNO08x
2
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

get_system_orientation bugfix and tes 

Signed-off-by: myles-parfeniuk <myles.parfenyuk@gmail.com>
This commit is contained in:
myles-parfeniuk 2025-06-06 21:45:34 -07:00
parent b8e3e6392c
commit e0081794f8
5 changed files with 94 additions and 30 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
source/BNO08x.cpp
View File

@ -1980,13 +1980,13 @@ void BNO08x::print_system_orientation()
}
// Converts a 32-bit signed Q30 fixed-point value to float
// Internal private function, converts a 32-bit signed Q30 fixed-point value to float
static inline float q30_to_float(int32_t q)
{
return ((float)q) / (float)(1UL << 30);
}
// Converts a float to 32-bit signed Q30 fixed-point value
// Internal private function, converts a float to 32-bit signed Q30 fixed-point value
static inline int32_t float_to_q30(float f)
{
if (f > 1.0f) f = 1.0f;
@ -2001,7 +2001,7 @@ static inline int32_t float_to_q30(float f)
* @note Use SQRT2OVER2 as a constant for sqrt(2)/2
* @note that a reset is required to apply changes.
* @note This configuration seems only to work if reports are already enabled.
* e.g. set .rpt.rv.enable(true) prior this call
* e.g. set .rpt.rv.enable(PERIOD) prior this call
*
* @param Qw Real component of mapping quaternion.
* @param Qx X (i) component of mapping quaternion.
@ -2039,7 +2039,7 @@ bool BNO08x::get_system_orientation(float& Qw, float& Qx, float& Qy, float& Qz)
if(!get_frs(BNO08xFrsID::SYSTEM_ORIENTATION, raw, words_rxd))
return false;
if(words_rxd >= 4U)
if(words_rxd <= 4U)
{
// clang-format off
#ifdef CONFIG_ESP32_BNO08x_LOG_STATEMENTS

6
test/CallbackTests.cpp
View File

@ -33,7 +33,7 @@ TEST_CASE("Void Input Param Flavor Cb", "[CallbackAllReportVoidInputParam]")
const constexpr char* TEST_TAG = "Void Input Param Flavor Cb";
static const constexpr uint8_t ENABLED_REPORT_COUNT = 8;
static const constexpr uint8_t RX_REPORT_TRIAL_CNT = ENABLED_REPORT_COUNT * 5;
static const constexpr uint32_t REPORT_PERIOD = 100000UL; // 100ms
constexpr uint32_t REPORT_PERIOD = 100000UL; // 100ms
BNO08x* imu = nullptr;
char msg_buff[200] = {};
@ -216,7 +216,7 @@ TEST_CASE("Report ID Input Param Flavor Cb", "[CallbackAllReportIDInputParam]")
const constexpr char* TEST_TAG = "Report ID Input Param Flavor Cb";
static const constexpr uint8_t ENABLED_REPORT_COUNT = 8;
static const constexpr uint8_t RX_REPORT_TRIAL_CNT = ENABLED_REPORT_COUNT * 5;
static const constexpr uint32_t REPORT_PERIOD = 100000UL; // 100ms
constexpr uint32_t REPORT_PERIOD = 100000UL; // 100ms
BNO08x* imu = nullptr;
char msg_buff[200] = {};
@ -408,7 +408,7 @@ TEST_CASE("Single Report Void Input Param Flavor Cb", "[CallbackSingleReportVoid
const constexpr char* TEST_TAG = "Single Report Void Input Param Flavor Cb";
static const constexpr uint8_t ENABLED_REPORT_COUNT = 1;
static const constexpr uint8_t RX_REPORT_TRIAL_CNT = ENABLED_REPORT_COUNT * 5;
static const constexpr uint32_t REPORT_PERIOD = 100000UL; // 100ms
constexpr uint32_t REPORT_PERIOD = 60000UL; // 60ms
BNO08x* imu = nullptr;
char msg_buff[200] = {};

76
test/FeatureTests.cpp
View File

@ -32,7 +32,7 @@ TEST_CASE("Hard Reset", "[FeatureTests]")
const constexpr char* TEST_TAG = "Hard Reset";
static const constexpr uint8_t ENABLED_REPORT_COUNT = 1;
static const constexpr uint8_t RX_REPORT_TRIAL_CNT = ENABLED_REPORT_COUNT * 5;
static const constexpr uint32_t REPORT_PERIOD = 100000UL; // 100ms
constexpr uint32_t REPORT_PERIOD = 60000UL; // 60ms
BNO08x* imu = nullptr;
char msg_buff[200] = {};
@ -77,7 +77,7 @@ TEST_CASE("Soft Reset", "[FeatureTests]")
const constexpr char* TEST_TAG = "Soft Reset";
static const constexpr uint8_t ENABLED_REPORT_COUNT = 1;
static const constexpr uint8_t RX_REPORT_TRIAL_CNT = ENABLED_REPORT_COUNT * 5;
static const constexpr uint32_t REPORT_PERIOD = 100000UL; // 100ms
constexpr uint32_t REPORT_PERIOD = 60000UL; // 60ms
BNO08x* imu = nullptr;
char msg_buff[200] = {};
@ -122,7 +122,7 @@ TEST_CASE("Sleep", "[FeatureTests]")
const constexpr char* TEST_TAG = "Sleep";
static const constexpr uint8_t ENABLED_REPORT_COUNT = 1;
static const constexpr uint8_t RX_REPORT_TRIAL_CNT = ENABLED_REPORT_COUNT * 5;
static const constexpr uint32_t REPORT_PERIOD = 100000UL; // 100ms
constexpr uint32_t REPORT_PERIOD = 60000UL; // 60ms
BNO08x* imu = nullptr;
char msg_buff[200] = {};
@ -176,7 +176,7 @@ TEST_CASE("Get Metadata", "[FeatureTests]")
const constexpr char* TEST_TAG = "Get Metadata";
static const constexpr uint8_t ENABLED_REPORT_COUNT = 1;
static const constexpr uint8_t RX_REPORT_TRIAL_CNT = ENABLED_REPORT_COUNT * 5;
static const constexpr uint32_t REPORT_PERIOD = 100000UL; // 100ms
constexpr uint32_t REPORT_PERIOD = 60000UL; // 60ms
BNO08x* imu = nullptr;
char msg_buff[200] = {};
@ -224,12 +224,76 @@ TEST_CASE("Get Metadata", "[FeatureTests]")
BNO08xTestHelper::print_test_end_banner(TEST_TAG);
}
TEST_CASE("Set and Get System Orientation", "[FeatureTests]")
{
const constexpr char* TEST_TAG = "Set System Orientation";
constexpr uint32_t REPORT_PERIOD = 60000UL; // 60ms
BNO08x* imu = nullptr;
imu = BNO08xTestHelper::get_test_imu();
char msg_buff[200] = {};
float Qw = 1.0f;
float Qx = 0.0f;
float Qy = 0.0f;
float Qz = 0.0f;
float epsilon = 0.000001f;
// 1. enable report as per FlyteSailing's instructions
TEST_ASSERT_EQUAL(true, imu->rpt.rv.enable(REPORT_PERIOD));
BNO08xTestHelper::print_test_start_banner(TEST_TAG);
// 2. set the system orientation to a value defined in figure 4-3 of the data sheet (X == West Aligned, Y == West Aligned, Z == Up)
sprintf(msg_buff, "Setting orientation to: Qw: %.6f Qx: %.6f Qy: %.6f Qz: %.6f", Qw, Qx, Qy, Qz);
BNO08xTestHelper::print_test_msg(TEST_TAG, msg_buff);
TEST_ASSERT_EQUAL(true, imu->set_system_orientation(Qw, Qx, Qy, Qz));
// 3. hard reset to apply changes and verify they are persistent within flash
TEST_ASSERT_EQUAL(true, imu->hard_reset());
// 4. read back the system orientation frs and verify it contains the orientation we wrote
TEST_ASSERT_EQUAL(true, imu->get_system_orientation(Qw, Qx, Qy, Qz));
sprintf(msg_buff, "Read back orientation: Qw: %.6f Qx: %.6f Qy: %.6f Qz: %.6f", Qw, Qx, Qy, Qz);
BNO08xTestHelper::print_test_msg(TEST_TAG, msg_buff);
// check that values match expected
TEST_ASSERT_EQUAL(true, fabs(1.0 - Qw) < epsilon);
TEST_ASSERT_EQUAL(true, fabs(0.0 - Qx) < epsilon);
TEST_ASSERT_EQUAL(true, fabs(0.0 - Qy) < epsilon);
TEST_ASSERT_EQUAL(true, fabs(0.0 - Qz) < epsilon);
// 5. reset the system orientation to default (all 0.0f)
Qw = 0.0f;
sprintf(msg_buff, "Re-setting orientation to: Qw: %.6f Qx: %.6f Qy: %.6f Qz: %.6f", Qw, Qx, Qy, Qz);
BNO08xTestHelper::print_test_msg(TEST_TAG, msg_buff);
TEST_ASSERT_EQUAL(true, imu->set_system_orientation(Qw, Qx, Qy, Qz));
// 6. hard reset to apply changes and verify they are persistent within flash
TEST_ASSERT_EQUAL(true, imu->hard_reset());
// 7. read back the system orientation frs and verify it has been returned to default
TEST_ASSERT_EQUAL(true, imu->get_system_orientation(Qw, Qx, Qy, Qz));
sprintf(msg_buff, "Read back orientation after reset: Qw: %.6f Qx: %.6f Qy: %.6f Qz: %.6f", Qw, Qx, Qy, Qz);
BNO08xTestHelper::print_test_msg(TEST_TAG, msg_buff);
// check that values match expected
TEST_ASSERT_EQUAL(true, fabs(0.0 - Qw) < epsilon);
TEST_ASSERT_EQUAL(true, fabs(0.0 - Qx) < epsilon);
TEST_ASSERT_EQUAL(true, fabs(0.0 - Qy) < epsilon);
TEST_ASSERT_EQUAL(true, fabs(0.0 - Qz) < epsilon);
TEST_ASSERT_EQUAL(true, imu->disable_all_reports());
BNO08xTestHelper::print_test_end_banner(TEST_TAG);
}
TEST_CASE("Get Sample Counts", "[FeatureTests]")
{
const constexpr char* TEST_TAG = "Get Sample Counts";
static const constexpr uint8_t ENABLED_REPORT_COUNT = 1;
static const constexpr uint8_t RX_REPORT_TRIAL_CNT = ENABLED_REPORT_COUNT * 5;
static const constexpr uint32_t REPORT_PERIOD = 100000UL; // 100ms
constexpr uint32_t REPORT_PERIOD = 60000UL; // 60ms
BNO08x* imu = nullptr;
char msg_buff[200] = {};
@ -277,7 +341,7 @@ TEST_CASE("Get Sample Counts", "[FeatureTests]")
TEST_CASE("Enable Dynamic Calibration", "[FeatureTests]")
{
const constexpr char* TEST_TAG = "Enable Dynamic Calibration";
static const constexpr uint32_t REPORT_PERIOD = 10000UL; // 10ms
constexpr uint32_t REPORT_PERIOD = 60000UL; // 60ms
static const constexpr uint8_t ENABLED_REPORT_COUNT = 1;
static const constexpr uint8_t RX_REPORT_TRIAL_CNT = ENABLED_REPORT_COUNT * 5;

6
test/MultiReportTests.cpp
View File

@ -32,7 +32,7 @@ TEST_CASE("Enable/Disable Dual Report", "[MultiReportEnableDisable]")
const constexpr char* TEST_TAG = "Enable/Disable Dual Report";
static const constexpr uint8_t ENABLED_REPORT_COUNT = 2;
static const constexpr uint8_t RX_REPORT_TRIAL_CNT = ENABLED_REPORT_COUNT * 5;
static const constexpr uint32_t REPORT_PERIOD = 100000UL; // 100ms
constexpr uint32_t REPORT_PERIOD = 60000UL; // 60ms
BNO08x* imu = nullptr;
char msg_buff[200] = {};
@ -90,7 +90,7 @@ TEST_CASE("Enable/Disable Quad Report", "[MultiReportEnableDisable]")
const constexpr char* TEST_TAG = "Enable/Disable Quad Report";
static const constexpr uint8_t ENABLED_REPORT_COUNT = 4;
static const constexpr uint8_t RX_REPORT_TRIAL_CNT = ENABLED_REPORT_COUNT * 5;
static const constexpr uint32_t REPORT_PERIOD = 100000UL; // 100ms
constexpr uint32_t REPORT_PERIOD = 60000UL; // 60ms
BNO08x* imu = nullptr;
char msg_buff[200] = {};
@ -177,7 +177,7 @@ TEST_CASE("Enable/Disable Octo Report", "[MultiReportEnableDisable]")
const constexpr char* TEST_TAG = "Enable/Disable Octo Report";
static const constexpr uint8_t ENABLED_REPORT_COUNT = 8;
static const constexpr uint8_t RX_REPORT_TRIAL_CNT = ENABLED_REPORT_COUNT * 5;
static const constexpr uint32_t REPORT_PERIOD = 100000UL; // 100ms
constexpr uint32_t REPORT_PERIOD = 60000UL; // 60ms
BNO08x* imu = nullptr;
char msg_buff[200] = {};

28
test/SingleReportTests.cpp
View File

@ -31,7 +31,7 @@ TEST_CASE("Enable Incorrect Report", "[SingleReportEnableDisable]")
{
const constexpr char* TEST_TAG = "Enable Incorrect Report";
static const constexpr uint8_t RX_REPORT_TRIAL_CNT = 5;
static const constexpr uint32_t REPORT_PERIOD = 100000UL; // 100ms
constexpr uint32_t REPORT_PERIOD = 60000UL; // 60ms
BNO08x* imu = nullptr;
char msg_buff[200] = {};
@ -71,7 +71,7 @@ TEST_CASE("Enable/Disable Accelerometer", "[SingleReportEnableDisable]")
{
const constexpr char* TEST_TAG = "Enable/Disable Accelerometer";
static const constexpr uint8_t RX_REPORT_TRIAL_CNT = 5;
static const constexpr uint32_t REPORT_PERIOD = 100000UL; // 100ms
constexpr uint32_t REPORT_PERIOD = 60000UL; // 60ms
BNO08x* imu = nullptr;
char msg_buff[200] = {};
@ -109,7 +109,7 @@ TEST_CASE("Enable/Disable Linear Accelerometer", "[SingleReportEnableDisable]")
{
const constexpr char* TEST_TAG = "Enable/Disable Linear Accelerometer";
static const constexpr uint8_t RX_REPORT_TRIAL_CNT = 5;
static const constexpr uint32_t REPORT_PERIOD = 100000UL; // 100ms
constexpr uint32_t REPORT_PERIOD = 60000UL; // 60ms
BNO08x* imu = nullptr;
char msg_buff[200] = {};
@ -149,7 +149,7 @@ TEST_CASE("Enable/Disable Gravity", "[SingleReportEnableDisable]")
{
const constexpr char* TEST_TAG = "Enable/Disable Gravity";
static const constexpr uint8_t RX_REPORT_TRIAL_CNT = 5;
static const constexpr uint32_t REPORT_PERIOD = 100000UL; // 100ms
constexpr uint32_t REPORT_PERIOD = 60000UL; // 60ms
BNO08x* imu = nullptr;
char msg_buff[200] = {};
@ -187,7 +187,7 @@ TEST_CASE("Enable/Disable Cal Magnetometer", "[SingleReportEnableDisable]")
{
const constexpr char* TEST_TAG = "Enable/Disable Cal Magnetometer";
static const constexpr uint8_t RX_REPORT_TRIAL_CNT = 5;
static const constexpr uint32_t REPORT_PERIOD = 100000UL; // 100ms
constexpr uint32_t REPORT_PERIOD = 60000UL; // 60ms
BNO08x* imu = nullptr;
char msg_buff[200] = {};
@ -227,7 +227,7 @@ TEST_CASE("Enable/Disable Uncal Magnetometer", "[SingleReportEnableDisable]")
{
const constexpr char* TEST_TAG = "Enable/Disable Uncal Magnetometer";
static const constexpr uint8_t RX_REPORT_TRIAL_CNT = 5;
static const constexpr uint32_t REPORT_PERIOD = 100000UL; // 100ms
constexpr uint32_t REPORT_PERIOD = 60000UL; // 60ms
BNO08x* imu = nullptr;
char msg_buff[200] = {};
@ -269,7 +269,7 @@ TEST_CASE("Enable/Disable Cal Gyro", "[SingleReportEnableDisable]")
{
const constexpr char* TEST_TAG = "Enable/Disable Cal Gyro";
static const constexpr uint8_t RX_REPORT_TRIAL_CNT = 5;
static const constexpr uint32_t REPORT_PERIOD = 100000UL; // 100ms
constexpr uint32_t REPORT_PERIOD = 60000UL; // 60ms
BNO08x* imu = nullptr;
char msg_buff[200] = {};
@ -307,7 +307,7 @@ TEST_CASE("Enable/Disable Uncal Gyro", "[SingleReportEnableDisable]")
{
const constexpr char* TEST_TAG = "Enable/Disable Uncal Gyro";
static const constexpr uint8_t RX_REPORT_TRIAL_CNT = 5;
static const constexpr uint32_t REPORT_PERIOD = 100000UL; // 100ms
constexpr uint32_t REPORT_PERIOD = 60000UL; // 60ms
BNO08x* imu = nullptr;
char msg_buff[200] = {};
@ -349,7 +349,7 @@ TEST_CASE("Enable/Disable RV", "[SingleReportEnableDisable]")
{
const constexpr char* TEST_TAG = "Enable/Disable RV";
static const constexpr uint8_t RX_REPORT_TRIAL_CNT = 5;
static const constexpr uint32_t REPORT_PERIOD = 100000UL; // 100ms
constexpr uint32_t REPORT_PERIOD = 60000UL; // 60ms
BNO08x* imu = nullptr;
char msg_buff[200] = {};
@ -389,7 +389,7 @@ TEST_CASE("Enable/Disable Game RV", "[SingleReportEnableDisable]")
{
const constexpr char* TEST_TAG = "Enable/Disable Game RV";
static const constexpr uint8_t RX_REPORT_TRIAL_CNT = 5;
static const constexpr uint32_t REPORT_PERIOD = 100000UL; // 100ms
constexpr uint32_t REPORT_PERIOD = 60000UL; // 60ms
BNO08x* imu = nullptr;
char msg_buff[200] = {};
@ -429,7 +429,7 @@ TEST_CASE("Enable/Disable ARVR Stabilized RV", "[SingleReportEnableDisable]")
{
const constexpr char* TEST_TAG = "Enable/Disable ARVR Stabilized RV";
static const constexpr uint8_t RX_REPORT_TRIAL_CNT = 5;
static const constexpr uint32_t REPORT_PERIOD = 100000UL; // 100ms
constexpr uint32_t REPORT_PERIOD = 60000UL; // 60ms
BNO08x* imu = nullptr;
char msg_buff[200] = {};
@ -469,7 +469,7 @@ TEST_CASE("Enable/Disable ARVR Stabilized Game RV", "[SingleReportEnableDisable]
{
const constexpr char* TEST_TAG = "Enable/Disable ARVR Stabilized Game RV";
static const constexpr uint8_t RX_REPORT_TRIAL_CNT = 5;
static const constexpr uint32_t REPORT_PERIOD = 100000UL; // 100ms
constexpr uint32_t REPORT_PERIOD = 60000UL; // 60ms
BNO08x* imu = nullptr;
char msg_buff[200] = {};
@ -509,7 +509,7 @@ TEST_CASE("Enable/Disable Gyro Integrated RV", "[SingleReportEnableDisable]")
{
const constexpr char* TEST_TAG = "Enable/Disable Gyro Integrated RV";
static const constexpr uint8_t RX_REPORT_TRIAL_CNT = 5;
static const constexpr uint32_t REPORT_PERIOD = 100000UL; // 100ms
constexpr uint32_t REPORT_PERIOD = 60000UL; // 60ms
BNO08x* imu = nullptr;
char msg_buff[200] = {};
@ -549,7 +549,7 @@ TEST_CASE("Enable/Disable Geomagnetic RV", "[SingleReportEnableDisable]")
{
const constexpr char* TEST_TAG = "Enable/Disable Geomagnetic RV";
static const constexpr uint8_t RX_REPORT_TRIAL_CNT = 5;
static const constexpr uint32_t REPORT_PERIOD = 100000UL; // 100ms
constexpr uint32_t REPORT_PERIOD = 60000UL; // 60ms
BNO08x* imu = nullptr;
char msg_buff[200] = {};