diff --git a/libraries/AP_WheelEncoder/AP_WheelEncoder.cpp b/libraries/AP_WheelEncoder/AP_WheelEncoder.cpp index eced728481..c934e23117 100644 --- a/libraries/AP_WheelEncoder/AP_WheelEncoder.cpp +++ b/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 { diff --git a/libraries/AP_WheelEncoder/AP_WheelEncoder.h b/libraries/AP_WheelEncoder/AP_WheelEncoder.h index 916ec87de5..c07d3bc3b5 100644 --- a/libraries/AP_WheelEncoder/AP_WheelEncoder.h +++ b/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; diff --git a/libraries/AP_WheelEncoder/WheelEncoder_Backend.cpp b/libraries/AP_WheelEncoder/WheelEncoder_Backend.cpp index 75017084a0..35a20c62c0 100644 --- a/libraries/AP_WheelEncoder/WheelEncoder_Backend.cpp +++ b/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; +} diff --git a/libraries/AP_WheelEncoder/WheelEncoder_Backend.h b/libraries/AP_WheelEncoder/WheelEncoder_Backend.h index b6ea9cd1e2..f8b47d482d 100644 --- a/libraries/AP_WheelEncoder/WheelEncoder_Backend.h +++ b/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; }; diff --git a/libraries/AP_WheelEncoder/WheelEncoder_Quadrature.cpp b/libraries/AP_WheelEncoder/WheelEncoder_Quadrature.cpp index c27e3a2db1..0c8a3eb481 100644 --- a/libraries/AP_WheelEncoder/WheelEncoder_Quadrature.cpp +++ b/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); diff --git a/libraries/AP_WheelEncoder/WheelEncoder_Quadrature.h b/libraries/AP_WheelEncoder/WheelEncoder_Quadrature.h index a29c2e5e57..1e3849623e 100644 --- a/libraries/AP_WheelEncoder/WheelEncoder_Quadrature.h +++ b/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