pbio/imu: Allow reading uncalibrated rotation.

This makes the calibration user routine simpler, so we can do everything from raw data.

Author: laurensvalk

lib/pbio/include/pbio/imu.h

@@ -124,7 +124,7 @@ void pbio_imu_get_acceleration(pbio_geometry_xyz_t *values, bool calibrated);
 
 void pbio_imu_get_tilt_vector(pbio_geometry_xyz_t *values);
 
-pbio_error_t pbio_imu_get_single_axis_rotation(pbio_geometry_xyz_t *axis, float *angle);
+pbio_error_t pbio_imu_get_single_axis_rotation(pbio_geometry_xyz_t *axis, float *angle, bool calibrated);
 
 pbio_geometry_side_t pbio_imu_get_up_side(bool calibrated);
 
@@ -134,7 +134,7 @@ void pbio_imu_set_heading(float desired_heading);
 
 void pbio_imu_get_heading_scaled(pbio_imu_heading_type_t type, pbio_angle_t *heading, int32_t *heading_rate, int32_t ctl_steps_per_degree);
 
-void pbio_orientation_imu_get_rotation(pbio_geometry_matrix_3x3_t *rotation);
+void pbio_orientation_imu_get_orientation(pbio_geometry_matrix_3x3_t *rotation);
 
 #else // PBIO_CONFIG_IMU
 
@@ -148,19 +148,23 @@ static inline void pbio_imu_apply_loaded_settings(pbio_imu_persistent_settings_t
 }
 
 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;
+    return PBIO_ERROR_NOT_SUPPORTED;
 }
 
 static inline bool pbio_imu_is_stationary(void) {
     return false;
 }
 
+static inline bool pbio_imu_is_ready(void) {
+    return false;
+}
+
 static inline pbio_error_t pbio_imu_get_settings(pbio_imu_persistent_settings_t **settings) {
     return PBIO_ERROR_NOT_SUPPORTED;
 }
 
-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;
+static inline pbio_error_t pbio_imu_set_settings(pbio_imu_persistent_settings_t *new_settings);
+return PBIO_ERROR_NOT_SUPPORTED;
 }
 
 static inline void pbio_imu_get_angular_velocity(pbio_geometry_xyz_t *values, bool calibrated) {
@@ -169,6 +173,13 @@ static inline void pbio_imu_get_angular_velocity(pbio_geometry_xyz_t *values, bo
 static inline void pbio_imu_get_acceleration(pbio_geometry_xyz_t *values, bool calibrated) {
 }
 
+static inline void pbio_imu_get_tilt_vector(pbio_geometry_xyz_t *values) {
+}
+
+static inline pbio_error_t pbio_imu_get_single_axis_rotation(pbio_geometry_xyz_t *axis, float *angle, bool calibrated) {
+    return PBIO_ERROR_NOT_SUPPORTED;
+}
+
 static inline pbio_geometry_side_t pbio_imu_get_up_side(bool calibrated) {
     return PBIO_GEOMETRY_SIDE_TOP;
 }
@@ -183,7 +194,7 @@ static inline void pbio_imu_set_heading(float desired_heading) {
 static inline void pbio_imu_get_heading_scaled(pbio_imu_heading_type_t type, pbio_angle_t *heading, int32_t *heading_rate, int32_t ctl_steps_per_degree) {
 }
 
-static inline void pbio_orientation_imu_get_rotation(pbio_geometry_matrix_3x3_t *rotation) {
+static inline void pbio_orientation_imu_get_orientation(pbio_geometry_matrix_3x3_t *rotation) {
 }
 
 

lib/pbio/src/imu.c

@@ -522,6 +522,9 @@ pbio_error_t pbio_imu_set_settings(pbio_imu_persistent_settings_t *new_settings)
     }
 
     if (new_settings->flags & PBIO_IMU_SETTINGS_FLAGS_HEADING_CORRECTION_1D_SET) {
+        if (new_settings->heading_correction_1d < 350 || new_settings->heading_correction_1d > 370) {
+            return PBIO_ERROR_INVALID_ARG;
+        }
         persistent_settings->heading_correction_1d = new_settings->heading_correction_1d;
     }
 
@@ -597,16 +600,30 @@ void pbio_imu_get_tilt_vector(pbio_geometry_xyz_t *values) {
  *
  * @param [in]  axis        The axis to project the rotation onto.
  * @param [out] angle       The angle of rotation in degrees.
+ * @param [in]  calibrated  Whether to use the adjusted scale (true) or the raw scale (false).
  * @return                  ::PBIO_SUCCESS on success, ::PBIO_ERROR_INVALID_ARG if axis has zero length.
  */
-pbio_error_t pbio_imu_get_single_axis_rotation(pbio_geometry_xyz_t *axis, float *angle) {
+pbio_error_t pbio_imu_get_single_axis_rotation(pbio_geometry_xyz_t *axis, float *angle, bool calibrated) {
+
+    // Local copy so we can change it in-place.
+    pbio_geometry_xyz_t axis_rotation = single_axis_rotation;
+    if (!calibrated) {
+        // In this context, calibrated means that the angular velocity values
+        // were scaled by the user calibration factors before integrating. This
+        // is done within the update loop since we need it for 3D integration.
+        // Since this method only returns the separate 1D rotations, we can
+        // undo this scaling here to get the "uncalibrated" values.
+        for (uint8_t i = 0; i < PBIO_ARRAY_SIZE(axis_rotation.values); i++) {
+            axis_rotation.values[i] *= persistent_settings->angular_velocity_scale.values[i] / 360.0f;
+        }
+    }
 
     // Transform the single axis rotations to the robot frame.
-    pbio_geometry_xyz_t rotation;
-    pbio_geometry_vector_map(&pbio_imu_base_orientation, &single_axis_rotation, &rotation);
+    pbio_geometry_xyz_t rotation_user;
+    pbio_geometry_vector_map(&pbio_imu_base_orientation, &axis_rotation, &rotation_user);
 
     // Get the requested scalar rotation along the given axis.
-    return pbio_geometry_vector_project(axis, &rotation, angle);
+    return pbio_geometry_vector_project(axis, &rotation_user, angle);
 }
 
 /**
@@ -711,7 +728,7 @@ void pbio_imu_get_heading_scaled(pbio_imu_heading_type_t type, pbio_angle_t *hea
  *
  * @param [out] rotation      The rotation matrix
  */
-void pbio_orientation_imu_get_rotation(pbio_geometry_matrix_3x3_t *rotation) {
+void pbio_orientation_imu_get_orientation(pbio_geometry_matrix_3x3_t *rotation) {
     *rotation = pbio_imu_rotation;
 }
 

pybricks/common/pb_type_imu.c

@@ -154,7 +154,8 @@ static MP_DEFINE_CONST_FUN_OBJ_KW(pb_type_imu_angular_velocity_obj, 1, pb_type_i
 static mp_obj_t pb_type_imu_rotation(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,
         pb_type_imu_obj_t, self,
-        PB_ARG_DEFAULT_NONE(axis));
+        PB_ARG_DEFAULT_NONE(axis),
+        PB_ARG_DEFAULT_TRUE(calibrated));
 
     (void)self;
 
@@ -163,7 +164,7 @@ static mp_obj_t pb_type_imu_rotation(size_t n_args, const mp_obj_t *pos_args, mp
     pb_type_imu_extract_axis(axis_in, &axis);
 
     float rotation_angle;
-    pb_assert(pbio_imu_get_single_axis_rotation(&axis, &rotation_angle));
+    pb_assert(pbio_imu_get_single_axis_rotation(&axis, &rotation_angle, mp_obj_is_true(calibrated_in)));
     return mp_obj_new_float_from_f(rotation_angle);
 }
 static MP_DEFINE_CONST_FUN_OBJ_KW(pb_type_imu_rotation_obj, 1, pb_type_imu_rotation);
@@ -318,7 +319,7 @@ STATIC mp_obj_t common_IMU_orientation(mp_obj_t self_in) {
     pb_type_Matrix_obj_t *matrix = MP_OBJ_TO_PTR(pb_type_Matrix_make_bitmap(3, 3, 1.0f, 0));
 
     pbio_geometry_matrix_3x3_t orientation;
-    pbio_orientation_imu_get_rotation(&orientation);
+    pbio_orientation_imu_get_orientation(&orientation);
 
     memcpy(matrix->data, orientation.values, sizeof(orientation.values));