From 003e8ec9d9fd6400e3e26c243fc82b02aba6e348 Mon Sep 17 00:00:00 2001 From: Austin Bowen Date: Thu, 16 Apr 2026 21:21:37 -0400 Subject: [PATCH 1/3] Improve sensor delta_angle calculation --- src/common/base_classes/Sensor.cpp | 15 +++------------ 1 file changed, 3 insertions(+), 12 deletions(-) diff --git a/src/common/base_classes/Sensor.cpp b/src/common/base_classes/Sensor.cpp index cd7bb170..c6909a0f 100644 --- a/src/common/base_classes/Sensor.cpp +++ b/src/common/base_classes/Sensor.cpp @@ -29,18 +29,9 @@ float Sensor::getVelocity() { } if (Ts < min_elapsed_time) return velocity; // don't update velocity if deltaT is too small - float current_angle = 0.0f; - float prev_angle = 0.0f; - // Avoid floating point precision loss for large full_rotations - // this is likely optional - if (full_rotations == vel_full_rotations) { - current_angle = angle_prev; - prev_angle = vel_angle_prev; - } else { - current_angle = (float) full_rotations * _2PI + angle_prev; - prev_angle = (float) vel_full_rotations * _2PI + vel_angle_prev; - } - const float delta_angle = current_angle - prev_angle; + const float delta_angle = + (int32_t)(full_rotations - vel_full_rotations) * _2PI + + (angle_prev - vel_angle_prev); // floating point equality checks are bad, so instead we check that the angle change is very small if (fabsf(delta_angle) > 1e-8f) { From 6ef4b431b295004a965962980fb442771ac37583 Mon Sep 17 00:00:00 2001 From: Austin Bowen Date: Thu, 16 Apr 2026 22:33:51 -0400 Subject: [PATCH 2/3] Prefer branch over useless float ops --- src/common/base_classes/Sensor.cpp | 8 +++++--- 1 file changed, 5 insertions(+), 3 deletions(-) diff --git a/src/common/base_classes/Sensor.cpp b/src/common/base_classes/Sensor.cpp index c6909a0f..bc51acb7 100644 --- a/src/common/base_classes/Sensor.cpp +++ b/src/common/base_classes/Sensor.cpp @@ -29,9 +29,11 @@ float Sensor::getVelocity() { } if (Ts < min_elapsed_time) return velocity; // don't update velocity if deltaT is too small - const float delta_angle = - (int32_t)(full_rotations - vel_full_rotations) * _2PI - + (angle_prev - vel_angle_prev); + float delta_angle = angle_prev - vel_angle_prev; + const int32_t delta_full_rotations = full_rotations - vel_full_rotations; + if (delta_full_rotations) { + delta_angle += delta_full_rotations * _2PI; + } // floating point equality checks are bad, so instead we check that the angle change is very small if (fabsf(delta_angle) > 1e-8f) { From 9d49754caa5443a562f458dc87e8c1a6ab218d8c Mon Sep 17 00:00:00 2001 From: Austin Bowen Date: Thu, 16 Apr 2026 22:45:14 -0400 Subject: [PATCH 3/3] Add comments for angle change calculation --- src/common/base_classes/Sensor.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/common/base_classes/Sensor.cpp b/src/common/base_classes/Sensor.cpp index bc51acb7..31434e71 100644 --- a/src/common/base_classes/Sensor.cpp +++ b/src/common/base_classes/Sensor.cpp @@ -29,6 +29,8 @@ float Sensor::getVelocity() { } if (Ts < min_elapsed_time) return velocity; // don't update velocity if deltaT is too small + // Calculate change in angle. Handles `full_rotations` integer wrap-arounds, + // and avoids float precision loss issues by keeping numbers small. float delta_angle = angle_prev - vel_angle_prev; const int32_t delta_full_rotations = full_rotations - vel_full_rotations; if (delta_full_rotations) {