pbio/orientation: Add noise threshold setter.

A similar debug function was previously dropped. However, users have requested to keep this control available in [1].

This makes a setter available in proper units, and sets defaults.

[1] pybricks/support#989

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,30 +242,19 @@ 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;
 }
 
 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

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.
  *

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) {
 }
 

lib/pbio/src/orientation.c

@@ -10,6 +10,7 @@
 
 #include <pbio/config.h>
 #include <pbio/error.h>
+#include <pbio/int_math.h>
 #include <pbio/geometry.h>
 #include <pbio/orientation.h>
 #include <pbio/util.h>
@@ -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.
  *

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);
 }