AP_ESC_Telem: added methods to get average rpm data by motor mask

libraries/AP_ESC_Telem/AP_ESC_Telem.cpp

@@ -34,20 +34,22 @@ AP_ESC_Telem::AP_ESC_Telem()
     _singleton = this;
 }
 
-// return the average motor frequency in Hz for dynamic filtering
-float AP_ESC_Telem::get_average_motor_frequency_hz() const
+// 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
 {
     float motor_freq = 0.0f;
     uint8_t valid_escs = 0;
 
     // average the rpm of each motor and convert to Hz
     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);
                 valid_escs++;
             }
         }
+    }
     if (valid_escs > 0) {
         motor_freq /= valid_escs;
     }
@@ -55,6 +57,18 @@ float AP_ESC_Telem::get_average_motor_frequency_hz() const
     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 all the motor frequencies in Hz for dynamic filtering
 uint8_t AP_ESC_Telem::get_motor_frequencies_hz(uint8_t nfreqs, float* freqs) const
 {

libraries/AP_ESC_Telem/AP_ESC_Telem.h

@@ -45,6 +45,12 @@ 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 esc_mask) const;
+
     // return the average motor frequency in Hz for dynamic filtering
     float get_average_motor_frequency_hz() const;