refactor FlyteSailing PR for internal wrappers, move enum helper fxns
Signed-off-by: myles-parfeniuk <myles.parfenyuk@gmail.com>
This commit is contained in:
myles-parfeniuk
parent
a136b8f7c7
commit
ec3b078468
|
|
@ -52,11 +52,10 @@ class BNO08x
|
|||
bool dynamic_calibration_autosave_enable();
|
||||
bool dynamic_calibration_autosave_disable();
|
||||
bool dynamic_calibration_save();
|
||||
bool dynamic_calibration_clear();
|
||||
bool dynamic_calibration_clear_data_ram();
|
||||
bool dynamic_calibration_delete_data();
|
||||
bool dynamic_calibration_run_routine();
|
||||
|
||||
bool delete_calibration_data();
|
||||
|
||||
constexpr static float SQRT2OVER2 = 0.7071067811865476f; // sqrt(2)/2, used for setting system orientation
|
||||
bool set_system_orientation(float w, float x, float y, float z);
|
||||
bool get_system_orientation(float &w, float &x, float &y, float &z);
|
||||
|
|
@ -72,11 +71,6 @@ class BNO08x
|
|||
void print_product_ids();
|
||||
void print_system_orientation();
|
||||
|
||||
// enum helper fxns
|
||||
static const char* activity_to_str(BNO08xActivity activity);
|
||||
static const char* stability_to_str(BNO08xStability stability);
|
||||
static const char* accuracy_to_str(BNO08xAccuracy accuracy);
|
||||
|
||||
/// @brief Contains report implementations.
|
||||
typedef struct bno08x_reports_t
|
||||
{
|
||||
|
|
|
|||
|
|
@ -112,6 +112,25 @@ enum class BNO08xAccuracy : uint8_t
|
|||
};
|
||||
using IMUAccuracy = BNO08xAccuracy; // legacy version compatibility
|
||||
|
||||
const constexpr char* BNO08xAccuracy_to_str(BNO08xAccuracy accuracy)
|
||||
{
|
||||
switch (accuracy)
|
||||
{
|
||||
case BNO08xAccuracy::UNRELIABLE:
|
||||
return "UNRELIABLE";
|
||||
case BNO08xAccuracy::LOW:
|
||||
return "LOW";
|
||||
case BNO08xAccuracy::MED:
|
||||
return "MED";
|
||||
case BNO08xAccuracy::HIGH:
|
||||
return "HIGH";
|
||||
case BNO08xAccuracy::UNDEFINED:
|
||||
return "UNDEFINED";
|
||||
default:
|
||||
return "UNDEFINED";
|
||||
}
|
||||
}
|
||||
|
||||
/// @brief BNO08xActivity Classifier enable bits passed to enable_activity_classifier()
|
||||
/// See ref manual 6.5.36.1
|
||||
enum class BNO08xActivityEnable : uint32_t
|
||||
|
|
@ -125,7 +144,7 @@ enum class BNO08xActivityEnable : uint32_t
|
|||
WALKING = (1U << 6U),
|
||||
RUNNING = (1U << 7U),
|
||||
ON_STAIRS = (1U << 8U),
|
||||
ALL = ( UNKNOWN | IN_VEHICLE | ON_BICYCLE | ON_FOOT | STILL | TILTING | WALKING | RUNNING | ON_STAIRS)
|
||||
ALL = (UNKNOWN | IN_VEHICLE | ON_BICYCLE | ON_FOOT | STILL | TILTING | WALKING | RUNNING | ON_STAIRS)
|
||||
};
|
||||
|
||||
/// @brief BNO08xActivity states returned from BNO08x::activity_classifier.get()
|
||||
|
|
@ -143,6 +162,40 @@ enum class BNO08xActivity : uint8_t
|
|||
UNDEFINED = 9 // used for unit tests
|
||||
};
|
||||
|
||||
/**
|
||||
* @brief Converts a BNO08xActivity enum to string.
|
||||
*
|
||||
* @return The resulting string conversion of the enum.
|
||||
*/
|
||||
const constexpr char* BNO08xActivity_to_str(BNO08xActivity activity)
|
||||
{
|
||||
switch (activity)
|
||||
{
|
||||
case BNO08xActivity::UNKNOWN:
|
||||
return "UNKNOWN";
|
||||
case BNO08xActivity::IN_VEHICLE:
|
||||
return "IN_VEHICLE";
|
||||
case BNO08xActivity::ON_BICYCLE:
|
||||
return "ON_BICYCLE";
|
||||
case BNO08xActivity::ON_FOOT:
|
||||
return "ON_FOOT";
|
||||
case BNO08xActivity::STILL:
|
||||
return "STILL";
|
||||
case BNO08xActivity::TILTING:
|
||||
return "TILTING";
|
||||
case BNO08xActivity::WALKING:
|
||||
return "WALKING";
|
||||
case BNO08xActivity::RUNNING:
|
||||
return "RUNNING";
|
||||
case BNO08xActivity::ON_STAIRS:
|
||||
return "ON_STAIRS";
|
||||
case BNO08xActivity::UNDEFINED:
|
||||
return "UNDEFINED";
|
||||
default:
|
||||
return "UNDEFINED";
|
||||
}
|
||||
}
|
||||
|
||||
/// @brief BNO08xStability states returned from BNO08x::stability_classifier.get()
|
||||
enum class BNO08xStability : uint8_t
|
||||
{
|
||||
|
|
@ -155,7 +208,36 @@ enum class BNO08xStability : uint8_t
|
|||
UNDEFINED = 6 // used for unit tests
|
||||
};
|
||||
|
||||
enum class BNO08xFrsID : uint16_t {
|
||||
/**
|
||||
* @brief Converts a BNO08xStability enum to string.
|
||||
*
|
||||
* @return The resulting string conversion of the enum.
|
||||
*/
|
||||
const constexpr char* BNO08xStability_to_str(BNO08xStability stability)
|
||||
{
|
||||
switch (stability)
|
||||
{
|
||||
case BNO08xStability::UNKNOWN:
|
||||
return "UNKNOWN";
|
||||
case BNO08xStability::ON_TABLE:
|
||||
return "ON_TABLE";
|
||||
case BNO08xStability::STATIONARY:
|
||||
return "STATIONARY";
|
||||
case BNO08xStability::STABLE:
|
||||
return "STABLE";
|
||||
case BNO08xStability::MOTION:
|
||||
return "MOTION";
|
||||
case BNO08xStability::RESERVED:
|
||||
return "RESERVED";
|
||||
case BNO08xStability::UNDEFINED:
|
||||
return "UNDEFINED";
|
||||
default:
|
||||
return "UNDEFINED";
|
||||
}
|
||||
}
|
||||
|
||||
enum class BNO08xFrsID : uint16_t
|
||||
{
|
||||
STATIC_CALIBRATION_AGM = 0x7979,
|
||||
NOMINAL_CALIBRATION = 0x4D4D,
|
||||
STATIC_CALIBRATION_SRA = 0x8A8A,
|
||||
|
|
@ -231,6 +313,161 @@ enum class BNO08xFrsID : uint16_t {
|
|||
META_GYRO_INTEGRATED_RV = 0xE324
|
||||
};
|
||||
|
||||
const constexpr char* BNO08xFrsID_to_str(BNO08xFrsID id)
|
||||
{
|
||||
switch (id)
|
||||
{
|
||||
case BNO08xFrsID::STATIC_CALIBRATION_AGM:
|
||||
return "STATIC_CALIBRATION_AGM";
|
||||
case BNO08xFrsID::NOMINAL_CALIBRATION:
|
||||
return "NOMINAL_CALIBRATION";
|
||||
case BNO08xFrsID::STATIC_CALIBRATION_SRA:
|
||||
return "STATIC_CALIBRATION_SRA";
|
||||
case BNO08xFrsID::NOMINAL_CALIBRATION_SRA:
|
||||
return "NOMINAL_CALIBRATION_SRA";
|
||||
case BNO08xFrsID::DYNAMIC_CALIBRATION:
|
||||
return "DYNAMIC_CALIBRATION";
|
||||
case BNO08xFrsID::ME_POWER_MGMT:
|
||||
return "ME_POWER_MGMT";
|
||||
case BNO08xFrsID::SYSTEM_ORIENTATION:
|
||||
return "SYSTEM_ORIENTATION";
|
||||
case BNO08xFrsID::ACCEL_ORIENTATION:
|
||||
return "ACCEL_ORIENTATION";
|
||||
case BNO08xFrsID::SCREEN_ACCEL_ORIENTATION:
|
||||
return "SCREEN_ACCEL_ORIENTATION";
|
||||
case BNO08xFrsID::GYROSCOPE_ORIENTATION:
|
||||
return "GYROSCOPE_ORIENTATION";
|
||||
case BNO08xFrsID::MAGNETOMETER_ORIENTATION:
|
||||
return "MAGNETOMETER_ORIENTATION";
|
||||
case BNO08xFrsID::ARVR_STABILIZATION_RV:
|
||||
return "ARVR_STABILIZATION_RV";
|
||||
case BNO08xFrsID::ARVR_STABILIZATION_GRV:
|
||||
return "ARVR_STABILIZATION_GRV";
|
||||
case BNO08xFrsID::TAP_DETECT_CONFIG:
|
||||
return "TAP_DETECT_CONFIG";
|
||||
case BNO08xFrsID::SIG_MOTION_DETECT_CONFIG:
|
||||
return "SIG_MOTION_DETECT_CONFIG";
|
||||
case BNO08xFrsID::SHAKE_DETECT_CONFIG:
|
||||
return "SHAKE_DETECT_CONFIG";
|
||||
case BNO08xFrsID::MAX_FUSION_PERIOD:
|
||||
return "MAX_FUSION_PERIOD";
|
||||
case BNO08xFrsID::SERIAL_NUMBER:
|
||||
return "SERIAL_NUMBER";
|
||||
case BNO08xFrsID::ES_PRESSURE_CAL:
|
||||
return "ES_PRESSURE_CAL";
|
||||
case BNO08xFrsID::ES_TEMPERATURE_CAL:
|
||||
return "ES_TEMPERATURE_CAL";
|
||||
case BNO08xFrsID::ES_HUMIDITY_CAL:
|
||||
return "ES_HUMIDITY_CAL";
|
||||
case BNO08xFrsID::ES_AMBIENT_LIGHT_CAL:
|
||||
return "ES_AMBIENT_LIGHT_CAL";
|
||||
case BNO08xFrsID::ES_PROXIMITY_CAL:
|
||||
return "ES_PROXIMITY_CAL";
|
||||
case BNO08xFrsID::ALS_CAL:
|
||||
return "ALS_CAL";
|
||||
case BNO08xFrsID::PROXIMITY_SENSOR_CAL:
|
||||
return "PROXIMITY_SENSOR_CAL";
|
||||
case BNO08xFrsID::PICKUP_DETECTOR_CONFIG:
|
||||
return "PICKUP_DETECTOR_CONFIG";
|
||||
case BNO08xFrsID::FLIP_DETECTOR_CONFIG:
|
||||
return "FLIP_DETECTOR_CONFIG";
|
||||
case BNO08xFrsID::STABILITY_DETECTOR_CONFIG:
|
||||
return "STABILITY_DETECTOR_CONFIG";
|
||||
case BNO08xFrsID::ACTIVITY_TRACKER_CONFIG:
|
||||
return "ACTIVITY_TRACKER_CONFIG";
|
||||
case BNO08xFrsID::SLEEP_DETECTOR_CONFIG:
|
||||
return "SLEEP_DETECTOR_CONFIG";
|
||||
case BNO08xFrsID::TILT_DETECTOR_CONFIG:
|
||||
return "TILT_DETECTOR_CONFIG";
|
||||
case BNO08xFrsID::POCKET_DETECTOR_CONFIG:
|
||||
return "POCKET_DETECTOR_CONFIG";
|
||||
case BNO08xFrsID::CIRCLE_DETECTOR_CONFIG:
|
||||
return "CIRCLE_DETECTOR_CONFIG";
|
||||
case BNO08xFrsID::USER_RECORD:
|
||||
return "USER_RECORD";
|
||||
case BNO08xFrsID::ME_TIME_SOURCE_SELECT:
|
||||
return "ME_TIME_SOURCE_SELECT";
|
||||
case BNO08xFrsID::UART_FORMAT:
|
||||
return "UART_FORMAT";
|
||||
case BNO08xFrsID::GYRO_INTEGRATED_RV_CONFIG:
|
||||
return "GYRO_INTEGRATED_RV_CONFIG";
|
||||
case BNO08xFrsID::META_RAW_ACCELEROMETER:
|
||||
return "META_RAW_ACCELEROMETER";
|
||||
case BNO08xFrsID::META_ACCELEROMETER:
|
||||
return "META_ACCELEROMETER";
|
||||
case BNO08xFrsID::META_LINEAR_ACCELERATION:
|
||||
return "META_LINEAR_ACCELERATION";
|
||||
case BNO08xFrsID::META_GRAVITY:
|
||||
return "META_GRAVITY";
|
||||
case BNO08xFrsID::META_RAW_GYROSCOPE:
|
||||
return "META_RAW_GYROSCOPE";
|
||||
case BNO08xFrsID::META_GYROSCOPE_CALIBRATED:
|
||||
return "META_GYROSCOPE_CALIBRATED";
|
||||
case BNO08xFrsID::META_GYROSCOPE_UNCALIBRATED:
|
||||
return "META_GYROSCOPE_UNCALIBRATED";
|
||||
case BNO08xFrsID::META_RAW_MAGNETOMETER:
|
||||
return "META_RAW_MAGNETOMETER";
|
||||
case BNO08xFrsID::META_MAGNETIC_FIELD_CALIBRATED:
|
||||
return "META_MAGNETIC_FIELD_CALIBRATED";
|
||||
case BNO08xFrsID::META_MAGNETIC_FIELD_UNCALIBRATED:
|
||||
return "META_MAGNETIC_FIELD_UNCALIBRATED";
|
||||
case BNO08xFrsID::META_ROTATION_VECTOR:
|
||||
return "META_ROTATION_VECTOR";
|
||||
case BNO08xFrsID::META_GAME_ROTATION_VECTOR:
|
||||
return "META_GAME_ROTATION_VECTOR";
|
||||
case BNO08xFrsID::META_GEOMAGNETIC_ROTATION_VECTOR:
|
||||
return "META_GEOMAGNETIC_ROTATION_VECTOR";
|
||||
case BNO08xFrsID::META_PRESSURE:
|
||||
return "META_PRESSURE";
|
||||
case BNO08xFrsID::META_AMBIENT_LIGHT:
|
||||
return "META_AMBIENT_LIGHT";
|
||||
case BNO08xFrsID::META_HUMIDITY:
|
||||
return "META_HUMIDITY";
|
||||
case BNO08xFrsID::META_PROXIMITY:
|
||||
return "META_PROXIMITY";
|
||||
case BNO08xFrsID::META_TEMPERATURE:
|
||||
return "META_TEMPERATURE";
|
||||
case BNO08xFrsID::META_TAP_DETECTOR:
|
||||
return "META_TAP_DETECTOR";
|
||||
case BNO08xFrsID::META_STEP_DETECTOR:
|
||||
return "META_STEP_DETECTOR";
|
||||
case BNO08xFrsID::META_STEP_COUNTER:
|
||||
return "META_STEP_COUNTER";
|
||||
case BNO08xFrsID::META_SIGNIFICANT_MOTION:
|
||||
return "META_SIGNIFICANT_MOTION";
|
||||
case BNO08xFrsID::META_STABILITY_CLASSIFIER:
|
||||
return "META_STABILITY_CLASSIFIER";
|
||||
case BNO08xFrsID::META_SHAKE_DETECTOR:
|
||||
return "META_SHAKE_DETECTOR";
|
||||
case BNO08xFrsID::META_FLIP_DETECTOR:
|
||||
return "META_FLIP_DETECTOR";
|
||||
case BNO08xFrsID::META_PICKUP_DETECTOR:
|
||||
return "META_PICKUP_DETECTOR";
|
||||
case BNO08xFrsID::META_STABILITY_DETECTOR:
|
||||
return "META_STABILITY_DETECTOR";
|
||||
case BNO08xFrsID::META_PERSONAL_ACTIVITY_CLASSIFIER:
|
||||
return "META_PERSONAL_ACTIVITY_CLASSIFIER";
|
||||
case BNO08xFrsID::META_SLEEP_DETECTOR:
|
||||
return "META_SLEEP_DETECTOR";
|
||||
case BNO08xFrsID::META_TILT_DETECTOR:
|
||||
return "META_TILT_DETECTOR";
|
||||
case BNO08xFrsID::META_POCKET_DETECTOR:
|
||||
return "META_POCKET_DETECTOR";
|
||||
case BNO08xFrsID::META_CIRCLE_DETECTOR:
|
||||
return "META_CIRCLE_DETECTOR";
|
||||
case BNO08xFrsID::META_HEART_RATE_MONITOR:
|
||||
return "META_HEART_RATE_MONITOR";
|
||||
case BNO08xFrsID::META_ARVR_STABILIZED_RV:
|
||||
return "META_ARVR_STABILIZED_RV";
|
||||
case BNO08xFrsID::META_ARVR_STABILIZED_GRV:
|
||||
return "META_ARVR_STABILIZED_GRV";
|
||||
case BNO08xFrsID::META_GYRO_INTEGRATED_RV:
|
||||
return "META_GYRO_INTEGRATED_RV";
|
||||
default:
|
||||
return "UNKNOWN";
|
||||
}
|
||||
}
|
||||
|
||||
/// @brief Struct to represent unit quaternion.
|
||||
typedef struct bno08x_quat_t
|
||||
{
|
||||
|
|
|
|||
|
|
@ -1408,12 +1408,12 @@ bool BNO08x::dynamic_calibration_save()
|
|||
}
|
||||
|
||||
/**
|
||||
* @brief Clears dynamic/motion engine calibration data and resets BNO08x device. See ref
|
||||
* @brief Clears dynamic/motion engine calibration data from ram and resets BNO08x device. See ref
|
||||
* manual 6.4.9.1
|
||||
*
|
||||
* @return True if save dynamic/ME calibration data succeeded.
|
||||
*/
|
||||
bool BNO08x::dynamic_calibration_clear()
|
||||
bool BNO08x::dynamic_calibration_clear_data_ram()
|
||||
{
|
||||
int op_success = SH2_ERR;
|
||||
|
||||
|
|
@ -1440,7 +1440,7 @@ bool BNO08x::dynamic_calibration_clear()
|
|||
{
|
||||
// clang-format off
|
||||
#ifdef CONFIG_ESP32_BNO08x_LOG_STATEMENTS
|
||||
ESP_LOGE(TAG, "Clear dynamic calibration failure, incorrect reset reason returned.");
|
||||
ESP_LOGE(TAG, "dynamic_calibration_clear_data_ram(): Clear dynamic calibration failure, incorrect reset reason returned.");
|
||||
#endif
|
||||
// clang-format on
|
||||
}
|
||||
|
|
@ -1449,7 +1449,7 @@ bool BNO08x::dynamic_calibration_clear()
|
|||
{
|
||||
// clang-format off
|
||||
#ifdef CONFIG_ESP32_BNO08x_LOG_STATEMENTS
|
||||
ESP_LOGE(TAG, "Clear dynamic calibration failure, reset never detected after sending command.");
|
||||
ESP_LOGE(TAG, "dynamic_calibration_clear_data_ram(): Clear dynamic calibration failure, reset never detected after sending command.");
|
||||
#endif
|
||||
// clang-format on
|
||||
}
|
||||
|
|
@ -1458,7 +1458,7 @@ bool BNO08x::dynamic_calibration_clear()
|
|||
{
|
||||
// clang-format off
|
||||
#ifdef CONFIG_ESP32_BNO08x_LOG_STATEMENTS
|
||||
ESP_LOGE(TAG, "Clear dynamic calibration failure, failed to send reset command");
|
||||
ESP_LOGE(TAG, "dynamic_calibration_clear_data_ram(): Clear dynamic calibration failure, failed to clearDcdAndReset command with %li", static_cast<int32_t>(op_success));
|
||||
#endif
|
||||
// clang-format on
|
||||
}
|
||||
|
|
@ -1609,7 +1609,7 @@ bool BNO08x::dynamic_calibration_run_routine()
|
|||
|
||||
// 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));
|
||||
ESP_LOGI(TAG, "dynamic_calibration_run_routine(): magf_avg_acc: %s quat_avg_acc: %s", BNO08xAccuracy_to_str(magf_accuracy_avg), BNO08xAccuracy_to_str(quat_accuracy_avg));
|
||||
#endif
|
||||
// clang-format on
|
||||
|
||||
|
|
@ -1686,7 +1686,7 @@ bool BNO08x::dynamic_calibration_run_routine()
|
|||
* Follows the steps outlined in ref. manual 6.4.9
|
||||
* @return True if delete dynamic calibration data operation succeeded.
|
||||
*/
|
||||
bool BNO08x::delete_calibration_data()
|
||||
bool BNO08x::dynamic_calibration_delete_data()
|
||||
{
|
||||
// 1. Reset hub (using hard_reset)
|
||||
if (!hard_reset()) {
|
||||
|
|
@ -1697,28 +1697,13 @@ bool BNO08x::delete_calibration_data()
|
|||
}
|
||||
|
||||
// 2. Delete flash copy of DCD via FRS
|
||||
int op_success = SH2_ERR;
|
||||
lock_sh2_HAL();
|
||||
// Deleting FRS record: use sh2_setFrs with nullptr and 0 words
|
||||
op_success = sh2_setFrs(DYNAMIC_CALIBRATION, nullptr, 0U);
|
||||
unlock_sh2_HAL();
|
||||
if (op_success != SH2_OK) {
|
||||
#ifdef CONFIG_ESP32_BNO08x_LOG_STATEMENTS
|
||||
ESP_LOGE(TAG, "delete_calibration_data(): failed to delete DCD FRS record, op_success: %li", (int32_t)op_success);
|
||||
#endif
|
||||
if(!write_frs(BNO08xFrsID::DYNAMIC_CALIBRATION, nullptr, 0U))
|
||||
return false;
|
||||
}
|
||||
|
||||
// 3. Issue Clear DCD and Reset Command (atomic clear DCD from RAM and reset)
|
||||
lock_sh2_HAL();
|
||||
op_success = sh2_clearDcdAndReset();
|
||||
unlock_sh2_HAL();
|
||||
if (op_success != SH2_OK) {
|
||||
#ifdef CONFIG_ESP32_BNO08x_LOG_STATEMENTS
|
||||
ESP_LOGE(TAG, "delete_calibration_data(): failed to clear DCD and reset, op_success: %li", (int32_t)op_success);
|
||||
#endif
|
||||
if(!dynamic_calibration_clear_data_ram())
|
||||
return false;
|
||||
}
|
||||
|
||||
#ifdef CONFIG_ESP32_BNO08x_LOG_STATEMENTS
|
||||
ESP_LOGI(TAG, "delete_calibration_data(): calibration data cleared successfully");
|
||||
|
|
@ -1747,6 +1732,16 @@ bool BNO08x::get_frs(BNO08xFrsID frs_ID, uint32_t (&data)[16], uint16_t& rx_data
|
|||
op_success = sh2_getFrs(static_cast<uint16_t>(frs_ID), data, &rx_data_sz);
|
||||
unlock_sh2_HAL();
|
||||
|
||||
if (op_success != SH2_OK)
|
||||
{
|
||||
// clang-format off
|
||||
#ifdef CONFIG_ESP32_BNO08x_LOG_STATEMENTS
|
||||
ESP_LOGE(TAG, "get_frs(): Failed to retrieve FRS record for ID: %s, op_success: %li", BNO08xFrsID_to_str(frs_ID), static_cast<int32_t>(op_success));
|
||||
#endif
|
||||
// clang-format off
|
||||
return false;
|
||||
}
|
||||
|
||||
return (op_success == SH2_OK);
|
||||
}
|
||||
|
||||
|
|
@ -1770,6 +1765,16 @@ bool BNO08x::write_frs(BNO08xFrsID frs_ID, uint32_t *data, const uint16_t tx_dat
|
|||
op_success = sh2_setFrs(static_cast<uint16_t>(frs_ID), data, tx_data_sz);
|
||||
unlock_sh2_HAL();
|
||||
|
||||
if (op_success != SH2_OK)
|
||||
{
|
||||
// clang-format off
|
||||
#ifdef CONFIG_ESP32_BNO08x_LOG_STATEMENTS
|
||||
ESP_LOGE(TAG, "write_frs(): Failed to write FRS record for ID: %s, op_success: %li", BNO08xFrsID_to_str(frs_ID), static_cast<int32_t>(op_success));
|
||||
#endif
|
||||
// clang-format off
|
||||
return false;
|
||||
}
|
||||
|
||||
return (op_success == SH2_OK);
|
||||
}
|
||||
|
||||
|
|
@ -2002,121 +2007,29 @@ bool BNO08x::set_system_orientation(float w, float x, float y, float z)
|
|||
static_cast<uint32_t>(float_to_q30(w)) // W component
|
||||
};
|
||||
|
||||
int op_success = SH2_ERR;
|
||||
lock_sh2_HAL();
|
||||
op_success = sh2_setFrs(SYSTEM_ORIENTATION, (uint32_t*)&orientation_raw, 4);
|
||||
unlock_sh2_HAL();
|
||||
|
||||
if (op_success != SH2_OK)
|
||||
{
|
||||
#ifdef CONFIG_ESP32_BNO08x_LOG_STATEMENTS
|
||||
ESP_LOGE(TAG, "Failed to set mounting orientation, op_success: %li", (int32_t)op_success);
|
||||
#endif
|
||||
if(!write_frs(BNO08xFrsID::SYSTEM_ORIENTATION, orientation_raw, sizeof(orientation_raw)))
|
||||
return false;
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
bool BNO08x::get_system_orientation(float& w, float& x, float& y, float& z)
|
||||
{
|
||||
uint16_t words = 4;
|
||||
uint32_t raw[4] = {0};
|
||||
|
||||
lock_sh2_HAL();
|
||||
int op_success = sh2_getFrs(SYSTEM_ORIENTATION, raw, &words);
|
||||
unlock_sh2_HAL();
|
||||
|
||||
if (op_success != SH2_OK) {
|
||||
return false;
|
||||
}
|
||||
|
||||
x = q30_to_float((int32_t)raw[0]);
|
||||
y = q30_to_float((int32_t)raw[1]);
|
||||
z = q30_to_float((int32_t)raw[2]);
|
||||
w = q30_to_float((int32_t)raw[3]);
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Converts a BNO08xActivity enum to string.
|
||||
*
|
||||
* @return The resulting string conversion of the enum.
|
||||
*/
|
||||
const char* BNO08x::activity_to_str(BNO08xActivity activity)
|
||||
bool BNO08x::get_system_orientation(float& real, float& i, float& j, float& k)
|
||||
{
|
||||
switch (activity)
|
||||
{
|
||||
case BNO08xActivity::UNKNOWN:
|
||||
return "UNKNOWN";
|
||||
case BNO08xActivity::IN_VEHICLE:
|
||||
return "IN_VEHICLE";
|
||||
case BNO08xActivity::ON_BICYCLE:
|
||||
return "ON_BICYCLE";
|
||||
case BNO08xActivity::ON_FOOT:
|
||||
return "ON_FOOT";
|
||||
case BNO08xActivity::STILL:
|
||||
return "STILL";
|
||||
case BNO08xActivity::TILTING:
|
||||
return "TILTING";
|
||||
case BNO08xActivity::WALKING:
|
||||
return "WALKING";
|
||||
case BNO08xActivity::RUNNING:
|
||||
return "RUNNING";
|
||||
case BNO08xActivity::ON_STAIRS:
|
||||
return "ON_STAIRS";
|
||||
case BNO08xActivity::UNDEFINED:
|
||||
return "UNDEFINED";
|
||||
default:
|
||||
return "UNDEFINED";
|
||||
}
|
||||
}
|
||||
uint16_t words_rxd = 0U;
|
||||
uint32_t raw[16] = {0};
|
||||
|
||||
/**
|
||||
* @brief Converts a BNO08xStability enum to string.
|
||||
*
|
||||
* @return The resulting string conversion of the enum.
|
||||
*/
|
||||
const char* BNO08x::stability_to_str(BNO08xStability stability)
|
||||
{
|
||||
switch (stability)
|
||||
{
|
||||
case BNO08xStability::UNKNOWN:
|
||||
return "UNKNOWN";
|
||||
case BNO08xStability::ON_TABLE:
|
||||
return "ON_TABLE";
|
||||
case BNO08xStability::STATIONARY:
|
||||
return "STATIONARY";
|
||||
case BNO08xStability::STABLE:
|
||||
return "STABLE";
|
||||
case BNO08xStability::MOTION:
|
||||
return "MOTION";
|
||||
case BNO08xStability::RESERVED:
|
||||
return "RESERVED";
|
||||
case BNO08xStability::UNDEFINED:
|
||||
return "UNDEFINED";
|
||||
default:
|
||||
return "UNDEFINED";
|
||||
}
|
||||
}
|
||||
if(!get_frs(BNO08xFrsID::SYSTEM_ORIENTATION, raw, words_rxd))
|
||||
return false;
|
||||
|
||||
const char* BNO08x::accuracy_to_str(BNO08xAccuracy accuracy)
|
||||
{
|
||||
switch (accuracy)
|
||||
{
|
||||
case BNO08xAccuracy::UNRELIABLE:
|
||||
return "UNRELIABLE";
|
||||
case BNO08xAccuracy::LOW:
|
||||
return "LOW";
|
||||
case BNO08xAccuracy::MED:
|
||||
return "MED";
|
||||
case BNO08xAccuracy::HIGH:
|
||||
return "HIGH";
|
||||
case BNO08xAccuracy::UNDEFINED:
|
||||
return "UNDEFINED";
|
||||
default:
|
||||
return "UNDEFINED";
|
||||
}
|
||||
if(words_rxd >= 4U)
|
||||
return false;
|
||||
|
||||
i = q30_to_float((int32_t)raw[0]);
|
||||
j = q30_to_float((int32_t)raw[1]);
|
||||
k = q30_to_float((int32_t)raw[2]);
|
||||
real = q30_to_float((int32_t)raw[3]);
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
/**
|
||||
|
|
|
|||
Loading…
Reference in New Issue