AP_WheelEncoder: calculate rate from last iteration

libraries/AP_WheelEncoder/AP_WheelEncoder.cpp

@@ -253,6 +253,23 @@ float AP_WheelEncoder::get_distance(uint8_t instance) const
     return get_delta_angle(instance) * _wheel_radius[instance];
 }
 
+// get the instantaneous rate in radians/second
+float AP_WheelEncoder::get_rate(uint8_t instance) const
+{
+    // for invalid instances return zero
+    if (instance >= WHEELENCODER_MAX_INSTANCES) {
+        return 0.0f;
+    }
+
+    // protect against divide by zero
+    if ((state[instance].dt_ms == 0) || _counts_per_revolution[instance] == 0) {
+        return 0;
+    }
+
+    // calculate delta_angle (in radians) per second
+    return M_2PI * (state[instance].dist_count_change / _counts_per_revolution[instance]) / (state[instance].dt_ms / 1000.0f);
+}
+
 // get the total number of sensor reading from the encoder
 uint32_t AP_WheelEncoder::get_total_count(uint8_t instance) const
 {

libraries/AP_WheelEncoder/AP_WheelEncoder.h

@@ -48,10 +48,12 @@ public:
     struct WheelEncoder_State {
         uint8_t                instance;        // the instance number of this WheelEncoder
         int32_t                distance_count;  // cumulative number of forward + backwards events received from wheel encoder
-        float                  distance;        // total distance measured
+        float                  distance;        // total distance measured in meters
         uint32_t               total_count;     // total number of successful readings from sensor (used for sensor quality calcs)
         uint32_t               error_count;     // total number of errors reading from sensor (used for sensor quality calcs)
         uint32_t               last_reading_ms; // time of last reading
+        int32_t                dist_count_change; // distance count change during the last update (used to calculating rate)
+        uint32_t               dt_ms;             // time change (in milliseconds) for the previous period (used to calculating rate)
     };
 
     // detect and initialise any available rpm sensors
@@ -84,6 +86,9 @@ public:
     // get the total distance traveled in meters
     float get_distance(uint8_t instance) const;
 
+    // get the instantaneous rate in radians/second
+    float get_rate(uint8_t instance) const;
+
     // get the total number of sensor reading from the encoder
     uint32_t get_total_count(uint8_t instance) const;
 

libraries/AP_WheelEncoder/WheelEncoder_Backend.cpp

@@ -43,3 +43,17 @@ int8_t AP_WheelEncoder_Backend::get_pin_b() const
     }
     return _frontend._pinb[_state.instance].get();
 }
+
+// copy state to front end helper function
+void AP_WheelEncoder_Backend::copy_state_to_frontend(int32_t distance_count, uint32_t total_count, uint32_t error_count, uint32_t last_reading_ms)
+{
+    // record distance and time change for calculating rate before previous state is overwritten
+    _state.dt_ms = last_reading_ms - _state.last_reading_ms;
+    _state.dist_count_change = distance_count - _state.distance_count;
+
+    // copy distance and error count so it is accessible to front end
+    _state.distance_count = distance_count;
+    _state.total_count = total_count;
+    _state.error_count = error_count;
+    _state.last_reading_ms = last_reading_ms;
+}

libraries/AP_WheelEncoder/WheelEncoder_Backend.h

@@ -37,6 +37,9 @@ protected:
     int8_t get_pin_a() const;
     int8_t get_pin_b() const;
 
+    // copy state to front end helper function
+    void copy_state_to_frontend(int32_t distance_count, uint32_t total_count, uint32_t error_count, uint32_t last_reading_ms);
+
     AP_WheelEncoder &_frontend;
     AP_WheelEncoder::WheelEncoder_State &_state;
 };

libraries/AP_WheelEncoder/WheelEncoder_Quadrature.cpp

@@ -77,10 +77,10 @@ void AP_WheelEncoder_Quadrature::update(void)
     void *irqstate = hal.scheduler->disable_interrupts_save();
 
     // copy distance and error count so it is accessible to front end
-    _state.distance_count = irq_state.distance_count;
-    _state.total_count = irq_state.total_count;
-    _state.error_count = irq_state.error_count;
-    _state.last_reading_ms = irq_state.last_reading_ms;
+    copy_state_to_frontend(irq_state.distance_count,
+                           irq_state.total_count,
+                           irq_state.error_count,
+                           irq_state.last_reading_ms);
 
     // restore interrupts
     hal.scheduler->restore_interrupts(irqstate);

libraries/AP_WheelEncoder/WheelEncoder_Quadrature.h

@@ -44,7 +44,7 @@ private:
 
     struct IrqState {
         uint8_t  phase;             // current phase of encoder (from 0 to 3)
-        int32_t  distance_count;    // distance measured by cumulative steps forward or backwards
+        int32_t  distance_count;    // distance measured by cumulative steps forward or backwards since last update
         uint32_t total_count;       // total number of successful readings from sensor (used for sensor quality calcs)
         uint32_t error_count;       // total number of errors reading from sensor (used for sensor quality calcs)
         uint32_t last_reading_ms;   // system time of last update from encoder