From 83c62761f315ac65950a19a12fb9e75296cd224e Mon Sep 17 00:00:00 2001 From: copper280z Date: Sat, 22 Feb 2025 12:22:51 -0500 Subject: [PATCH 1/2] change velocity calc to not update unless an angle change is observed. --- src/common/base_classes/Sensor.cpp | 27 +++++++++++++++++++++++---- 1 file changed, 23 insertions(+), 4 deletions(-) diff --git a/src/common/base_classes/Sensor.cpp b/src/common/base_classes/Sensor.cpp index db17e92e..32d11d8a 100644 --- a/src/common/base_classes/Sensor.cpp +++ b/src/common/base_classes/Sensor.cpp @@ -19,6 +19,7 @@ void Sensor::update() { /** get current angular velocity (rad/s) */ float Sensor::getVelocity() { // calculate sample time + // if timestamps were unsigned, we could get rid of this section, unsigned overflow handles it correctly float Ts = (angle_prev_ts - vel_angle_prev_ts)*1e-6f; if (Ts < 0.0f) { // handle micros() overflow - we need to reset vel_angle_prev_ts vel_angle_prev = angle_prev; @@ -28,10 +29,28 @@ float Sensor::getVelocity() { } if (Ts < min_elapsed_time) return velocity; // don't update velocity if deltaT is too small - velocity = ( (float)(full_rotations - vel_full_rotations)*_2PI + (angle_prev - vel_angle_prev) ) / Ts; - vel_angle_prev = angle_prev; - vel_full_rotations = full_rotations; - vel_angle_prev_ts = angle_prev_ts; + float current_angle = 0; + float prev_angle = 0; + // 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; + + // floating point equality checks are bad, so instead we check that the angle change is very small + if (fabsf(delta_angle) < 1e-8f) { + velocity = delta_angle / Ts; + + vel_angle_prev = angle_prev; + vel_full_rotations = full_rotations; + vel_angle_prev_ts = angle_prev_ts; + } + return velocity; } From 77c38a4ef6add80362fa5a46f1386e0e8d19e68e Mon Sep 17 00:00:00 2001 From: copper280z Date: Wed, 26 Feb 2025 07:36:16 -0500 Subject: [PATCH 2/2] angle GREATER than epsilon --- src/common/base_classes/Sensor.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/common/base_classes/Sensor.cpp b/src/common/base_classes/Sensor.cpp index 32d11d8a..cd7bb170 100644 --- a/src/common/base_classes/Sensor.cpp +++ b/src/common/base_classes/Sensor.cpp @@ -29,8 +29,8 @@ float Sensor::getVelocity() { } if (Ts < min_elapsed_time) return velocity; // don't update velocity if deltaT is too small - float current_angle = 0; - float prev_angle = 0; + 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) { @@ -43,7 +43,7 @@ float Sensor::getVelocity() { const float delta_angle = current_angle - prev_angle; // floating point equality checks are bad, so instead we check that the angle change is very small - if (fabsf(delta_angle) < 1e-8f) { + if (fabsf(delta_angle) > 1e-8f) { velocity = delta_angle / Ts; vel_angle_prev = angle_prev;