diff --git a/CHANGELOG.md b/CHANGELOG.md index 6d96c6fdd..1f8d0e810 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -8,11 +8,20 @@ - Added optional `calibrated=True` parameter to `acceleration()` and `up()` and `angular_velocity()` methods of the IMU ([support#943]). +- Implemented `hub.imu.orientation()` to give the rotation matrix of the hub or + robot with respect to the inertial frame. +- Added calibration parameters that can be set for angular velocity offset and + scale and acceleration offset and scale. ### Changed - The method `DriveBase.angle()` now returns a float ([support#1844]). This makes it properly equivalent to `hub.imu.heading`. +- Re-implemented tilt using the gyro data by default. Pure accelerometer tilt + can still be obtained with `hub.imu.tilt(use_gyro=False)`. +- Re-implemented `hub.imu.heading()` to use projection of 3D orientation to + improve performance when the hub is lifted off the ground. If necessary, + pure 1D rotation can still be obtained from `hub.imu.rotation()`. ### Fixed - Fixed `DriveBase.angle()` getting an incorrectly rounded gyro value, which diff --git a/lib/pbio/include/pbio/geometry.h b/lib/pbio/include/pbio/geometry.h index fdb62ca89..be176d835 100644 --- a/lib/pbio/include/pbio/geometry.h +++ b/lib/pbio/include/pbio/geometry.h @@ -43,29 +43,46 @@ typedef union { /** * Representation of a 3x3 matrix. */ -typedef struct _pbio_geometry_matrix_3x3_t { - union { - struct { - float m11; - float m12; - float m13; - float m21; - float m22; - float m23; - float m31; - float m32; - float m33; - }; - float values[9]; +typedef union { + struct { + float m11; + float m12; + float m13; + float m21; + float m22; + float m23; + float m31; + float m32; + float m33; }; + float values[9]; } pbio_geometry_matrix_3x3_t; +/** + * Quaternion orientation or its time derivative. + */ +typedef union { + struct { + float q1; /**< q1 coordinate.*/ + float q2; /**< q2 coordinate.*/ + float q3; /**< q3 coordinate.*/ + float q4; /**< q4 coordinate.*/ + }; + float values[4]; +} pbio_geometry_quaternion_t; + +#define pbio_geometry_degrees_to_radians(degrees) ((degrees) * 0.017453293f) + +#define pbio_geometry_radians_to_degrees(radians) ((radians) * 57.29577951f) + void pbio_geometry_side_get_axis(pbio_geometry_side_t side, uint8_t *index, int8_t *sign); void pbio_geometry_get_complementary_axis(uint8_t *index, int8_t *sign); pbio_geometry_side_t pbio_geometry_side_from_vector(pbio_geometry_xyz_t *vector); +float pbio_geometry_vector_norm(pbio_geometry_xyz_t *input); + pbio_error_t pbio_geometry_vector_normalize(pbio_geometry_xyz_t *input, pbio_geometry_xyz_t *output); void pbio_geometry_vector_cross_product(pbio_geometry_xyz_t *a, pbio_geometry_xyz_t *b, pbio_geometry_xyz_t *output); @@ -74,8 +91,22 @@ pbio_error_t pbio_geometry_vector_project(pbio_geometry_xyz_t *axis, pbio_geomet void pbio_geometry_vector_map(pbio_geometry_matrix_3x3_t *map, pbio_geometry_xyz_t *input, pbio_geometry_xyz_t *output); +void pbio_geometry_matrix_multiply(pbio_geometry_matrix_3x3_t *a, pbio_geometry_matrix_3x3_t *b, pbio_geometry_matrix_3x3_t *output); + pbio_error_t pbio_geometry_map_from_base_axes(pbio_geometry_xyz_t *x_axis, pbio_geometry_xyz_t *z_axis, pbio_geometry_matrix_3x3_t *rotation); +void pbio_geometry_quaternion_to_rotation_matrix(pbio_geometry_quaternion_t *q, pbio_geometry_matrix_3x3_t *R); + +void pbio_geometry_quaternion_from_gravity_unit_vector(pbio_geometry_xyz_t *g, pbio_geometry_quaternion_t *q); + +void pbio_geometry_quaternion_get_rate_of_change(pbio_geometry_quaternion_t *q, pbio_geometry_xyz_t *w, pbio_geometry_quaternion_t *dq); + +void pbio_geometry_quaternion_normalize(pbio_geometry_quaternion_t *q); + +float pbio_geometry_maxf(float a, float b); + +float pbio_geometry_absf(float a); + #endif // _PBIO_GEOMETRY_H_ /** @} */ diff --git a/lib/pbio/include/pbio/imu.h b/lib/pbio/include/pbio/imu.h index 5ebd21c3f..981316559 100644 --- a/lib/pbio/include/pbio/imu.h +++ b/lib/pbio/include/pbio/imu.h @@ -92,6 +92,8 @@ void pbio_imu_get_angular_velocity(pbio_geometry_xyz_t *values, bool calibrated) 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_geometry_side_t pbio_imu_get_up_side(bool calibrated); @@ -102,6 +104,8 @@ void pbio_imu_set_heading(float desired_heading); void pbio_imu_get_heading_scaled(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); + #else // PBIO_CONFIG_IMU static inline void pbio_imu_init(void) { @@ -149,6 +153,10 @@ static inline void pbio_imu_set_heading(float desired_heading) { static inline void pbio_imu_get_heading_scaled(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) { +} + + #endif // PBIO_CONFIG_IMU #endif // _PBIO_IMU_H_ diff --git a/lib/pbio/src/geometry.c b/lib/pbio/src/geometry.c index e137e9fda..c77c3b574 100644 --- a/lib/pbio/src/geometry.c +++ b/lib/pbio/src/geometry.c @@ -104,9 +104,21 @@ pbio_geometry_side_t pbio_geometry_side_from_vector(pbio_geometry_xyz_t *vector) return axis | (negative << 2); } +/** + * Gets the norm of a vector. + * + * @param [in] input The vector. + * @return The norm of the vector. + */ +float pbio_geometry_vector_norm(pbio_geometry_xyz_t *input) { + return sqrtf(input->x * input->x + input->y * input->y + input->z * input->z); +} + /** * Normalizes a vector so it has unit length. * + * Output is allowed to be same as input, in which case input is normalized. + * * @param [in] input The vector to normalize. * @param [out] output The normalized vector. * @return ::PBIO_ERROR_INVALID_ARG if the input has zero length, otherwise ::PBIO_SUCCESS. @@ -114,7 +126,7 @@ pbio_geometry_side_t pbio_geometry_side_from_vector(pbio_geometry_xyz_t *vector) pbio_error_t pbio_geometry_vector_normalize(pbio_geometry_xyz_t *input, pbio_geometry_xyz_t *output) { // Compute the norm. - float norm = sqrtf(input->x * input->x + input->y * input->y + input->z * input->z); + float norm = pbio_geometry_vector_norm(input); // If the vector norm is zero, do nothing. if (norm == 0.0f) { @@ -176,6 +188,25 @@ void pbio_geometry_vector_map(pbio_geometry_matrix_3x3_t *map, pbio_geometry_xyz output->z = input->x * map->m31 + input->y * map->m32 + input->z * map->m33; } +/** + * Multiplies two 3x3 matrices. + * + * @param [in] a The first 3x3 matrix. + * @param [in] b The second 3x3 matrix. + * @param [out] output The resulting 3x3 matrix after multiplication. + */ +void pbio_geometry_matrix_multiply(pbio_geometry_matrix_3x3_t *a, pbio_geometry_matrix_3x3_t *b, pbio_geometry_matrix_3x3_t *output) { + output->m11 = a->m11 * b->m11 + a->m12 * b->m21 + a->m13 * b->m31; + output->m12 = a->m11 * b->m12 + a->m12 * b->m22 + a->m13 * b->m32; + output->m13 = a->m11 * b->m13 + a->m12 * b->m23 + a->m13 * b->m33; + output->m21 = a->m21 * b->m11 + a->m22 * b->m21 + a->m23 * b->m31; + output->m22 = a->m21 * b->m12 + a->m22 * b->m22 + a->m23 * b->m32; + output->m23 = a->m21 * b->m13 + a->m22 * b->m23 + a->m23 * b->m33; + output->m31 = a->m31 * b->m11 + a->m32 * b->m21 + a->m33 * b->m31; + output->m32 = a->m31 * b->m12 + a->m32 * b->m22 + a->m33 * b->m32; + output->m33 = a->m31 * b->m13 + a->m32 * b->m23 + a->m33 * b->m33; +} + /** * Gets a mapping (a rotation matrix) from two orthogonal base axes. * @@ -216,3 +247,101 @@ pbio_error_t pbio_geometry_map_from_base_axes(pbio_geometry_xyz_t *x_axis, pbio_ return PBIO_SUCCESS; } + +/** + * Computes a rotation matrix for a quaternion. + * + * @param [in] q The quaternion. + * @param [out] R The rotation matrix. + */ +void pbio_geometry_quaternion_to_rotation_matrix(pbio_geometry_quaternion_t *q, pbio_geometry_matrix_3x3_t *R) { + R->m11 = 1 - 2 * (q->q2 * q->q2 + q->q3 * q->q3); + R->m21 = 2 * (q->q1 * q->q2 + q->q3 * q->q4); + R->m31 = 2 * (q->q1 * q->q3 - q->q2 * q->q4); + R->m12 = 2 * (q->q1 * q->q2 - q->q3 * q->q4); + R->m22 = 1 - 2 * (q->q1 * q->q1 + q->q3 * q->q3); + R->m32 = 2 * (q->q2 * q->q3 + q->q1 * q->q4); + R->m13 = 2 * (q->q1 * q->q3 + q->q2 * q->q4); + R->m23 = 2 * (q->q2 * q->q3 - q->q1 * q->q4); + R->m33 = 1 - 2 * (q->q1 * q->q1 + q->q2 * q->q2); +} + +/** + * Computes a quaternion from a gravity unit vector. + * + * @param [in] g The gravity unit vector. + * @param [out] q The resulting quaternion. + */ +void pbio_geometry_quaternion_from_gravity_unit_vector(pbio_geometry_xyz_t *g, pbio_geometry_quaternion_t *q) { + if (g->z >= 0) { + q->q4 = sqrtf((g->z + 1) / 2); + q->q1 = g->y / sqrtf(2 * (g->z + 1)); + q->q2 = -g->x / sqrtf(2 * (g->z + 1)); + q->q3 = 0; + } else { + q->q4 = -g->y / sqrtf(2 * (1 - g->z)); + q->q1 = -sqrtf((1 - g->z) / 2); + q->q2 = 0; + q->q3 = -g->x / sqrtf(2 * (1 - g->z)); + } +} + +/** + * Computes the rate of change of a quaternion given the angular velocity vector. + * + * @param [in] q Quaternion of the current orientation. + * @param [in] angular_velocity The angular velocity vector in the body frame. + * @param [out] dq The rate of change of the quaternion. + */ +void pbio_geometry_quaternion_get_rate_of_change(pbio_geometry_quaternion_t *q, pbio_geometry_xyz_t *angular_velocity, pbio_geometry_quaternion_t *dq) { + + pbio_geometry_xyz_t w = { + .x = pbio_geometry_degrees_to_radians(angular_velocity->x), + .y = pbio_geometry_degrees_to_radians(angular_velocity->y), + .z = pbio_geometry_degrees_to_radians(angular_velocity->z), + }; + + dq->q1 = 0.5f * (w.z * q->q2 - w.y * q->q3 + w.x * q->q4); + dq->q2 = 0.5f * (-w.z * q->q1 + w.x * q->q3 + w.y * q->q4); + dq->q3 = 0.5f * (w.y * q->q1 - w.x * q->q2 + w.z * q->q4); + dq->q4 = 0.5f * (-w.x * q->q1 - w.y * q->q2 - w.z * q->q3); +} + +/** + * Normalizes a quaternion so it has unit length. + * + * @param [inout] q The quaternion to normalize. + */ +void pbio_geometry_quaternion_normalize(pbio_geometry_quaternion_t *q) { + float norm = sqrtf(q->q1 * q->q1 + q->q2 * q->q2 + q->q3 * q->q3 + q->q4 * q->q4); + + if (norm < 0.0001f && norm > -0.0001f) { + return; + } + + q->q1 /= norm; + q->q2 /= norm; + q->q3 /= norm; + q->q4 /= norm; +} + +/** + * Returns the maximum of two floating-point numbers. + * + * @param a The first floating-point number. + * @param b The second floating-point number. + * @return The maximum of the two floating-point numbers. + */ +float pbio_geometry_maxf(float a, float b) { + return a > b ? a : b; +} + +/** + * Returns the absolute value of a floating-point number. + * + * @param a The floating-point number. + * @return The absolute value of the floating-point number. + */ +float pbio_geometry_absf(float a) { + return a < 0 ? -a : a; +} diff --git a/lib/pbio/src/imu.c b/lib/pbio/src/imu.c index 0f044700c..55fc36f82 100644 --- a/lib/pbio/src/imu.c +++ b/lib/pbio/src/imu.c @@ -6,6 +6,8 @@ #include #include +#include + #include #include @@ -22,21 +24,156 @@ #if PBIO_CONFIG_IMU +/** + * Driver device. + */ static pbdrv_imu_dev_t *imu_dev; + +/** + * Driver configuration. Contains settings like gyro and accelerometer scale + * from raw units to (uncalibrated) physical units and thresholds that say + * when the hub is stationary to be considered ready for calibration. + */ static pbdrv_imu_config_t *imu_config; -// Cached sensor values that can be read at any time without polling again. -static pbio_geometry_xyz_t angular_velocity_uncalibrated; // deg/s, in hub frame -static pbio_geometry_xyz_t angular_velocity_calibrated; // deg/s, in hub frame, already adjusted for bias and scale. -static pbio_geometry_xyz_t acceleration_uncalibrated; // mm/s^2, in hub frame +/** + * Uncalibrated angular velocity in the hub frame. + * + * These are scaled from raw units to degrees per second using only the + * datasheet/hal conversion constant, but otherwise not further adjusted. + */ +static pbio_geometry_xyz_t angular_velocity_uncalibrated; + +/** + * Estimated gyro bias value in degrees per second. + * + * This is a measure for the uncalibrated angular velocity above, averaged over + * time. If specified, the value starts at the last saved user value, then + * updates over time. + */ +static pbio_geometry_xyz_t gyro_bias; + +/** + * Calibrated angular velocity in the hub frame degrees per second. + * + * This takes the uncalibrated value above, subtracts the bias estimate, and + * rescales by a user calibration factor to ensure that integrating over one + * full rotation adds up to 360 degrees. + */ +static pbio_geometry_xyz_t angular_velocity_calibrated; + +/** + * Uncalibrated acceleration in the hub frame in mm/s^2. + * + * These are scaled from raw units to mm/s^2 using only the + * datasheet/hal conversion constant, but otherwise not further adjusted. + */ +static pbio_geometry_xyz_t acceleration_uncalibrated; + +/** + * Calibrated acceleration in the hub frame mm/s^2. + * + * This takes the uncalibrated value above, and subtracts a constant user offset + * and scales by a previously determined user factor to normalize to gravity magnitude. + */ static pbio_geometry_xyz_t acceleration_calibrated; // mm/s^2, in hub frame -static pbio_geometry_xyz_t gyro_bias; // Starts at value from settings, then updated when stationary. + +/** + * 1D integrated angular velocity for each body axis. + * + * This is based on integrating the calibrated angular velocity over time, so + * including its bias and adjustments to achhieve 360 degrees per rotation. + * + * This is not used for 3D attitude estimation, but serves as a useful way to + * estimate 1D rotations without being effected by accelerometer fusion which + * may leads to unwanted adjustments in applications like balancing robots. + */ static pbio_geometry_xyz_t single_axis_rotation; // deg, in hub frame -// Asynchronously loaded on boot. Cannot be used until loaded. +/** + * Rotation of the hub with respect to the inertial frame, see R(q) below. + * + * Initialized as the identity quaternion. Updated on first gravity sample. + */ +static pbio_geometry_quaternion_t quaternion = { + .q1 = 0.0f, + .q2 = 0.0f, + .q3 = 0.0f, + .q4 = 1.0f, +}; + +/** + * Flag to indicate if the quaternion has been initialized to the very first + * gravity sample. + */ +static bool quaternion_initialized = false; + +/** + * Rotation of the hub with respect to the inertial frame. + * + * Does *not* use the user application frame. + * + * The matrix R(q) is defined such that it transforms hub body frame vectors to + * vectors in the inertial frame as: + * + * v_inertial = R(q) * v_body + * + * Initialized as the identity matrix. Must match initial value of quaternion. + */ +static pbio_geometry_matrix_3x3_t pbio_imu_rotation = { + .m11 = 1.0f, .m12 = 0.0f, .m13 = 0.0f, + .m21 = 0.0f, .m22 = 1.0f, .m23 = 0.0f, + .m31 = 0.0f, .m32 = 0.0f, .m33 = 1.0f, +}; + + +/** + * The "neutral" base orientation of the hub, describing how it is mounted + * in the robot. All getters (tilt, acceleration, rotation, etc) give results + * relative to this base orientation: + * + * vector_reported = R_base * vector_in_hub_body_frame + * + * Default orientation is identity, hub flat. + */ +static pbio_geometry_matrix_3x3_t pbio_imu_base_orientation = { + .m11 = 1.0f, .m12 = 0.0f, .m13 = 0.0f, + .m21 = 0.0f, .m22 = 1.0f, .m23 = 0.0f, + .m31 = 0.0f, .m32 = 0.0f, .m33 = 1.0f, +}; + +/** + * The heading is defined as follows. + * + * Take the x-axis (after transformation to application frame) and project + * into the inertial frame. Then project onto the horizontal (X-Y) plane. Then + * take the angle between the projection and the x-axis, counterclockwise + * positive. + * + * In practice, this means that when you look at a robot from the top, it is + * the angle that its "forward direction vector" makes with respect to the + * x-axis, even when the robot isn't perfectly flat. + * + */ +static float heading_projection; + +/** + * When the heading_projection flips from 180 to -180 or vice versa, we + * increment or decrement the overal rotation counter to maintain a continuous + * heading. + */ +static int32_t heading_rotations; + + +/** + * Hub calibration settings. Cannot be used until loaded. + */ static pbio_imu_persistent_settings_t *persistent_settings = NULL; -const float standard_gravity = 9806.65f; // mm/s^2 +/** + * Standard gravity in mm/s^2. + */ +const float standard_gravity = 9806.65f; /** * Applies (newly set) settings to the driver. @@ -90,8 +227,63 @@ void pbio_imu_apply_loaded_settings(pbio_imu_persistent_settings_t *settings) { pbio_imu_apply_pbdrv_settings(settings); } +/** + * Given current orientation matrix, update the heading projection. + * + * This is called from the update loop so we can catch the projection jumping + * across the 180/-180 boundary, and increment or decrement the rotation to + * have a continuous heading. + * + * This is also called when the orientation frame is changed because this sets + * the application x-axis used for the heading projection. + */ +static void update_heading_projection(void) { + + // Transform application x axis back into the hub frame (R_base^T * x_unit). + pbio_geometry_xyz_t x_application = { + .x = pbio_imu_base_orientation.m11, + .y = pbio_imu_base_orientation.m12, + .z = pbio_imu_base_orientation.m13 + }; + + // Transform application x axis into the inertial frame via quaternion matrix. + pbio_geometry_xyz_t x_inertial; + pbio_geometry_vector_map(&pbio_imu_rotation, &x_application, &x_inertial); + + // Project onto the horizontal plane and use atan2 to get the angle. + float heading_now = pbio_geometry_radians_to_degrees(atan2f(-x_inertial.y, x_inertial.x)); + + // Update full rotation counter if the projection jumps across the 180/-180 boundary. + if (heading_now < -90 && heading_projection > 90) { + heading_rotations++; + } else if (heading_now > 90 && heading_projection < -90) { + heading_rotations--; + } + heading_projection = heading_now; +} + // Called by driver to process one frame of unfiltered gyro and accelerometer data. static void pbio_imu_handle_frame_data_func(int16_t *data) { + + // Initialize quaternion from first gravity sample as a best-effort estimate. + // From here, fusion will gradually converge the quaternion to the true value. + if (!quaternion_initialized) { + pbio_geometry_xyz_t g = { .x = data[3], .y = data[4], .z = data[5]}; + pbio_error_t err = pbio_geometry_vector_normalize(&g, &g); + if (err != PBIO_SUCCESS) { + // First sample not suited, try again on next sample. + return; + } + pbio_geometry_quaternion_from_gravity_unit_vector(&g, &quaternion); + quaternion_initialized = true; + } + + // Compute current orientation matrix to obtain the current heading. + pbio_geometry_quaternion_to_rotation_matrix(&quaternion, &pbio_imu_rotation); + + // Projects application x-axis into the inertial frame to compute the heading. + update_heading_projection(); + for (uint8_t i = 0; i < PBIO_ARRAY_SIZE(angular_velocity_calibrated.values); i++) { // Update angular velocity and acceleration cache so user can read them. angular_velocity_uncalibrated.values[i] = data[i] * imu_config->gyro_scale; @@ -115,6 +307,55 @@ static void pbio_imu_handle_frame_data_func(int16_t *data) { // applications so long as the vehicle drives on a flat surface. single_axis_rotation.values[i] += angular_velocity_calibrated.values[i] * imu_config->sample_time; } + + // Estimate for gravity vector based on orientation estimate. + pbio_geometry_xyz_t s = { + .x = pbio_imu_rotation.m31, + .y = pbio_imu_rotation.m32, + .z = pbio_imu_rotation.m33, + }; + + // We would like to adjust the attitude such that the gravity estimate + // converges to the gravity value in the stationary case. If we subtract + // both vectors we get the required direction of changes. This can be + // thought of as a virtual spring between both vectors. This produces a + // moment about the origin, which ultimately simplies to the following, + // which we inject to the attitude integration. + pbio_geometry_xyz_t correction; + pbio_geometry_vector_cross_product(&s, &acceleration_calibrated, &correction); + + // Qualitative measures for how far the current state is from being stationary. + float accl_stationary_error = pbio_geometry_absf(pbio_geometry_vector_norm(&acceleration_calibrated) - standard_gravity); + float gyro_stationary_error = pbio_geometry_absf(pbio_geometry_vector_norm(&angular_velocity_calibrated)); + + // Cut off value below which value is considered stationary enough for fusion. + const float gyro_stationary_min = 10; + const float accl_stationary_min = 150; + + // Measure for being statinonary ranging from 0 (moving) to 1 (moving less than above thresholds). + float stationary_measure = accl_stationary_min / pbio_geometry_maxf(accl_stationary_error, accl_stationary_min) * + gyro_stationary_min / pbio_geometry_maxf(gyro_stationary_error, gyro_stationary_min); + + // The virtual moment would produce motion in that direction, so we can + // simulate that effect by injecting it into the attitude integration, the + // strength of which is based on the stationary measure. It is scaled down + // by the gravity amount since one of the two vectors to produce this has + // units of gravity. Hence if the hub is stationary (measure = 1), and the + // error is 90 degrees (which is unlikely), the correction is at + // most 200 deg/s, but usually much less. + float fusion = -stationary_measure / standard_gravity * 200; + pbio_geometry_xyz_t adjusted_angular_velocity; + adjusted_angular_velocity.x = angular_velocity_calibrated.x + correction.x * fusion; + adjusted_angular_velocity.y = angular_velocity_calibrated.y + correction.y * fusion; + adjusted_angular_velocity.z = angular_velocity_calibrated.z + correction.z * fusion; + + // Update 3D attitude, basic forward integration. + pbio_geometry_quaternion_t dq; + pbio_geometry_quaternion_get_rate_of_change(&quaternion, &adjusted_angular_velocity, &dq); + for (uint8_t i = 0; i < PBIO_ARRAY_SIZE(dq.values); i++) { + quaternion.values[i] += dq.values[i] * imu_config->sample_time; + } + pbio_geometry_quaternion_normalize(&quaternion); } // This counter is a measure for calibration accuracy, roughly equivalent @@ -172,17 +413,6 @@ void pbio_imu_init(void) { pbdrv_imu_set_data_handlers(imu_dev, pbio_imu_handle_frame_data_func, pbio_imu_handle_stationary_data_func); } -/** - * The "neutral" base orientation of the hub, describing how it is mounted - * in the robot. All getters (tilt, acceleration, rotation, etc) give results - * relative to this base orientation. Initial orientation is identity, hub flat. - */ -static pbio_geometry_matrix_3x3_t pbio_orientation_base_orientation = { - .m11 = 1.0f, .m12 = 0.0f, .m13 = 0.0f, - .m21 = 0.0f, .m22 = 1.0f, .m23 = 0.0f, - .m31 = 0.0f, .m32 = 0.0f, .m33 = 1.0f, -}; - /** * Sets the hub base orientation. * @@ -194,13 +424,16 @@ static pbio_geometry_matrix_3x3_t pbio_orientation_base_orientation = { */ pbio_error_t pbio_imu_set_base_orientation(pbio_geometry_xyz_t *front_side_axis, pbio_geometry_xyz_t *top_side_axis) { - pbio_error_t err = pbio_geometry_map_from_base_axes(front_side_axis, top_side_axis, &pbio_orientation_base_orientation); + pbio_error_t err = pbio_geometry_map_from_base_axes(front_side_axis, top_side_axis, &pbio_imu_base_orientation); if (err != PBIO_SUCCESS) { return err; } - pbio_imu_set_heading(0.0f); + // Need to update heading projection since the application axes were changed. + update_heading_projection(); + // Reset offsets such that the new frame starts with zero heading. + pbio_imu_set_heading(0.0f); return PBIO_SUCCESS; } @@ -314,7 +547,7 @@ pbio_error_t pbio_imu_get_settings(pbio_imu_persistent_settings_t **settings) { */ void pbio_imu_get_angular_velocity(pbio_geometry_xyz_t *values, bool calibrated) { pbio_geometry_xyz_t *angular_velocity = calibrated ? &angular_velocity_calibrated : &angular_velocity_uncalibrated; - pbio_geometry_vector_map(&pbio_orientation_base_orientation, angular_velocity, values); + pbio_geometry_vector_map(&pbio_imu_base_orientation, angular_velocity, values); } /** @@ -326,7 +559,21 @@ void pbio_imu_get_angular_velocity(pbio_geometry_xyz_t *values, bool calibrated) */ void pbio_imu_get_acceleration(pbio_geometry_xyz_t *values, bool calibrated) { pbio_geometry_xyz_t *acceleration = calibrated ? &acceleration_calibrated : &acceleration_uncalibrated; - pbio_geometry_vector_map(&pbio_orientation_base_orientation, acceleration, values); + pbio_geometry_vector_map(&pbio_imu_base_orientation, acceleration, values); +} + +/** + * Gets the vector that is parallel to the acceleration measurement of the stationary case. + * + * @param [out] values The acceleration vector. + */ +void pbio_imu_get_tilt_vector(pbio_geometry_xyz_t *values) { + pbio_geometry_xyz_t direction = { + .x = pbio_imu_rotation.m31, + .y = pbio_imu_rotation.m32, + .z = pbio_imu_rotation.m33, + }; + pbio_geometry_vector_map(&pbio_imu_base_orientation, &direction, values); } /** @@ -342,7 +589,7 @@ pbio_error_t pbio_imu_get_single_axis_rotation(pbio_geometry_xyz_t *axis, float // Transform the single axis rotations to the robot frame. pbio_geometry_xyz_t rotation; - pbio_geometry_vector_map(&pbio_orientation_base_orientation, &single_axis_rotation, &rotation); + pbio_geometry_vector_map(&pbio_imu_base_orientation, &single_axis_rotation, &rotation); // Get the requested scalar rotation along the given axis. return pbio_geometry_vector_project(axis, &rotation, angle); @@ -375,9 +622,7 @@ static float heading_offset = 0; * @return Heading angle in the base frame. */ 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 - heading_offset; + return heading_rotations * 360.0f + heading_projection - heading_offset; } /** @@ -389,6 +634,7 @@ float pbio_imu_get_heading(void) { * @param [in] desired_heading The desired heading value. */ void pbio_imu_set_heading(float desired_heading) { + heading_rotations = 0; heading_offset = pbio_imu_get_heading() + heading_offset - desired_heading; } @@ -424,4 +670,13 @@ void pbio_imu_get_heading_scaled(pbio_angle_t *heading, int32_t *heading_rate, i *heading_rate = (int32_t)(-angular_rate.z * ctl_steps_per_degree); } +/** + * Reads the current rotation matrix. + * + * @param [out] rotation The rotation matrix + */ +void pbio_orientation_imu_get_rotation(pbio_geometry_matrix_3x3_t *rotation) { + *rotation = pbio_imu_rotation; +} + #endif // PBIO_CONFIG_IMU diff --git a/pybricks/common/pb_type_imu.c b/pybricks/common/pb_type_imu.c index 1e6665389..a0aa8b164 100644 --- a/pybricks/common/pb_type_imu.c +++ b/pybricks/common/pb_type_imu.c @@ -57,25 +57,33 @@ static mp_obj_t pb_type_imu_up(size_t n_args, const mp_obj_t *pos_args, mp_map_t static MP_DEFINE_CONST_FUN_OBJ_KW(pb_type_imu_up_obj, 1, pb_type_imu_up); // pybricks._common.IMU.tilt -static mp_obj_t pb_type_imu_tilt(mp_obj_t self_in) { +static mp_obj_t pb_type_imu_tilt(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_TRUE(use_gyro), + PB_ARG_DEFAULT_TRUE(calibrated)); - // Revisit: optionally use not just calibrated but also IMU. + (void)self; // Read acceleration in the user frame. pbio_geometry_xyz_t accl; - pbio_imu_get_acceleration(&accl, false); + if (mp_obj_is_true(use_gyro_in)) { + pbio_imu_get_tilt_vector(&accl); + } else { + pbio_imu_get_acceleration(&accl, mp_obj_is_true(calibrated_in)); + } mp_obj_t tilt[2]; // Pitch float pitch = atan2f(-accl.x, sqrtf(accl.z * accl.z + accl.y * accl.y)); - tilt[0] = mp_obj_new_int_from_float(pitch * 57.296f); + tilt[0] = mp_obj_new_float_from_f(pitch * 57.296f); // Roll float roll = atan2f(accl.y, accl.z); - tilt[1] = mp_obj_new_int_from_float(roll * 57.296f); + tilt[1] = mp_obj_new_float_from_f(roll * 57.296f); return mp_obj_new_tuple(2, tilt); } -MP_DEFINE_CONST_FUN_OBJ_1(pb_type_imu_tilt_obj, pb_type_imu_tilt); +static MP_DEFINE_CONST_FUN_OBJ_KW(pb_type_imu_tilt_obj, 1, pb_type_imu_tilt); static void pb_type_imu_extract_axis(mp_obj_t obj_in, pbio_geometry_xyz_t *vector) { if (!mp_obj_is_type(obj_in, &pb_type_Matrix)) { @@ -288,6 +296,21 @@ static mp_obj_t pb_type_imu_heading(mp_obj_t self_in) { } MP_DEFINE_CONST_FUN_OBJ_1(pb_type_imu_heading_obj, pb_type_imu_heading); +// pybricks._common.IMU.orientation +STATIC mp_obj_t common_IMU_orientation(mp_obj_t self_in) { + + // Make matrix. REVISIT: Dedicated call from orientation matrix. + 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); + + memcpy(matrix->data, orientation.values, sizeof(orientation.values)); + + return MP_OBJ_FROM_PTR(matrix); +} +MP_DEFINE_CONST_FUN_OBJ_1(common_IMU_orientation_obj, common_IMU_orientation); + // pybricks._common.IMU.reset_heading static mp_obj_t pb_type_imu_reset_heading(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, @@ -339,6 +362,7 @@ static const mp_rom_map_elem_t pb_type_imu_locals_dict_table[] = { { MP_ROM_QSTR(MP_QSTR_stationary), MP_ROM_PTR(&pb_type_imu_stationary_obj) }, { MP_ROM_QSTR(MP_QSTR_tilt), MP_ROM_PTR(&pb_type_imu_tilt_obj) }, { MP_ROM_QSTR(MP_QSTR_up), MP_ROM_PTR(&pb_type_imu_up_obj) }, + { MP_ROM_QSTR(MP_QSTR_orientation), MP_ROM_PTR(&common_IMU_orientation_obj) }, { MP_ROM_QSTR(MP_QSTR_update_heading_correction), MP_ROM_PTR(&pb_type_imu_update_heading_correction_obj)}, }; static MP_DEFINE_CONST_DICT(pb_type_imu_locals_dict, pb_type_imu_locals_dict_table);