Copter: use a complementary filter for rangefinder data

this should reduce the impact of noise in the rangefinder data. Filter
cutoff frequency is low in loiter and higher otherwise

ArduCopter/APM_Config.h

@@ -5,7 +5,8 @@
 
 #define ARMING_DELAY_SEC 3.0f
 
-#define RANGEFINDER_WPNAV_FILT_HZ 3.0f
+#define RANGEFINDER_WPNAV_FILT_MIN_HZ 0.05
+#define RANGEFINDER_WPNAV_FILT_MAX_HZ 0.5
 
 #define LAND_DETECTOR_TRIGGER_SEC 0.5f
 

ArduCopter/Copter.h

@@ -49,6 +49,7 @@
 #include <AP_Motors/AP_Motors.h>          // AP Motors library
 #include <AP_Stats/AP_Stats.h>     // statistics library
 #include <Filter/Filter.h>             // Filter library
+#include <Filter/ComplementaryFilter.h>
 #include <AP_Airspeed/AP_Airspeed.h>        // needed for AHRS build
 #include <AP_Vehicle/AP_Vehicle.h>         // needed for AHRS build
 #include <AP_InertialNav/AP_InertialNav.h>     // ArduPilot Mega inertial navigation library
@@ -281,7 +282,8 @@ class Copter : public AP_Vehicle {
         int16_t alt_cm;     // tilt compensated altitude (in cm) from rangefinder
         float inertial_alt_cm; // inertial alt at time of last rangefinder sample
         uint32_t last_healthy_ms;
-        LowPassFilterFloat alt_cm_filt; // altitude filter
+        ComplementaryFilter alt_cm_filt; // altitude filter
+        float filter_hz;
         int16_t alt_cm_glitch_protected;    // last glitch protected altitude
         int8_t glitch_count;    // non-zero number indicates rangefinder is glitching
         uint32_t glitch_cleared_ms; // system time glitch cleared

ArduCopter/mode.h

@@ -366,6 +366,9 @@ class ModeAuto : public Mode {
     // for GCS_MAVLink to call:
     bool do_guided(const AP_Mission::Mission_Command& cmd);
 
+    // are we in a loiter?
+    bool in_loiter(void) const { return loiter_time != 0; }
+
     AP_Mission mission{
         FUNCTOR_BIND_MEMBER(&ModeAuto::start_command, bool, const AP_Mission::Mission_Command &),
         FUNCTOR_BIND_MEMBER(&ModeAuto::verify_command, bool, const AP_Mission::Mission_Command &),
@@ -1470,4 +1473,4 @@ class ModeAutorotate : public Mode {
     void warning_message(uint8_t message_n);    //Handles output messages to the terminal
 
 };
-#endif
+#endif

ArduCopter/sensors.cpp

@@ -83,9 +83,44 @@ void Copter::read_rangefinder(void)
         if (rf_state.alt_healthy) {
             if (timed_out) {
                 // reset filter if we haven't used it within the last second
-                rf_state.alt_cm_filt.reset(rf_state.alt_cm);
+                rf_state.alt_cm_filt.reset();
             } else {
-                rf_state.alt_cm_filt.apply(rf_state.alt_cm, 0.05f);
+                /*
+                  Apply a complementary filter to the rangefinder data, using the
+                  inertial altitude for the high frequency component and the
+                  rangefinder data for the low frequency component.
+
+                  When we are loitering shift the time constant of the
+                  complementary filter towards a much lower frequency to filter
+                  out even quite long period noise from the rangefinder. When not
+                  loitering shift back to a shorter time period to handle changes
+                  in terrain
+                */
+                float target_filter_hz = RANGEFINDER_WPNAV_FILT_MAX_HZ;
+                if (control_mode == Mode::Number::AUTO &&
+                    mode_auto.mission.get_current_nav_cmd().id == MAV_CMD_NAV_WAYPOINT &&
+                    mode_auto.in_loiter()) {
+                    // we are holding position at a waypoint, slew the time constant
+                    target_filter_hz = RANGEFINDER_WPNAV_FILT_MIN_HZ;
+                }
+
+                if (!is_equal(rangefinder_state.filter_hz, target_filter_hz)) {
+                    rf_state.alt_cm_filt.set_cutoff_frequency(target_filter_hz);
+                    rf_state.alt_cm_filt.reset();
+                    rangefinder_state.filter_hz = target_filter_hz;
+                }
+
+                rf_state.alt_cm_filt.apply(rf_state.alt_cm, inertial_nav.get_altitude(), AP_HAL::micros());
+
+                // temporary debug log message to look at the
+                // performance of the complementary filter
+                AP::logger().Write("RF2", "TimeUS,LF,HF,Out,FHz", "Qffff",
+                                   AP_HAL::micros64(),
+                                   rf_state.alt_cm*0.01,
+                                   inertial_nav.get_altitude()*0.01,
+                                   rf_state.alt_cm_filt.get()*0.01,
+                                   rf_state.filter_hz);
+
             }
             rf_state.last_healthy_ms = now;
         }