AP_Terrain: added terrain reference adjustment

this restores the terrain adjustment functionality removed in #19946,
but without the problematic approach of always using home (which can
be moved in flight) and with a TERR_OFS_MAX parameter to limit the
amount of adjustment

libraries/AP_Terrain/AP_Terrain.cpp

@@ -67,6 +67,14 @@ const AP_Param::GroupInfo AP_Terrain::var_info[] = {
     // @Range: 0.05 50000
     // @User: Advanced
     AP_GROUPINFO("MARGIN",   3, AP_Terrain, margin, 0.05),
+
+    // @Param: OFS_MAX
+    // @DisplayName: Maximum reference offset
+    // @Description: The maximum adjustment of terrain altitude based on the assumption that the vehicle is on the ground when it is armed. When the vehicle is armed the location of the vehicle is recorded, and when terrain data is available for that location a height adjustment for terrain data is calculated that aligns the terrain height at that location with the altitude recorded at arming. This height adjustment is applied to all terrain data. This parameter clamps the amount of adjustment. A value of zero disables the use of terrain height adjustment.
+    // @Units: m
+    // @Range: 0 50
+    // @User: Advanced
+    AP_GROUPINFO("OFS_MAX",  4, AP_Terrain, offset_max, 15),
     
     AP_GROUPEND
 };
@@ -95,7 +103,7 @@ AP_Terrain::AP_Terrain() :
 
   This function costs about 20 microseconds on Pixhawk
  */
-bool AP_Terrain::height_amsl(const Location &loc, float &height)
+bool AP_Terrain::height_amsl(const Location &loc, float &height, bool corrected)
 {
     if (!allocate()) {
         return false;
@@ -107,6 +115,9 @@ bool AP_Terrain::height_amsl(const Location &loc, float &height)
     if (loc.lat == home_loc.lat &&
         loc.lng == home_loc.lng) {
         height = home_height;
+        if (corrected && have_reference_offset) {
+            height += reference_offset;
+        }
         return true;
     }
 
@@ -157,6 +168,10 @@ bool AP_Terrain::height_amsl(const Location &loc, float &height)
         home_loc = loc;
     }
 
+    if (corrected && have_reference_offset) {
+        height += reference_offset;
+    }
+    
     return true;
 }
 
@@ -404,6 +419,78 @@ bool AP_Terrain::allocate(void)
     return true;
 }
 
+/*
+  setup a reference location for terrain adjustment. This should
+  be called when the vehicle is definately on the ground
+*/
+void AP_Terrain::set_reference_location(void)
+{
+    const auto &ahrs = AP::ahrs();
+
+    // check we have absolute position
+    nav_filter_status status;
+    if (!ahrs.get_filter_status(status) ||
+        !status.flags.vert_pos ||
+        !status.flags.horiz_pos_abs ||
+        !status.flags.attitude) {
+        return;
+    }
+
+    // check we have a small 3D velocity
+    Vector3f vel;
+    if (!ahrs.get_velocity_NED(vel) ||
+        vel.length() > 3) {
+        return;
+    }
+
+    have_reference_offset = false;
+    have_reference_loc = ahrs.get_location(reference_loc);
+
+    update_reference_offset();
+}
+
+/*
+  get the offset between terrain height and reference alt at the
+  reference location
+ */
+void AP_Terrain::update_reference_offset(void)
+{
+    // TERR_OFS_MAX of zero means no adjustment
+    if (!is_positive(offset_max)) {
+        have_reference_offset = false;
+        return;
+    }
+
+    // allow for change to TERRAIN_OFS_MAX while flying
+    if (have_reference_offset) {
+        reference_offset = constrain_float(reference_offset, -offset_max, offset_max);
+        return;
+    }
+
+    if (!have_reference_loc) {
+        // no reference available yet
+        return;
+    }
+
+    // calculate adjustment
+    float height;
+    if (!height_amsl(reference_loc, height)) {
+        return;
+    }
+    int32_t alt_cm;
+    if (!reference_loc.get_alt_cm(Location::AltFrame::ABSOLUTE, alt_cm)) {
+        return;
+    }
+    float adjustment = alt_cm*0.01 - height;
+    reference_offset = constrain_float(adjustment, -offset_max, offset_max);
+    if (fabsf(adjustment) > offset_max.get()+0.5) {
+        GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "Terrain: clamping offset %.0f to %.0f",
+                      adjustment, reference_offset);
+    }
+    have_reference_offset = true;
+}
+
+
 namespace AP {
 
 AP_Terrain *terrain()

libraries/AP_Terrain/AP_Terrain.h

@@ -122,7 +122,7 @@ public:
 
       return false if not available
      */
-    bool height_amsl(const Location &loc, float &height);
+    bool height_amsl(const Location &loc, float &height, bool corrected = true);
 
     /* 
        find difference between home terrain height and the terrain
@@ -189,6 +189,12 @@ public:
      */
     bool init_failed() const { return memory_alloc_failed; }
 
+    /*
+      setup a reference location for terrain adjustment. This should
+      be called when the vehicle is definately on the ground
+     */
+    void set_reference_location(void);
+
 private:
     // allocate the terrain subsystem data
     bool allocate(void);
@@ -344,12 +350,18 @@ private:
      */
     void update_rally_data(void);
 
+    /*
+      calculate reference offset if needed
+     */
+    void update_reference_offset(void);
+
 
     // parameters
     AP_Int8  enable;
     AP_Float margin;
     AP_Int16 grid_spacing; // meters between grid points
     AP_Int16 options; // option bits
+    AP_Float offset_max;
 
     enum class Options {
         DisableDownload = (1U<<0),
@@ -396,6 +408,15 @@ private:
     float home_height;
     Location home_loc;
 
+    // reference position for terrain adjustment, set at arming
+    bool have_reference_loc;
+    Location reference_loc;
+
+    // calculated reference offset
+    bool have_reference_offset;
+    float reference_offset;
+
+
     // cache the last terrain height (AMSL) of the AHRS current
     // location. This is used for extrapolation when terrain data is
     // temporarily unavailable

libraries/AP_Terrain/TerrainIO.cpp

@@ -351,6 +351,8 @@ void AP_Terrain::io_timer(void)
         return;
     }
 
+    update_reference_offset();
+
     switch (disk_io_state) {
     case DiskIoIdle:
     case DiskIoDoneRead: