Copter: Add support for terrain altitude time constant

ArduCopter/Copter.cpp

@@ -137,6 +137,8 @@ const AP_Scheduler::Task Copter::scheduler_tasks[] = {
     FAST_TASK(update_home_from_EKF),
     // check if we've landed or crashed
     FAST_TASK(update_land_and_crash_detectors),
+    // surface tracking update
+    FAST_TASK(update_rangefinder_terrain_offset),
 #if HAL_MOUNT_ENABLED
     // camera mount's fast update
     FAST_TASK_CLASS(AP_Mount, &copter.camera_mount, update_fast),

ArduCopter/Copter.h

@@ -262,6 +262,7 @@ private:
         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
+        float terrain_offset_cm;    // filtered terrain offset (e.g. terrain's height above EKF origin)
     } rangefinder_state, rangefinder_up_state;
 
     // return rangefinder height interpolated using inertial altitude
@@ -269,6 +270,7 @@ private:
 
     class SurfaceTracking {
     public:
+
         // update_surface_offset - manages the vertical offset of the position controller to follow the
         //   measured ground or ceiling level measured using the range finder.
         void update_surface_offset();
@@ -900,6 +902,7 @@ private:
     void read_rangefinder(void);
     bool rangefinder_alt_ok() const;
     bool rangefinder_up_ok() const;
+    void update_rangefinder_terrain_offset();
     void update_optical_flow(void);
 
     // takeoff_check.cpp

ArduCopter/Parameters.cpp

@@ -1200,6 +1200,14 @@ const AP_Param::GroupInfo ParametersG2::var_info2[] = {
     // @User: Standard
     AP_GROUPINFO("PLDP_SPEED_DN", 4, ParametersG2, pldp_descent_speed_ms, 0.0),
 
+    // @Param: SURFTRAK_TC
+    // @DisplayName: Surface Tracking Filter Time Constant
+    // @Description: Time to achieve 63.2% of the surface altitude measurement change.  If 0 filtering is disabled
+    // @Units: s
+    // @Range: 0 5
+    // @User: Advanced
+    AP_GROUPINFO("SURFTRAK_TC", 5, ParametersG2, surftrak_tc, 1.0),
+
     // ID 62 is reserved for the AP_SUBGROUPEXTENSION
 
     AP_GROUPEND

ArduCopter/Parameters.h

@@ -674,6 +674,7 @@ public:
     AP_Int8                 surftrak_mode;
     AP_Int8                 failsafe_dr_enable;
     AP_Int16                failsafe_dr_timeout;
+    AP_Float                surftrak_tc;
 
     // ramp time of throttle during take-off
     AP_Float takeoff_throttle_slew_time;

ArduCopter/sensors.cpp

@@ -74,6 +74,7 @@ void Copter::read_rangefinder(void)
             // clear glitch and record time so consumers (i.e. surface tracking) can reset their target altitudes
             rf_state.glitch_count = 0;
             rf_state.alt_cm_glitch_protected = rf_state.alt_cm;
+            rf_state.terrain_offset_cm = rf_state.inertial_alt_cm - rf_state.alt_cm;
             rf_state.glitch_cleared_ms = AP_HAL::millis();
         }
 
@@ -84,6 +85,7 @@ void Copter::read_rangefinder(void)
             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.terrain_offset_cm = rf_state.inertial_alt_cm - rf_state.alt_cm;
             } else {
                 rf_state.alt_cm_filt.apply(rf_state.alt_cm, 0.05f);
             }
@@ -91,17 +93,13 @@ void Copter::read_rangefinder(void)
         }
 
         // send downward facing lidar altitude and health to the libraries that require it
+#if HAL_PROXIMITY_ENABLED
         if (rf_orient == ROTATION_PITCH_270) {
             if (rangefinder_state.alt_healthy || timed_out) {
-                wp_nav->set_rangefinder_alt(rangefinder_state.enabled, rangefinder_state.alt_healthy, rangefinder_state.alt_cm_filt.get());
-#if MODE_CIRCLE_ENABLED
-                circle_nav->set_rangefinder_alt(rangefinder_state.enabled && wp_nav->rangefinder_used(), rangefinder_state.alt_healthy, rangefinder_state.alt_cm_filt.get());
-#endif
-#if HAL_PROXIMITY_ENABLED
                 g2.proximity.set_rangefinder_alt(rangefinder_state.enabled, rangefinder_state.alt_healthy, rangefinder_state.alt_cm_filt.get());
-#endif
             }
         }
+#endif
     }
 
 #else
@@ -129,6 +127,24 @@ bool Copter::rangefinder_up_ok() const
     return (rangefinder_up_state.enabled && rangefinder_up_state.alt_healthy);
 }
 
+// update rangefinder based terrain offset
+// terrain offset is the terrain's height above the EKF origin
+void Copter::update_rangefinder_terrain_offset()
+{
+    float terrain_offset_cm = rangefinder_state.inertial_alt_cm - rangefinder_state.alt_cm_glitch_protected;
+    rangefinder_state.terrain_offset_cm += (terrain_offset_cm - rangefinder_state.terrain_offset_cm) * (copter.G_Dt / MAX(copter.g2.surftrak_tc, copter.G_Dt));
+
+    terrain_offset_cm = rangefinder_up_state.inertial_alt_cm + rangefinder_up_state.alt_cm_glitch_protected;
+    rangefinder_up_state.terrain_offset_cm += (terrain_offset_cm - rangefinder_up_state.terrain_offset_cm) * (copter.G_Dt / MAX(copter.g2.surftrak_tc, copter.G_Dt));
+
+    if (rangefinder_state.alt_healthy || (AP_HAL::millis() - rangefinder_state.last_healthy_ms > RANGEFINDER_TIMEOUT_MS)) {
+        wp_nav->set_rangefinder_terrain_offset(rangefinder_state.enabled, rangefinder_state.alt_healthy, rangefinder_state.terrain_offset_cm);
+#if MODE_CIRCLE_ENABLED
+        circle_nav->set_rangefinder_terrain_offset(rangefinder_state.enabled && wp_nav->rangefinder_used(), rangefinder_state.alt_healthy, rangefinder_state.terrain_offset_cm);
+#endif
+    }
+}
+
 /*
   get inertially interpolated rangefinder height. Inertial height is
   recorded whenever we update the rangefinder height, then we use the

ArduCopter/surface_tracking.cpp

@@ -16,11 +16,9 @@ void Copter::SurfaceTracking::update_surface_offset()
         // calculate surfaces height above the EKF origin
         // e.g. if vehicle is 10m above the EKF origin and rangefinder reports alt of 3m.  curr_surface_alt_above_origin_cm is 7m (or 700cm)
         RangeFinderState &rf_state = (surface == Surface::GROUND) ? copter.rangefinder_state : copter.rangefinder_up_state;
-        const float dir = (surface == Surface::GROUND) ? 1.0f : -1.0f;
-        const float curr_surface_alt_above_origin_cm = rf_state.inertial_alt_cm - dir * rf_state.alt_cm_filt.get();
 
         // update position controller target offset to the surface's alt above the EKF origin
-        copter.pos_control->set_pos_offset_target_z_cm(curr_surface_alt_above_origin_cm);
+        copter.pos_control->set_pos_offset_target_z_cm(rf_state.terrain_offset_cm);
         last_update_ms = now_ms;
         valid_for_logging = true;
 
@@ -30,7 +28,7 @@ void Copter::SurfaceTracking::update_surface_offset()
         if (timeout ||
             reset_target ||
             (last_glitch_cleared_ms != rf_state.glitch_cleared_ms)) {
-            copter.pos_control->set_pos_offset_z_cm(curr_surface_alt_above_origin_cm);
+            copter.pos_control->set_pos_offset_z_cm(rf_state.terrain_offset_cm);
             reset_target = false;
             last_glitch_cleared_ms = rf_state.glitch_cleared_ms;
         }