diff --git a/lib/pbio/include/pbio/imu.h b/lib/pbio/include/pbio/imu.h index 6807f3bdf..93c9ad9da 100644 --- a/lib/pbio/include/pbio/imu.h +++ b/lib/pbio/include/pbio/imu.h @@ -18,10 +18,30 @@ #include #include +/** + * 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; } diff --git a/lib/pbio/include/pbsys/storage.h b/lib/pbio/include/pbsys/storage.h index 1d94cffd9..16331d75e 100644 --- a/lib/pbio/include/pbsys/storage.h +++ b/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_ diff --git a/lib/pbio/include/pbsys/storage_settings.h b/lib/pbio/include/pbsys/storage_settings.h index 98df2f695..ff3a528bf 100644 --- a/lib/pbio/include/pbsys/storage_settings.h +++ b/lib/pbio/include/pbsys/storage_settings.h @@ -18,6 +18,7 @@ #include #include +#include #include /** @@ -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_ diff --git a/lib/pbio/src/imu.c b/lib/pbio/src/imu.c index ad773f24b..2e49fd6b0 100644 --- a/lib/pbio/src/imu.c +++ b/lib/pbio/src/imu.c @@ -17,8 +17,48 @@ #include #include +#include +#include + #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; } /** diff --git a/lib/pbio/sys/storage.h b/lib/pbio/sys/storage.h index cd64d73ab..2c9bae2c9 100644 --- a/lib/pbio/sys/storage.h +++ b/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 diff --git a/lib/pbio/sys/storage_settings.c b/lib/pbio/sys/storage_settings.c index 90d45fb45..c63c0cbf1 100644 --- a/lib/pbio/sys/storage_settings.c +++ b/lib/pbio/sys/storage_settings.c @@ -14,6 +14,7 @@ #include #include #include +#include #include #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 } diff --git a/pybricks/common/pb_type_imu.c b/pybricks/common/pb_type_imu.c index 1e8f74994..a922112b3 100644 --- a/pybricks/common/pb_type_imu.c +++ b/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);