AP_WheelEncoder: last_reading is last update time instead of system time

libraries/AP_WheelEncoder/WheelEncoder_Quadrature.cpp

@@ -77,7 +77,7 @@ void AP_WheelEncoder_Quadrature::update(void)
     _state.distance_count = irq_state[instance].distance_count;
     _state.total_count = irq_state[instance].total_count;
     _state.error_count = irq_state[instance].error_count;
-    _state.last_reading_ms = AP_HAL::millis();
+    _state.last_reading_ms = irq_state[instance].last_reading_ms;
 
     // restore interrupts
     irqrestore(istate);
@@ -190,6 +190,9 @@ void AP_WheelEncoder_Quadrature::irq_handler(uint8_t instance, bool pin_a)
 
     // update distance and error counts
     update_phase_and_error_count(pin_a_high, pin_b_high, irq_state[instance].phase, irq_state[instance].distance_count, irq_state[instance].total_count, irq_state[instance].error_count);
+
+    // record update time
+    irq_state[instance].last_reading_ms = AP_HAL::millis();
 }
 
 #endif // CONFIG_HAL_BOARD

libraries/AP_WheelEncoder/WheelEncoder_Quadrature.h

@@ -53,6 +53,7 @@ private:
         int32_t  distance_count;    // distance measured by cumulative steps forward or backwards
         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
     };
     static struct IrqState irq_state[WHEELENCODER_MAX_INSTANCES];