pbio/imu: Prevent numeric divergence.

If the user sets only one value, the other value was first obtained with a getter and then set again. Since both operations could introduce rounding errors, this may not be a stable operation over time.

Instead, only set a new value if it is given.

Author: laurensvalk

lib/pbio/src/imu.c

@@ -137,13 +137,19 @@ bool pbio_imu_is_stationary(void) {
  * Sets the thresholds that define when the hub is stationary. When the
  * measurements are steadily below these levels, the orientation module
  * automatically recalibrates.
+ * 
+ * If a value is negative, it is ignored.
  *
  * @param [in]  angular_velocity Angular velocity threshold in deg/s.
  * @param [in]  acceleration     Acceleration threshold in mm/s^2
  */
 void pbio_imu_set_stationary_thresholds(float angular_velocity, float acceleration) {
-    imu_config->gyro_stationary_threshold = pbio_int_math_bind(angular_velocity / imu_config->gyro_scale, 1, INT16_MAX);
-    imu_config->accel_stationary_threshold = pbio_int_math_bind(acceleration / imu_config->accel_scale, 1, INT16_MAX);
+    if (angular_velocity >= 0) {
+        imu_config->gyro_stationary_threshold = pbio_int_math_bind(angular_velocity / imu_config->gyro_scale, 1, INT16_MAX);
+    }
+    if (acceleration >= 0) {
+        imu_config->accel_stationary_threshold = pbio_int_math_bind(acceleration / imu_config->accel_scale, 1, INT16_MAX);
+    }
 }
 
 /**

pybricks/common/pb_type_imu.c

@@ -168,27 +168,25 @@ STATIC mp_obj_t pb_type_imu_settings(size_t n_args, const mp_obj_t *pos_args, mp
 
     (void)self;
 
-    float angular_velocity;
-    float acceleration;
-    pbio_imu_get_stationary_thresholds(&angular_velocity, &acceleration);
-
     // Return current values if no arguments are given.
     if (angular_velocity_threshold_in == mp_const_none && acceleration_threshold_in == mp_const_none) {
+        float angular_velocity;
+        float acceleration;
+        pbio_imu_get_stationary_thresholds(&angular_velocity, &acceleration);
         mp_obj_t ret[] = {
             mp_obj_new_float_from_f(angular_velocity),
             mp_obj_new_float_from_f(acceleration),
         };
         return mp_obj_new_tuple(MP_ARRAY_SIZE(ret), ret);
     }
 
-    // Otherwise set new values.
-    if (angular_velocity_threshold_in != mp_const_none) {
-        angular_velocity = mp_obj_get_float(angular_velocity_threshold_in);
-    }
-    if (acceleration_threshold_in != mp_const_none) {
-        acceleration = mp_obj_get_float(acceleration_threshold_in);
-    }
-    pbio_imu_set_stationary_thresholds(angular_velocity, acceleration);
+    // Otherwise set new values, only if given.
+    pbio_imu_set_stationary_thresholds(
+        angular_velocity_threshold_in == mp_const_none ? -1.0f : mp_obj_get_float(angular_velocity_threshold_in),
+        acceleration_threshold_in == mp_const_none ? -1.0f : mp_obj_get_float(acceleration_threshold_in)
+    );
+
+    // Request that changed settings are saved on shutdown.
     pbsys_storage_settings_save_imu_settings();
     return mp_const_none;
 }