diff --git a/libraries/AP_ESC_Telem/AP_ESC_Telem.cpp b/libraries/AP_ESC_Telem/AP_ESC_Telem.cpp index ed803d2716..a82a1bdf90 100644 --- a/libraries/AP_ESC_Telem/AP_ESC_Telem.cpp +++ b/libraries/AP_ESC_Telem/AP_ESC_Telem.cpp @@ -34,39 +34,28 @@ AP_ESC_Telem::AP_ESC_Telem() _singleton = this; } -// return the average motor frequency in Hz for selected motors -float AP_ESC_Telem::get_average_motor_frequency_hz(uint32_t servo_channel_mask) const +// return the average motor RPM +float AP_ESC_Telem::get_average_motor_rpm(uint32_t servo_channel_mask) const { - float motor_freq = 0.0f; + float rpm_avg = 0.0f; uint8_t valid_escs = 0; - // average the rpm of each motor and convert to Hz + // average the rpm of each motor for (uint8_t i = 0; i < ESC_TELEM_MAX_ESCS; i++) { if (BIT_IS_SET(servo_channel_mask,i)) { float rpm; if (get_rpm(i, rpm)) { - motor_freq += rpm * (1.0f / 60.0f); + rpm_avg += rpm; valid_escs++; } } } + if (valid_escs > 0) { - motor_freq /= valid_escs; + rpm_avg /= valid_escs; } - return motor_freq; -} - -// return the average motor frequency in Hz for dynamic filtering -float AP_ESC_Telem::get_average_motor_frequency_hz() const -{ - return get_average_motor_frequency_hz(0xFFFFFFFF); -} - -// return the average motor rpm for selected motor channels -float AP_ESC_Telem::get_average_motor_rpm(uint32_t servo_channel_mask) const -{ - return get_average_motor_frequency_hz(servo_channel_mask)*60.f; // Hz to rpm + return rpm_avg; } // return all the motor frequencies in Hz for dynamic filtering diff --git a/libraries/AP_ESC_Telem/AP_ESC_Telem.h b/libraries/AP_ESC_Telem/AP_ESC_Telem.h index 0fde9bb44a..021f5b277f 100644 --- a/libraries/AP_ESC_Telem/AP_ESC_Telem.h +++ b/libraries/AP_ESC_Telem/AP_ESC_Telem.h @@ -27,6 +27,12 @@ public: // get an individual ESC's raw rpm if available bool get_raw_rpm(uint8_t esc_index, float& rpm) const; + // return the average motor RPM + float get_average_motor_rpm(uint32_t servo_channel_mask) const; + + // return the average motor RPM + float get_average_motor_rpm() const { return get_average_motor_rpm(0xFFFFFFFF); } + // get an individual ESC's temperature in centi-degrees if available, returns true on success bool get_temperature(uint8_t esc_index, int16_t& temp) const; @@ -45,14 +51,11 @@ public: // get an individual ESC's consumption in milli-Ampere.hour if available, returns true on success bool get_consumption_mah(uint8_t esc_index, float& consumption_mah) const; - // return the average selected motor rpm - float get_average_motor_rpm(uint32_t esc_mask) const; + // return the average motor frequency in Hz for dynamic filtering + float get_average_motor_frequency_hz(uint32_t servo_channel_mask) const { return get_average_motor_rpm(servo_channel_mask) * (1.0f / 60.0f); }; // return the average motor frequency in Hz for dynamic filtering - float get_average_motor_frequency_hz(uint32_t esc_mask) const; - - // return the average motor frequency in Hz for dynamic filtering - float get_average_motor_frequency_hz() const; + float get_average_motor_frequency_hz() const { return get_average_motor_frequency_hz(0xFFFFFFFF); } // return all of the motor frequencies in Hz for dynamic filtering uint8_t get_motor_frequencies_hz(uint8_t nfreqs, float* freqs) const;