Merge pull request #453 from Copper280z/velocity_rework

Velocity Calculation rework

Author: runger1101001

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.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;
+
+    // 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;
 }