From d7fb4266e5fc7cb005f592f4c45550dbfd5ffc48 Mon Sep 17 00:00:00 2001 From: Laurens Valk Date: Tue, 3 Dec 2024 13:27:25 +0100 Subject: [PATCH] pbio/imu: Also apply 1D correction to final 3D result. This allows model-specific correction when calibration doesn't fully cover it, which is particularly useful in hubs mounted at a slight tilt. --- lib/pbio/src/imu.c | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/lib/pbio/src/imu.c b/lib/pbio/src/imu.c index 5e4dfbcde..e66090e8f 100644 --- a/lib/pbio/src/imu.c +++ b/lib/pbio/src/imu.c @@ -657,9 +657,15 @@ static float heading_offset_3d = 0; */ float pbio_imu_get_heading(pbio_imu_heading_type_t type) { + float correction = (persistent_settings && (persistent_settings->flags & PBIO_IMU_SETTINGS_FLAGS_HEADING_CORRECTION_1D_SET)) ? + // If set, adjust by the user-specified scaling constant. + (360.0f / persistent_settings->heading_correction_1d): + // No (additional) correction. + 1.0f; + // 3D. Mapping into user frame is already accounted for in the projection. if (type == PBIO_IMU_HEADING_TYPE_3D) { - return heading_rotations * 360.0f + heading_projection - heading_offset_3d; + return (heading_rotations * 360.0f + heading_projection) * correction - heading_offset_3d; } // 1D. Map the per-axis integrated rotation to the user frame, then take @@ -667,12 +673,6 @@ float pbio_imu_get_heading(pbio_imu_heading_type_t type) { pbio_geometry_xyz_t heading_mapped; pbio_geometry_vector_map(&pbio_imu_base_orientation, &single_axis_rotation, &heading_mapped); - float correction = (persistent_settings && (persistent_settings->flags & PBIO_IMU_SETTINGS_FLAGS_HEADING_CORRECTION_1D_SET)) ? - // If set, adjust by the user-specified scaling constant. - (360.0f / persistent_settings->heading_correction_1d): - // No (additional) correction. - 1.0f; - return -heading_mapped.z * correction - heading_offset_1d; }