pbio/sys/storage_settings: Make IMU settings persistent.

Author: laurensvalk

lib/pbio/drv/imu/imu_lsm6ds3tr_c_stm32.c

@@ -227,9 +227,11 @@ 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;
 
-    // default noise thresholds, can be changed during runtime
-    imu_dev->config.gyro_stationary_threshold = 71; // 5 deg/s
-    imu_dev->config.accel_stationary_threshold = 1044; // 2500 mm/s^2, or approx 25% of gravity
+    // Noise thresholds. Will be loaded from user preferences. Can be changed
+    // during runtime. Zero for now, so measurements are never lower. So
+    // calibration will not start until these values are loaded or set.
+    imu_dev->config.gyro_stationary_threshold = 0;
+    imu_dev->config.accel_stationary_threshold = 0;
 
     // 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) {

lib/pbio/include/pbsys/storage_settings.h

@@ -17,6 +17,7 @@
 
 #include <pbio/error.h>
 
+#include <pbio/config.h>
 #include <pbsys/config.h>
 
 /**
@@ -33,27 +34,43 @@ typedef enum {
  * System settings. All data types are little-endian.
  */
 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;
+    #endif
 } pbsys_storage_settings_t;
 
 #if PBSYS_CONFIG_STORAGE
 
-void pbsys_storage_set_default_settings(pbsys_storage_settings_t *settings);
+void pbsys_storage_settings_set_defaults(pbsys_storage_settings_t *settings);
+
+void pbsys_storage_settings_apply_loaded_settings(pbsys_storage_settings_t *settings);
 
 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_set_default_settings(pbsys_storage_settings_t *settings) {
+static inline void pbsys_storage_settings_set_defaults(pbsys_storage_settings_t *settings) {
+}
+static inline void pbsys_storage_settings_apply_loaded_settings(pbsys_storage_settings_t *settings) {
 }
 static inline bool pbsys_storage_settings_bluetooth_enabled(void) {
     return true;
 }
 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/sys/bluetooth.c

@@ -293,7 +293,7 @@ PROCESS_THREAD(pbsys_bluetooth_process, ev, data) {
 
     // Ensures Bluetooth preferences are loaded before we read them.
     #if PBSYS_CONFIG_STORAGE && PBSYS_CONFIG_BLUETOOTH_TOGGLE
-    PROCESS_WAIT_EVENT_UNTIL(pbsys_storage_get_settings() != NULL);
+    PROCESS_WAIT_EVENT_UNTIL(pbsys_storage_settings_get_settings() != NULL);
     #endif // PBSYS_CONFIG_STORAGE && PBSYS_CONFIG_BLUETOOTH_TOGGLE
 
     pbdrv_bluetooth_set_on_event(pbsys_bluetooth_process_poll);

lib/pbio/sys/storage.c

@@ -33,7 +33,7 @@ static pbsys_storage_data_map_t *map = &pbsys_user_ram_data_map.data_map;
 
 static bool data_map_is_loaded = false;
 
-pbsys_storage_settings_t *pbsys_storage_get_settings(void) {
+pbsys_storage_settings_t *pbsys_storage_settings_get_settings(void) {
     if (!data_map_is_loaded) {
         return NULL;
     }
@@ -246,7 +246,7 @@ PROCESS_THREAD(pbsys_storage_process, ev, data) {
         // Reset storage except for program data. It is sufficient to set its
         // size to 0, which is what happens here since it is in the map.
         memset(map, 0, sizeof(pbsys_storage_data_map_t));
-        pbsys_storage_set_default_settings(&map->settings);
+        pbsys_storage_settings_set_defaults(&map->settings);
 
         // Set firmware version used to create current storage map.
         map->stored_firmware_version = PBIO_HEXVERSION;
@@ -255,6 +255,9 @@ PROCESS_THREAD(pbsys_storage_process, ev, data) {
         pbsys_storage_request_write();
     }
 
+    // Apply loaded settings as necesary.
+    pbsys_storage_settings_apply_loaded_settings(&map->settings);
+
     // Poke processes that await on system settings to become available.
     data_map_is_loaded = true;
     process_post(PROCESS_BROADCAST, PROCESS_EVENT_COM, NULL);

lib/pbio/sys/storage.h

@@ -19,15 +19,15 @@ 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);
 pbio_error_t pbsys_storage_assert_program_valid(void);
 void pbsys_storage_get_program_data(pbsys_main_program_t *program);
-pbsys_storage_settings_t *pbsys_storage_get_settings(void);
+pbsys_storage_settings_t *pbsys_storage_settings_get_settings(void);
 void pbsys_storage_request_write(void);
 
 #else
 static inline void pbsys_storage_init(void) {
 }
 static inline void pbsys_storage_deinit(void) {
 }
-static inline pbsys_storage_settings_t *pbsys_storage_get_settings(void) {
+static inline pbsys_storage_settings_t *pbsys_storage_settings_get_settings(void) {
     return NULL;
 }
 static inline pbio_error_t pbsys_storage_set_program_size(uint32_t size) {

lib/pbio/sys/storage_settings.c

@@ -12,6 +12,7 @@
 #include <pbdrv/bluetooth.h>
 
 #include <pbio/error.h>
+#include <pbio/imu.h>
 #include <pbsys/status.h>
 #include <pbsys/storage_settings.h>
 
@@ -23,15 +24,46 @@
  *
  * @param [in]  settings  Settings to populate.
  */
-void pbsys_storage_set_default_settings(pbsys_storage_settings_t *settings) {
+void pbsys_storage_settings_set_defaults(pbsys_storage_settings_t *settings) {
     #if PBSYS_CONFIG_BLUETOOTH_TOGGLE
     settings->flags |= PBSYS_STORAGE_SETTINGS_FLAGS_BLUETOOTH_ENABLED;
     #endif
+    #if PBIO_CONFIG_IMU
+    settings->gyro_stationary_threshold = 5;
+    settings->accel_stationary_threshold = 2500;
+    #endif // PBIO_CONFIG_IMU
+}
+
+/**
+ * Applies the loaded settings and preferences after boot.
+ *
+ * @param [in]  settings  Settings to populate.
+ */
+void pbsys_storage_settings_apply_loaded_settings(pbsys_storage_settings_t *settings) {
+    #if PBIO_CONFIG_IMU
+    pbio_imu_set_stationary_thresholds(settings->gyro_stationary_threshold, settings->accel_stationary_threshold);
+    #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_stationary_thresholds(&settings->gyro_stationary_threshold, &settings->accel_stationary_threshold);
+    pbsys_storage_request_write();
+    #endif // PBIO_CONFIG_IMU
 }
 
 bool pbsys_storage_settings_bluetooth_enabled(void) {
     #if PBSYS_CONFIG_BLUETOOTH_TOGGLE
-    pbsys_storage_settings_t *settings = pbsys_storage_get_settings();
+    pbsys_storage_settings_t *settings = pbsys_storage_settings_get_settings();
     if (!settings) {
         return true;
     }
@@ -43,7 +75,7 @@ bool pbsys_storage_settings_bluetooth_enabled(void) {
 
 void pbsys_storage_settings_bluetooth_enabled_request_toggle(void) {
     #if PBSYS_CONFIG_BLUETOOTH_TOGGLE
-    pbsys_storage_settings_t *settings = pbsys_storage_get_settings();
+    pbsys_storage_settings_t *settings = pbsys_storage_settings_get_settings();
 
     // Ignore toggle request in all but idle system status.
     if (!settings

pybricks/common/pb_type_imu.c

@@ -13,6 +13,8 @@
 #include <pbio/geometry.h>
 #include <pbio/imu.h>
 
+#include <pbsys/storage_settings.h>
+
 #include "py/obj.h"
 
 #include <pybricks/common.h>
@@ -187,6 +189,7 @@ STATIC mp_obj_t pb_type_imu_settings(size_t n_args, const mp_obj_t *pos_args, mp
         acceleration = mp_obj_get_float(acceleration_threshold_in);
     }
     pbio_imu_set_stationary_thresholds(angular_velocity, acceleration);
+    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);
@@ -248,9 +251,6 @@ mp_obj_t pb_type_IMU_obj_new(mp_obj_t top_side_axis_in, mp_obj_t front_side_axis
 
     pbio_imu_set_base_orientation(&front_side_axis, &top_side_axis);
 
-    // Default noise thresholds.
-    pbio_imu_set_stationary_thresholds(5.0f, 2500.0f);
-
     // Return singleton instance.
     return MP_OBJ_FROM_PTR(&singleton_imu_obj);
 }