diff --git a/lib/pbio/drv/imu/imu_lsm6ds3tr_c_stm32.c b/lib/pbio/drv/imu/imu_lsm6ds3tr_c_stm32.c index 846626d87..9bce43365 100644 --- a/lib/pbio/drv/imu/imu_lsm6ds3tr_c_stm32.c +++ b/lib/pbio/drv/imu/imu_lsm6ds3tr_c_stm32.c @@ -51,6 +51,10 @@ struct _pbdrv_imu_dev_t { int16_t data[6]; /** Raw data point to which new samples are compared to detect stationary. */ int16_t stationary_data_start[6]; + /** Raw gyro value below which the hub is considered stationary, smaller data is considered noise. */ + int16_t gyro_noise_threshold; + /** Raw accl value below which the hub is considered stationary, smaller data is considered noise. */ + int16_t accl_noise_threshold; /** Sum of gyro samples during the stationary period. */ int32_t stationary_gyro_data_sum[3]; /** Sum of accelerometer samples during the stationary period. */ @@ -211,6 +215,12 @@ static PT_THREAD(pbdrv_imu_lsm6ds3tr_c_stm32_init(struct pt *pt)) { PT_SPAWN(pt, &child, lsm6ds3tr_c_gy_full_scale_set(&child, ctx, LSM6DS3TR_C_2000dps)); imu_dev->config.gyro_scale = lsm6ds3tr_c_from_fs2000dps_to_mdps(1) / 1000.0f; + /* + * Set noise thresholds + */ + imu_dev->gyro_noise_threshold = 20; + imu_dev->accl_noise_threshold = 100; + // Configure INT1 to trigger when new gyro data is ready. PT_SPAWN(pt, &child, lsm6ds3tr_c_pin_int1_route_set(&child, ctx, (lsm6ds3tr_c_int1_route_t) { .int1_drdy_g = 1, @@ -232,17 +242,6 @@ static PT_THREAD(pbdrv_imu_lsm6ds3tr_c_stm32_init(struct pt *pt)) { PT_END(pt); } -// REVISIT: These values should be selected based on expected noise -// characteristics given the selected parameters. For now, use a setter so we -// can experiment with finding the right values. -static int16_t pbdrv_imu_lsm6ds3tr_c_gyro_noise = 20; -static int16_t pbdrv_imu_lsm6ds3tr_c_accl_noise = 100; - -void pbdrv_imu_lsm6ds3tr_c_stm32_set_noise_thresholds(int16_t gyro_noise, int16_t accl_noise) { - pbdrv_imu_lsm6ds3tr_c_gyro_noise = gyro_noise; - pbdrv_imu_lsm6ds3tr_c_accl_noise = accl_noise; -} - static inline bool bounded(int16_t diff, int16_t threshold) { return diff < threshold && diff > -threshold; } @@ -250,12 +249,12 @@ static inline bool bounded(int16_t diff, int16_t threshold) { static void pbdrv_imu_lsm6ds3tr_c_stm32_update_stationary_status(pbdrv_imu_dev_t *imu_dev) { // Check whether still stationary compared to constant start sample. - if (bounded(imu_dev->data[0] - imu_dev->stationary_data_start[0], pbdrv_imu_lsm6ds3tr_c_gyro_noise) && - bounded(imu_dev->data[1] - imu_dev->stationary_data_start[1], pbdrv_imu_lsm6ds3tr_c_gyro_noise) && - bounded(imu_dev->data[2] - imu_dev->stationary_data_start[2], pbdrv_imu_lsm6ds3tr_c_gyro_noise) && - bounded(imu_dev->data[3] - imu_dev->stationary_data_start[3], pbdrv_imu_lsm6ds3tr_c_accl_noise) && - bounded(imu_dev->data[4] - imu_dev->stationary_data_start[4], pbdrv_imu_lsm6ds3tr_c_accl_noise) && - bounded(imu_dev->data[5] - imu_dev->stationary_data_start[5], pbdrv_imu_lsm6ds3tr_c_accl_noise) + if (bounded(imu_dev->data[0] - imu_dev->stationary_data_start[0], imu_dev->gyro_noise_threshold) && + bounded(imu_dev->data[1] - imu_dev->stationary_data_start[1], imu_dev->gyro_noise_threshold) && + bounded(imu_dev->data[2] - imu_dev->stationary_data_start[2], imu_dev->gyro_noise_threshold) && + bounded(imu_dev->data[3] - imu_dev->stationary_data_start[3], imu_dev->accl_noise_threshold) && + bounded(imu_dev->data[4] - imu_dev->stationary_data_start[4], imu_dev->accl_noise_threshold) && + bounded(imu_dev->data[5] - imu_dev->stationary_data_start[5], imu_dev->accl_noise_threshold) ) { // Still not moved, so increment stationary sample counter. imu_dev->stationary_sample_count++; @@ -414,4 +413,9 @@ bool pbdrv_imu_is_stationary(pbdrv_imu_dev_t *imu_dev) { return imu_dev->stationary_now; } +void pbdrv_imu_set_stationary_thresholds(pbdrv_imu_dev_t *imu_dev, int16_t gyro_threshold, int16_t accl_threshold) { + imu_dev->gyro_noise_threshold = gyro_threshold; + imu_dev->accl_noise_threshold = accl_threshold; +} + #endif // PBDRV_CONFIG_IMU_LSM6S3TR_C_STM32 diff --git a/lib/pbio/include/pbdrv/imu.h b/lib/pbio/include/pbdrv/imu.h index 2d23411b2..c51440fe3 100644 --- a/lib/pbio/include/pbdrv/imu.h +++ b/lib/pbio/include/pbdrv/imu.h @@ -52,6 +52,16 @@ pbio_error_t pbdrv_imu_get_imu(pbdrv_imu_dev_t **imu_dev, pbdrv_imu_config_t **c */ bool pbdrv_imu_is_stationary(pbdrv_imu_dev_t *imu_dev); +/** + * Sets the thresholds that define when the hub is stationary. Below these + * levels, the sensor will automatically (re-)calibrate. + * + * @param [in] imu_dev The IMU device instance. + * @param [in] gyro_threshold Angular velocity threshold in raw units. + * @param [in] accl_threshold Acceleration threshold in raw units. + */ +void pbdrv_imu_set_stationary_thresholds(pbdrv_imu_dev_t *imu_dev, int16_t gyro_threshold, int16_t accl_threshold); + /** * Callback to process one frame of unfiltered gyro and accelerometer data. * diff --git a/lib/pbio/include/pbio/orientation.h b/lib/pbio/include/pbio/orientation.h index 20b3e6987..b32576976 100644 --- a/lib/pbio/include/pbio/orientation.h +++ b/lib/pbio/include/pbio/orientation.h @@ -56,6 +56,8 @@ pbio_error_t pbio_orientation_set_base_orientation(pbio_geometry_xyz_t *x_axis, bool pbio_orientation_imu_is_stationary(void); +void pbio_orientation_imu_set_stationary_thresholds(float angular_velocity, float acceleration); + void pbio_orientation_imu_get_angular_velocity(pbio_geometry_xyz_t *values); void pbio_orientation_imu_get_acceleration(pbio_geometry_xyz_t *values); @@ -79,6 +81,9 @@ static inline bool pbio_orientation_imu_is_stationary(void) { return false; } +static inline void pbio_orientation_imu_set_stationary_thresholds(float angular_velocity, float acceleration) { +} + static inline void pbio_orientation_imu_get_angular_velocity(pbio_geometry_xyz_t *values) { } diff --git a/lib/pbio/src/orientation.c b/lib/pbio/src/orientation.c index b68dc47e2..ce0a36e76 100644 --- a/lib/pbio/src/orientation.c +++ b/lib/pbio/src/orientation.c @@ -10,6 +10,7 @@ #include #include +#include #include #include #include @@ -209,6 +210,20 @@ bool pbio_orientation_imu_is_stationary(void) { return pbdrv_imu_is_stationary(imu_dev); } +/** + * Sets the thresholds that define when the hub is stationary. When the + * measurements are steadily below these levels, the orientation module + * automatically recalibrates. + * + * @param [in] angular_velocity Angular velocity threshold in deg/s. + * @param [in] acceleration Acceleration threshold in mm/s^2 + */ +void pbio_orientation_imu_set_stationary_thresholds(float angular_velocity, float acceleration) { + int16_t gyro_threshold = pbio_int_math_bind(angular_velocity / imu_config->gyro_scale, 1, INT16_MAX); + int16_t accl_threshold = pbio_int_math_bind(acceleration / imu_config->accel_scale, 1, INT16_MAX); + pbdrv_imu_set_stationary_thresholds(imu_dev, gyro_threshold, accl_threshold); +} + /** * Reads the current IMU angular velocity in deg/s, compensated for offset. * diff --git a/pybricks/common/pb_type_imu.c b/pybricks/common/pb_type_imu.c index 412a3ba1f..5e0cfed89 100644 --- a/pybricks/common/pb_type_imu.c +++ b/pybricks/common/pb_type_imu.c @@ -128,6 +128,19 @@ STATIC mp_obj_t common_IMU_stationary(mp_obj_t self_in) { } MP_DEFINE_CONST_FUN_OBJ_1(common_IMU_stationary_obj, common_IMU_stationary); +// pybricks._common.IMU.set_stationary_thresholds +STATIC mp_obj_t common_IMU_set_stationary_thresholds(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args) { + PB_PARSE_ARGS_METHOD(n_args, pos_args, kw_args, + common_IMU_obj_t, self, + PB_ARG_REQUIRED(angular_velocity), + PB_ARG_REQUIRED(acceleration)); + + (void)self; + pbio_orientation_imu_set_stationary_thresholds(mp_obj_get_float(angular_velocity_in), mp_obj_get_float(acceleration_in)); + return mp_const_none; +} +STATIC MP_DEFINE_CONST_FUN_OBJ_KW(common_IMU_set_stationary_thresholds_obj, 1, common_IMU_set_stationary_thresholds); + // pybricks._common.IMU.heading STATIC mp_obj_t common_IMU_heading(mp_obj_t self_in) { (void)self_in; @@ -155,6 +168,7 @@ STATIC const mp_rom_map_elem_t common_IMU_locals_dict_table[] = { { MP_ROM_QSTR(MP_QSTR_acceleration), MP_ROM_PTR(&common_IMU_acceleration_obj) }, { MP_ROM_QSTR(MP_QSTR_angular_velocity), MP_ROM_PTR(&common_IMU_angular_velocity_obj)}, { MP_ROM_QSTR(MP_QSTR_stationary), MP_ROM_PTR(&common_IMU_stationary_obj) }, + { MP_ROM_QSTR(MP_QSTR_set_stationary_thresholds), MP_ROM_PTR(&common_IMU_set_stationary_thresholds_obj) }, { MP_ROM_QSTR(MP_QSTR_heading), MP_ROM_PTR(&common_IMU_heading_obj) }, { MP_ROM_QSTR(MP_QSTR_reset_heading), MP_ROM_PTR(&common_IMU_reset_heading_obj) }, }; @@ -183,6 +197,9 @@ mp_obj_t pb_type_IMU_obj_new(mp_obj_t top_side_axis_in, mp_obj_t front_side_axis pbio_orientation_set_base_orientation(&front_side_axis, &top_side_axis); + // Default noise thresholds. + pbio_orientation_imu_set_stationary_thresholds(1.5f, 250.0f); + // Return singleton instance. return MP_OBJ_FROM_PTR(&singleton_imu_obj); }