re-introduced dynamic calibration routine, disabled turn-table calibration
Signed-off-by: myles <myles.parfenyuk@gmail.com>
This commit is contained in:
myles
parent
72042763bf
commit
85ad14a030
|
|
@ -27,9 +27,6 @@ SpaceAfterCStyleCast: true
|
|||
CommentPragmas: '^[/!]<'
|
||||
|
||||
ColumnLimit: 130
|
||||
WrapComments: true
|
||||
AllowShortCommentsOnASingleLine: true
|
||||
AlignConsecutiveComments: true
|
||||
|
||||
BreakBeforeBraces: Allman
|
||||
IndentAccessModifiers: true
|
||||
|
|
|
|||
|
|
@ -44,15 +44,16 @@ class BNO08x
|
|||
bool on();
|
||||
bool sleep();
|
||||
|
||||
bool calibration_start(uint32_t period_us);
|
||||
bool calibration_end(sh2_CalStatus_t& status);
|
||||
// bool calibration_turntable_start(uint32_t period_us);
|
||||
// bool calibration_turntable_end(sh2_CalStatus_t& status);
|
||||
|
||||
bool dynamic_calibration_enable(BNO08xCalSel sensor);
|
||||
bool dynamic_calibration_disable(BNO08xCalSel sensor);
|
||||
bool dynamic_calibration_autosave_enable();
|
||||
bool dynamic_calibration_autosave_disable();
|
||||
bool save_dynamic_calibration();
|
||||
bool clear_dynamic_calibration();
|
||||
bool dynamic_calibration_save();
|
||||
bool dynamic_calibration_clear();
|
||||
bool dynamic_calibration_run_routine();
|
||||
|
||||
bool get_frs(uint16_t frs_ID, uint32_t (&data)[16], uint16_t& rx_data_sz);
|
||||
sh2_ProductIds_t get_product_IDs();
|
||||
|
|
|
|||
|
|
@ -185,6 +185,7 @@ void BNO08x::sh2_HAL_service_task_trampoline(void* arg)
|
|||
void BNO08x::sh2_HAL_service_task()
|
||||
{
|
||||
EventBits_t evt_grp_bno08x_task_bits = 0U;
|
||||
|
||||
// clang-format off
|
||||
#ifdef CONFIG_ESP32_BNO08x_DEBUG_STATEMENTS
|
||||
int64_t last_hint_time = esp_timer_get_time();
|
||||
|
|
@ -1260,8 +1261,11 @@ bool BNO08x::sleep()
|
|||
*
|
||||
* @return True if start simple calibration operation succeeded.
|
||||
*/
|
||||
bool BNO08x::calibration_start(uint32_t period_us)
|
||||
/*
|
||||
bool BNO08x::calibration_turntable_start(uint32_t period_us)
|
||||
{
|
||||
// currently broken, correct packet is sent over SPI but IMU responds
|
||||
// with unsolicited initialize response instead of the expected Turntable Cal response (0x0C)
|
||||
int op_success = SH2_ERR;
|
||||
|
||||
lock_sh2_HAL();
|
||||
|
|
@ -1270,6 +1274,7 @@ bool BNO08x::calibration_start(uint32_t period_us)
|
|||
|
||||
return (op_success == SH2_OK);
|
||||
}
|
||||
*/
|
||||
|
||||
/**
|
||||
* @brief Ends turn-table calibration, see ref. manual 6.4.10.2
|
||||
|
|
@ -1278,7 +1283,7 @@ bool BNO08x::calibration_start(uint32_t period_us)
|
|||
*
|
||||
* @return True if enable start turn-table calibration operation succeeded.
|
||||
*/
|
||||
bool BNO08x::calibration_end(sh2_CalStatus_t& status)
|
||||
/*bool BNO08x::calibration_turntable_end(sh2_CalStatus_t& status)
|
||||
{
|
||||
int op_success = SH2_ERR;
|
||||
|
||||
|
|
@ -1288,6 +1293,7 @@ bool BNO08x::calibration_end(sh2_CalStatus_t& status)
|
|||
|
||||
return (op_success == SH2_OK);
|
||||
}
|
||||
*/
|
||||
|
||||
/**
|
||||
* @brief Enables dynamic/motion engine calibration for specified sensor(s), see ref. manual 6.4.6.1
|
||||
|
|
@ -1376,7 +1382,7 @@ bool BNO08x::dynamic_calibration_autosave_disable()
|
|||
*
|
||||
* @return True if save dynamic/ME calibration data succeeded.
|
||||
*/
|
||||
bool BNO08x::save_dynamic_calibration()
|
||||
bool BNO08x::dynamic_calibration_save()
|
||||
{
|
||||
int op_success = SH2_ERR;
|
||||
|
||||
|
|
@ -1393,7 +1399,7 @@ bool BNO08x::save_dynamic_calibration()
|
|||
*
|
||||
* @return True if save dynamic/ME calibration data succeeded.
|
||||
*/
|
||||
bool BNO08x::clear_dynamic_calibration()
|
||||
bool BNO08x::dynamic_calibration_clear()
|
||||
{
|
||||
int op_success = SH2_ERR;
|
||||
|
||||
|
|
@ -1446,6 +1452,221 @@ bool BNO08x::clear_dynamic_calibration()
|
|||
return false;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Example calibration routine using dynamic/ME calibration commands.
|
||||
*
|
||||
* Routine does the following:alignas
|
||||
*
|
||||
* 1) disables all enabled reports
|
||||
* 2) sends a command to enable dynamic/motion engine calibration for all possible options (SH2_CAL_ACCEL | SH2_CAL_GYRO | SH2_CAL_MAG | SH2_CAL_PLANAR)
|
||||
* 3) enables game rotation vector reports and calibrated magnetic field reports
|
||||
* 4) moving window average for accuracy received through reports
|
||||
* 5) deems calibration accuracy threshold met when magf accuracy avg is >=2 (MED) and quat accuracy avg >=3 (HIGH) for longer than 5 seconds
|
||||
* 6) sends command to save dynamic calibration data
|
||||
* 7) disables all enabled reports
|
||||
*
|
||||
* Note the DCD commands don't have to be used this way, this is just an example,
|
||||
* but the dynamic_calibration_autosave_enable() allows calibration to be run and
|
||||
* saved constantly even while data is used for other operations.
|
||||
*
|
||||
* @return True if calibration routine succeeded.
|
||||
*/
|
||||
bool BNO08x::dynamic_calibration_run_routine()
|
||||
{
|
||||
constexpr size_t WINDOW_SZ = 50;
|
||||
constexpr int64_t STABLE_TIME_CRITERIA_US = 5000000LL; // meet accuracy criteria for 5 seconds
|
||||
|
||||
bno08x_quat_t quat_data;
|
||||
bno08x_magf_t magf_data;
|
||||
uint8_t quat_accuracy_window[WINDOW_SZ] = {0U};
|
||||
uint8_t magf_accuracy_window[WINDOW_SZ] = {0U};
|
||||
uint8_t quat_window_idx = 0U;
|
||||
uint8_t magf_window_idx = 0U;
|
||||
uint16_t quat_window_sum = 0U;
|
||||
uint16_t magf_window_sum = 0U;
|
||||
BNO08xAccuracy quat_accuracy_avg = BNO08xAccuracy::UNRELIABLE;
|
||||
BNO08xAccuracy magf_accuracy_avg = BNO08xAccuracy::UNRELIABLE;
|
||||
int64_t stable_time = 0LL;
|
||||
int64_t stable_start_time = 0ULL;
|
||||
|
||||
// clang-format off
|
||||
#ifdef CONFIG_ESP32_BNO08x_LOG_STATEMENTS
|
||||
ESP_LOGI(TAG, "dynamic_calibration_run_routine(): disabling all currently enabled reports and starting calibration...");
|
||||
#endif
|
||||
// clang-format on
|
||||
|
||||
if (!disable_all_reports())
|
||||
{
|
||||
// clang-format off
|
||||
#ifdef CONFIG_ESP32_BNO08x_LOG_STATEMENTS
|
||||
ESP_LOGE(TAG, "dynamic_calibration_run_routine(): failed to disable all reports before calibration");
|
||||
#endif
|
||||
// clang-format on
|
||||
|
||||
return false;
|
||||
}
|
||||
|
||||
if (!dynamic_calibration_enable(BNO08xCalSel::all))
|
||||
{
|
||||
// clang-format off
|
||||
#ifdef CONFIG_ESP32_BNO08x_LOG_STATEMENTS
|
||||
ESP_LOGE(TAG, "dynamic_calibration_run_routine(): failed to enable dynamic calibration");
|
||||
#endif
|
||||
// clang-format on
|
||||
|
||||
return false;
|
||||
}
|
||||
|
||||
if (!rpt.rv_game.enable(100000UL))
|
||||
{
|
||||
// clang-format off
|
||||
#ifdef CONFIG_ESP32_BNO08x_LOG_STATEMENTS
|
||||
ESP_LOGE(TAG, "dynamic_calibration_run_routine(): failed to enable game rotation vector report");
|
||||
#endif
|
||||
// clang-format on
|
||||
|
||||
return false;
|
||||
}
|
||||
|
||||
if (!rpt.cal_magnetometer.enable(100000UL))
|
||||
{
|
||||
// clang-format off
|
||||
#ifdef CONFIG_ESP32_BNO08x_LOG_STATEMENTS
|
||||
ESP_LOGE(TAG, "dynamic_calibration_run_routine(): failed to enable calibrated magnetometer report");
|
||||
#endif
|
||||
// clang-format on
|
||||
|
||||
return false;
|
||||
}
|
||||
|
||||
while (1)
|
||||
{
|
||||
if (data_available())
|
||||
{
|
||||
if (rpt.rv_game.has_new_data())
|
||||
{
|
||||
quat_data = rpt.rv_game.get_quat();
|
||||
|
||||
quat_accuracy_window[quat_window_idx] = static_cast<uint8_t>(quat_data.accuracy);
|
||||
quat_window_idx++;
|
||||
|
||||
if (quat_window_idx >= WINDOW_SZ)
|
||||
quat_window_idx = 0U;
|
||||
|
||||
// clang-format off
|
||||
#ifdef CONFIG_ESP32_BNO08x_LOG_STATEMENTS
|
||||
ESP_LOGI(TAG, "dynamic_calibration_run_routine(): quat_data: accuracy: %d i: %.3f j: %.3f k: %.3f real: %.3f",
|
||||
static_cast<uint8_t>(quat_data.accuracy), quat_data.i, quat_data.j, quat_data.k, quat_data.real);
|
||||
#endif
|
||||
// clang-format on
|
||||
}
|
||||
|
||||
if (rpt.cal_magnetometer.has_new_data())
|
||||
{
|
||||
magf_data = rpt.cal_magnetometer.get();
|
||||
|
||||
magf_accuracy_window[magf_window_idx] = static_cast<uint8_t>(magf_data.accuracy);
|
||||
magf_window_idx++;
|
||||
|
||||
if (magf_window_idx >= WINDOW_SZ)
|
||||
magf_window_idx = 0U;
|
||||
|
||||
// clang-format off
|
||||
#ifdef CONFIG_ESP32_BNO08x_LOG_STATEMENTS
|
||||
ESP_LOGI(TAG, "dynamic_calibration_run_routine(): magf_data: accuracy: %d x: %.3f y: %.3f z: %.3f ",
|
||||
static_cast<uint8_t>(magf_data.accuracy), magf_data.x, magf_data.y, magf_data.z);
|
||||
#endif
|
||||
// clang-format on
|
||||
}
|
||||
}
|
||||
|
||||
quat_window_sum = 0U;
|
||||
magf_window_sum = 0U;
|
||||
|
||||
// sum windows and take average
|
||||
for (int i = 0U; i < WINDOW_SZ; i++)
|
||||
{
|
||||
quat_window_sum += quat_accuracy_window[i];
|
||||
magf_window_sum += magf_accuracy_window[i];
|
||||
}
|
||||
|
||||
quat_accuracy_avg = static_cast<BNO08xAccuracy>(quat_window_sum / WINDOW_SZ);
|
||||
magf_accuracy_avg = static_cast<BNO08xAccuracy>(magf_window_sum / WINDOW_SZ);
|
||||
|
||||
// clang-format off
|
||||
#ifdef CONFIG_ESP32_BNO08x_LOG_STATEMENTS
|
||||
ESP_LOGI(TAG, "dynamic_calibration_run_routine(): magf_avg_acc: %s quat_avg_acc: %s", accuracy_to_str(magf_accuracy_avg), accuracy_to_str(quat_accuracy_avg));
|
||||
#endif
|
||||
// clang-format on
|
||||
|
||||
if ((quat_accuracy_avg >= BNO08xAccuracy::HIGH) && (magf_accuracy_avg >= BNO08xAccuracy::MED))
|
||||
{
|
||||
// start timer if accuracy criteria is met
|
||||
if (stable_start_time == 0LL)
|
||||
stable_start_time = esp_timer_get_time();
|
||||
|
||||
// calculate time for which accuracy criteria has been met
|
||||
stable_time = esp_timer_get_time() - stable_start_time;
|
||||
}
|
||||
else
|
||||
{
|
||||
// reset timer if accuracy criteria is not met
|
||||
stable_time = 0LL;
|
||||
}
|
||||
|
||||
// check if average accuracy has been stable for required time
|
||||
if (stable_time >= STABLE_TIME_CRITERIA_US)
|
||||
break;
|
||||
}
|
||||
|
||||
// clang-format off
|
||||
#ifdef CONFIG_ESP32_BNO08x_LOG_STATEMENTS
|
||||
ESP_LOGI(TAG, "dynamic_calibration_run_routine(): calibration accuracy threshold reached, sending command to save calibration data...");
|
||||
#endif
|
||||
// clang-format on
|
||||
|
||||
if (!dynamic_calibration_save())
|
||||
{
|
||||
// clang-format off
|
||||
#ifdef CONFIG_ESP32_BNO08x_LOG_STATEMENTS
|
||||
ESP_LOGI(TAG, "dynamic_calibration_run_routine(): failed to save calibration data");
|
||||
#endif
|
||||
// clang-format on
|
||||
|
||||
return false;
|
||||
}
|
||||
|
||||
if (!dynamic_calibration_disable(BNO08xCalSel::all))
|
||||
{
|
||||
// clang-format off
|
||||
#ifdef CONFIG_ESP32_BNO08x_LOG_STATEMENTS
|
||||
ESP_LOGI(TAG, "dynamic_calibration_run_routine(): failed to disable calibration");
|
||||
#endif
|
||||
// clang-format on
|
||||
|
||||
return false;
|
||||
}
|
||||
|
||||
if (!disable_all_reports())
|
||||
{
|
||||
// clang-format off
|
||||
#ifdef CONFIG_ESP32_BNO08x_LOG_STATEMENTS
|
||||
ESP_LOGE(TAG, "dynamic_calibration_run_routine(): failed to disable all reports after calibration");
|
||||
#endif
|
||||
// clang-format on
|
||||
|
||||
return false;
|
||||
}
|
||||
|
||||
// clang-format off
|
||||
#ifdef CONFIG_ESP32_BNO08x_LOG_STATEMENTS
|
||||
ESP_LOGI(TAG, "dynamic_calibration_run_routine(): calibration success");
|
||||
#endif
|
||||
// clang-format on
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Retrieves a record from flash record system (if your goal is to retrieve meta data use the
|
||||
* BNO08xRpt:get_meta_data() method instead)
|
||||
|
|
|
|||
|
|
@ -103,7 +103,11 @@ int BNO08xSH2HAL::spi_write(sh2_Hal_t* self, uint8_t* pBuffer, unsigned len)
|
|||
imu->spi_transaction.flags = 0;
|
||||
|
||||
gpio_set_level(imu->imu_config.io_cs, 0); // assert chip select
|
||||
spi_device_polling_transmit(imu->spi_hdl, &imu->spi_transaction); // send data packet
|
||||
|
||||
// send data packet
|
||||
if(spi_device_polling_transmit(imu->spi_hdl, &imu->spi_transaction) != ESP_OK)
|
||||
return 0;
|
||||
|
||||
gpio_set_level(imu->imu_config.io_cs, 1); // de-assert chip select
|
||||
|
||||
return len;
|
||||
|
|
|
|||
|
|
@ -320,7 +320,7 @@ TEST_CASE("Save Dynamic Calibration", "[FeatureTests]")
|
|||
|
||||
imu = BNO08xTestHelper::get_test_imu();
|
||||
|
||||
TEST_ASSERT_EQUAL(true, imu->save_dynamic_calibration());
|
||||
TEST_ASSERT_EQUAL(true, imu->dynamic_calibration_save());
|
||||
|
||||
for (int i = 0; i < RX_REPORT_TRIAL_CNT; i++)
|
||||
{
|
||||
|
|
@ -409,7 +409,7 @@ TEST_CASE("Clear Dynamic Calibration", "[FeatureTests]")
|
|||
|
||||
imu = BNO08xTestHelper::get_test_imu();
|
||||
|
||||
TEST_ASSERT_EQUAL(true, imu->clear_dynamic_calibration());
|
||||
TEST_ASSERT_EQUAL(true, imu->dynamic_calibration_clear());
|
||||
|
||||
for (int i = 0; i < RX_REPORT_TRIAL_CNT; i++)
|
||||
{
|
||||
|
|
|
|||
Loading…
Reference in New Issue