pbio/sys/storage: Refactor IMU settings to IMU module.

lib/pbio/include/pbio/imu.h

@@ -18,10 +18,30 @@
 #include <pbio/error.h>
 #include <pbio/geometry.h>
 
+/**
+ * Persistent IMU settings. All data types are little-endian.
+ */
+typedef struct {
+    /** Angular velocity threshold below which the IMU is considered stationary, in deg/s. */
+    float gyro_stationary_threshold;
+    /** Acceleration threshold below which the IMU is considered stationary, in mm/s^2. */
+    float accel_stationary_threshold;
+    /**
+     * Number of degrees measured for one full turn along the user Z axis. This
+     * is used to correct the heading value. Other rotation methods are not
+     * affected.
+     */
+    float heading_correction;
+} pbio_imu_persistent_settings_t;
+
 #if PBIO_CONFIG_IMU
 
 void pbio_imu_init(void);
 
+void pbio_imu_set_default_settings(pbio_imu_persistent_settings_t *settings);
+
+void pbio_imu_apply_loaded_settings(pbio_imu_persistent_settings_t *settings);
+
 pbio_error_t pbio_imu_set_base_orientation(pbio_geometry_xyz_t *x_axis, pbio_geometry_xyz_t *z_axis);
 
 bool pbio_imu_is_stationary(void);
@@ -30,7 +50,7 @@ bool pbio_imu_is_ready(void);
 
 void pbio_imu_get_settings(float *angular_velocity, float *acceleration, float *heading_correction);
 
-pbio_error_t pbio_imu_set_settings(float angular_velocity, float acceleration, float heading_correction);
+pbio_error_t pbio_imu_set_settings(float angular_velocity, float acceleration, float heading_correction, bool request_save);
 
 void pbio_imu_get_angular_velocity(pbio_geometry_xyz_t *values);
 
@@ -51,6 +71,12 @@ void pbio_imu_get_heading_scaled(pbio_angle_t *heading, int32_t *heading_rate, i
 static inline void pbio_imu_init(void) {
 }
 
+static inline void pbio_imu_set_default_settings(pbio_imu_persistent_settings_t *settings) {
+}
+
+static inline void pbio_imu_apply_loaded_settings(pbio_imu_persistent_settings_t *settings) {
+}
+
 static inline pbio_error_t pbio_imu_set_base_orientation(pbio_geometry_xyz_t *x_axis, pbio_geometry_xyz_t *z_axis) {
     return PBIO_ERROR_NOT_IMPLEMENTED;
 }
@@ -62,7 +88,7 @@ static inline bool pbio_imu_is_stationary(void) {
 static inline void pbio_imu_get_settings(float *angular_velocity, float *acceleration, float *heading_correction) {
 }
 
-static inline pbio_error_t pbio_imu_set_settings(float angular_velocity, float acceleration, float heading_correction) {
+static inline pbio_error_t pbio_imu_set_settings(float angular_velocity, float acceleration, float heading_correction, bool request_save) {
     return PBIO_ERROR_NOT_SUPPORTED;
 }
 

lib/pbio/include/pbsys/storage.h

@@ -25,6 +25,8 @@ pbio_error_t pbsys_storage_set_user_data(uint32_t offset, const uint8_t *data, u
 
 pbio_error_t pbsys_storage_get_user_data(uint32_t offset, uint8_t **data, uint32_t size);
 
+void pbsys_storage_request_write(void);
+
 #else
 
 static inline uint32_t pbsys_storage_get_maximum_program_size(void) {
@@ -39,6 +41,9 @@ static inline pbio_error_t pbsys_storage_get_user_data(uint32_t offset, uint8_t
     return PBIO_ERROR_NOT_SUPPORTED;
 }
 
+static inline void pbsys_storage_request_write(void) {
+}
+
 #endif // PBSYS_CONFIG_STORAGE
 
 #endif // _PBSYS_STORAGE_H_

lib/pbio/include/pbsys/storage_settings.h

@@ -18,6 +18,7 @@
 #include <pbio/error.h>
 
 #include <pbio/config.h>
+#include <pbio/imu.h>
 #include <pbsys/config.h>
 
 /**
@@ -37,16 +38,7 @@ typedef struct _pbsys_storage_settings_t {
     /** System setting flags. */
     uint32_t flags;
     #if PBIO_CONFIG_IMU
-    /** Angular velocity threshold below which the IMU is considered stationary, in deg/s. */
-    float gyro_stationary_threshold;
-    /** Acceleration threshold below which the IMU is considered stationary, in mm/s^2. */
-    float accel_stationary_threshold;
-    /**
-     * Number of degrees measured for one full turn along the user Z axis. This
-     * is used to correct the heading value. Other rotation methods are not
-     * affected.
-     */
-    float heading_correction;
+    pbio_imu_persistent_settings_t imu_settings;
     #endif
 } pbsys_storage_settings_t;
 
@@ -60,8 +52,6 @@ bool pbsys_storage_settings_bluetooth_enabled(void);
 
 void pbsys_storage_settings_bluetooth_enabled_request_toggle(void);
 
-void pbsys_storage_settings_save_imu_settings(void);
-
 #else
 
 static inline void pbsys_storage_settings_set_defaults(pbsys_storage_settings_t *settings) {
@@ -74,9 +64,6 @@ static inline bool pbsys_storage_settings_bluetooth_enabled(void) {
 static inline void pbsys_storage_settings_bluetooth_enabled_request_toggle(void) {
 }
 
-static inline void pbsys_storage_settings_save_imu_settings(void) {
-}
-
 #endif // PBSYS_CONFIG_STORAGE
 
 #endif // _PBSYS_STORAGE_SETTINGS_H_

lib/pbio/src/imu.c

@@ -17,8 +17,48 @@
 #include <pbio/int_math.h>
 #include <pbio/util.h>
 
+#include <pbsys/storage.h>
+#include <pbsys/storage_settings.h>
+
 #if PBIO_CONFIG_IMU
 
+// Asynchronously loaded on boot. Cannot be used until loaded.
+pbio_imu_persistent_settings_t *persistent_settings = NULL;
+
+/**
+ * Sets default settings. This is called by the storage module if it has to
+ * erase the settings and reinitialize them, including when a different
+ * firmware version is detected.
+ *
+ * @param [in]  settings  The loaded settings to apply.
+ */
+void pbio_imu_set_default_settings(pbio_imu_persistent_settings_t *settings) {
+    settings->gyro_stationary_threshold = 3.0f;
+    settings->accel_stationary_threshold = 2500.0f;
+    settings->heading_correction = 360.0f;
+}
+
+/**
+ * Applies settings loaded from storage to this imu module.
+ *
+ * @param [in]  settings  The loaded settings to apply.
+ */
+void pbio_imu_apply_loaded_settings(pbio_imu_persistent_settings_t *settings) {
+
+    // Direct reference to the settings so we can update them later, then
+    // request saving on shutdown without another copy.
+    persistent_settings = settings;
+
+    // Apply the settings, just as if being set by user, but don't request
+    // writing, because there are no changes yet.
+    pbio_imu_set_settings(
+        settings->gyro_stationary_threshold,
+        settings->accel_stationary_threshold,
+        settings->heading_correction,
+        false
+        );
+}
+
 static pbdrv_imu_dev_t *imu_dev;
 static pbdrv_imu_config_t *imu_config;
 
@@ -140,11 +180,6 @@ bool pbio_imu_is_stationary(void) {
     return pbdrv_imu_is_stationary(imu_dev) && pbio_dcmotor_all_coasting();
 }
 
-// Measured rotation of the Z axis in the user frame for exactly one rotation
-// of the hub. This will be used to adjust the heading value, which is slightly
-// different for each hub.
-static float heading_degrees_per_rotation = 360.0f;
-
 /**
  * Sets the IMU settings. This includes the thresholds that define when the hub
  * is stationary. When the measurements are steadily below these levels, the
@@ -156,21 +191,30 @@ static float heading_degrees_per_rotation = 360.0f;
  * @param [in]  angular_velocity    Angular velocity threshold in deg/s.
  * @param [in]  acceleration        Acceleration threshold in mm/s^2
  * @param [in]  heading_correction  Measured degrees per full rotation of the hub.
+ * @param [in]  request_save        Request to save the settings to storage.
  * @returns ::PBIO_ERROR_INVALID_ARG if the heading correction is out of range,
  *          otherwise ::PBIO_SUCCESS.
  */
-pbio_error_t pbio_imu_set_settings(float angular_velocity, float acceleration, float heading_correction) {
+pbio_error_t pbio_imu_set_settings(float angular_velocity, float acceleration, float heading_correction, bool request_save) {
     if (!isnan(angular_velocity)) {
         imu_config->gyro_stationary_threshold = pbio_int_math_bind(angular_velocity / imu_config->gyro_scale, 1, INT16_MAX);
     }
     if (!isnan(acceleration)) {
         imu_config->accel_stationary_threshold = pbio_int_math_bind(acceleration / imu_config->accel_scale, 1, INT16_MAX);
     }
+
+    if (!persistent_settings) {
+        return PBIO_ERROR_FAILED;
+    }
+
     if (!isnan(heading_correction)) {
         if (heading_correction < 350 || heading_correction > 370) {
             return PBIO_ERROR_INVALID_ARG;
         }
-        heading_degrees_per_rotation = heading_correction;
+        persistent_settings->heading_correction = heading_correction;
+    }
+    if (request_save) {
+        pbsys_storage_request_write();
     }
     return PBIO_SUCCESS;
 }
@@ -183,9 +227,14 @@ pbio_error_t pbio_imu_set_settings(float angular_velocity, float acceleration, f
  * @param [out]  heading_correction  Measured degrees per full rotation of the hub.
  */
 void pbio_imu_get_settings(float *angular_velocity, float *acceleration, float *heading_correction) {
+
+    if (!persistent_settings) {
+        return;
+    }
+
     *angular_velocity = imu_config->gyro_stationary_threshold * imu_config->gyro_scale;
     *acceleration = imu_config->accel_stationary_threshold * imu_config->accel_scale;
-    *heading_correction = heading_degrees_per_rotation;
+    *heading_correction = persistent_settings->heading_correction;
 }
 
 /**
@@ -251,10 +300,11 @@ static float heading_offset = 0;
 float pbio_imu_get_heading(void) {
 
     pbio_geometry_xyz_t heading_mapped;
-
     pbio_geometry_vector_map(&pbio_orientation_base_orientation, &single_axis_rotation, &heading_mapped);
 
-    return -heading_mapped.z * 360.0f / heading_degrees_per_rotation - heading_offset;
+    float correction = persistent_settings ? (360.0f / persistent_settings->heading_correction) : 1.0f;
+
+    return -heading_mapped.z * correction - heading_offset;
 }
 
 /**

lib/pbio/sys/storage.h

@@ -19,7 +19,6 @@ pbio_error_t pbsys_storage_set_program_size(uint32_t size);
 pbio_error_t pbsys_storage_set_program_data(uint32_t offset, const void *data, uint32_t size);
 void pbsys_storage_get_program_data(pbsys_main_program_t *program);
 pbsys_storage_settings_t *pbsys_storage_settings_get_settings(void);
-void pbsys_storage_request_write(void);
 
 #else
 static inline void pbsys_storage_init(void) {
@@ -37,8 +36,6 @@ static inline pbio_error_t pbsys_storage_set_program_data(uint32_t offset, const
 }
 static inline void pbsys_storage_get_program_data(pbsys_main_program_t *program) {
 }
-static inline void pbsys_storage_request_write(void) {
-}
 
 #endif // PBSYS_CONFIG_STORAGE
 

lib/pbio/sys/storage_settings.c

@@ -14,6 +14,7 @@
 #include <pbio/error.h>
 #include <pbio/imu.h>
 #include <pbsys/status.h>
+#include <pbsys/storage.h>
 #include <pbsys/storage_settings.h>
 
 #include "bluetooth.h"
@@ -29,9 +30,7 @@ void pbsys_storage_settings_set_defaults(pbsys_storage_settings_t *settings) {
     settings->flags |= PBSYS_STORAGE_SETTINGS_FLAGS_BLUETOOTH_ENABLED;
     #endif
     #if PBIO_CONFIG_IMU
-    settings->gyro_stationary_threshold = 3.0f;
-    settings->accel_stationary_threshold = 2500.0f;
-    settings->heading_correction = 360.0f;
+    pbio_imu_set_default_settings(&settings->imu_settings);
     #endif // PBIO_CONFIG_IMU
 }
 
@@ -42,32 +41,7 @@ void pbsys_storage_settings_set_defaults(pbsys_storage_settings_t *settings) {
  */
 void pbsys_storage_settings_apply_loaded_settings(pbsys_storage_settings_t *settings) {
     #if PBIO_CONFIG_IMU
-    // return value not checked, assumed to be set valid previously
-    pbio_imu_set_settings(
-        settings->gyro_stationary_threshold,
-        settings->accel_stationary_threshold,
-        settings->heading_correction
-        );
-    #endif // PBIO_CONFIG_IMU
-}
-
-/**
- * Copies the configured IMU settings to storage and requests them to be saved.
- *
- * @param [in]  settings  Settings to populate.
- */
-void pbsys_storage_settings_save_imu_settings(void) {
-    pbsys_storage_settings_t *settings = pbsys_storage_settings_get_settings();
-    if (!settings) {
-        return;
-    }
-    #if PBIO_CONFIG_IMU
-    pbio_imu_get_settings(
-        &settings->gyro_stationary_threshold,
-        &settings->accel_stationary_threshold,
-        &settings->heading_correction
-        );
-    pbsys_storage_request_write();
+    pbio_imu_apply_loaded_settings(&settings->imu_settings);
     #endif // PBIO_CONFIG_IMU
 }
 

pybricks/common/pb_type_imu.c

@@ -192,11 +192,9 @@ static mp_obj_t pb_type_imu_settings(size_t n_args, const mp_obj_t *pos_args, mp
     pb_assert(pbio_imu_set_settings(
         angular_velocity_threshold_in == mp_const_none ? NAN : mp_obj_get_float(angular_velocity_threshold_in),
         acceleration_threshold_in == mp_const_none ? NAN : mp_obj_get_float(acceleration_threshold_in),
-        heading_correction_in == mp_const_none ? NAN : mp_obj_get_float(heading_correction_in)
-        ));
+        heading_correction_in == mp_const_none ? NAN : mp_obj_get_float(heading_correction_in),
+        true));
 
-    // Request that changed settings are saved on shutdown.
-    pbsys_storage_settings_save_imu_settings();
     return mp_const_none;
 }
 static MP_DEFINE_CONST_FUN_OBJ_KW(pb_type_imu_settings_obj, 1, pb_type_imu_settings);