AP_RPM: Add Signal Quality synthetic method.

libraries/AP_RPM/AP_RPM.h

@@ -45,6 +45,7 @@ public:
         uint8_t                instance;        // the instance number of this RPM
         float                  rate_rpm;        // measured rate in revs per minute
         uint32_t               last_reading_ms; // time of last reading
+        float                  signal_quality;  // synthetic quality metric 
     };
 
     // parameters for each instance
@@ -75,6 +76,13 @@ public:
         return state[instance].rate_rpm;
     }
 
+    /*
+      return signal quality for a sensor.
+     */
+    float get_signal_quality(uint8_t instance) const {
+        return state[instance].signal_quality;
+    }
+
     bool healthy(uint8_t instance) const;
 
     bool enabled(uint8_t instance) const;

libraries/AP_RPM/RPM_PX4_PWM.cpp

@@ -81,7 +81,8 @@ void AP_RPM_PX4_PWM::update(void)
     struct pwm_input_s pwm;
     uint16_t count = 0;
     const float scaling = ap_rpm._scaling[state.instance];
-    float sum = 0;
+    float maximum = ap_rpm._maximum[state.instance];
+    float quality = 0;
 
     while (::read(_fd, &pwm, sizeof(pwm)) == sizeof(pwm)) {
         // the px4 pwm_input driver reports the period in microseconds
@@ -89,11 +90,20 @@ void AP_RPM_PX4_PWM::update(void)
             continue;
         }
         float rpm = scaling * (1.0e6f * 60) / pwm.period;
-        float maximum = ap_rpm._maximum[state.instance];
+        float filter_value = signal_quality_filter.get();
         if (maximum <= 0 || rpm <= maximum) {
-            sum += rpm;
+            state.rate_rpm = signal_quality_filter.apply(rpm);
+            if (is_zero(filter_value)){
+                quality = 0;
+            } else {
+                quality = 1 - constrain_float((fabsf(rpm-filter_value))/filter_value, 0.0, 1.0);
+                quality = powf(quality, 2.0);
+            }
             count++;
+        } else {
+            quality = 0;
         }
+
 #if PWM_LOGGING
         if (_logfd != -1) {
             dprintf(_logfd, "%u %u %u\n",
@@ -102,10 +112,11 @@ void AP_RPM_PX4_PWM::update(void)
                     (unsigned)pwm.pulse_width);
         }
 #endif
+
+        state.signal_quality = (0.1 * quality) + (0.9 * state.signal_quality);      // simple LPF
     }
 
     if (count != 0) {
-        state.rate_rpm = sum / count;
         state.last_reading_ms = AP_HAL::millis();
     }
 }

libraries/AP_RPM/RPM_PX4_PWM.h

@@ -19,6 +19,8 @@
 
 #include "AP_RPM.h"
 #include "RPM_Backend.h"
+#include <Filter/Filter.h>
+#include <AP_Math/AP_Math.h>
 
 class AP_RPM_PX4_PWM : public AP_RPM_Backend
 {
@@ -36,6 +38,8 @@ private:
     int _fd = -1;
     int _logfd = -1;
     uint64_t _last_timestamp = 0;
+
+    ModeFilterFloat_Size5 signal_quality_filter {3};
 };
 
 #endif // AP_RPM_PX4_PWM_H