Plane: Refactor to request relative altitudes from AHRS

Removes the need for plane to do the math for finding the relative height.

Also caches the value at the same time we update current_loc, which is a
non behaviour change as that was the only time you could see a change in
the relative height propegate through the system anyways

ArduPlane/ArduPlane.cpp

@@ -450,6 +450,8 @@ void Plane::update_GPS_50Hz(void)
 {
     // get position from AHRS
     have_position = ahrs.get_position(current_loc);
+    ahrs.get_relative_position_D_home(relative_altitude);
+    relative_altitude *= -1.0f;
 
     static uint32_t last_gps_reading[GPS_MAX_INSTANCES];
     gps.update();
@@ -1037,7 +1039,7 @@ float Plane::tecs_hgt_afe(void)
     } else {
         // when in normal flight we pass the hgt_afe as relative
         // altitude to home
-        hgt_afe = relative_altitude();
+        hgt_afe = relative_altitude;
     }
     return hgt_afe;
 }

ArduPlane/Attitude.cpp

@@ -225,7 +225,7 @@ void Plane::stabilize_yaw(float speed_scaler)
         // otherwise use ground steering when no input control and we
         // are below the GROUND_STEER_ALT
         steering_control.ground_steering = (channel_roll->get_control_in() == 0 && 
-                                            fabsf(relative_altitude()) < g.ground_steer_alt);
+                                            fabsf(relative_altitude) < g.ground_steer_alt);
         if (landing.is_on_approach()) {
             // don't use ground steering on landing approach
             steering_control.ground_steering = false;
@@ -390,7 +390,7 @@ void Plane::stabilize()
       see if we should zero the attitude controller integrators. 
      */
     if (channel_throttle->get_control_in() == 0 &&
-        relative_altitude_abs_cm() < 500 && 
+        fabsf(relative_altitude) < 5.0f && 
         fabsf(barometer.get_climb_rate()) < 0.5f &&
         gps.ground_speed() < 3) {
         // we are low, with no climb rate, and zero throttle, and very

ArduPlane/GCS_Mavlink.cpp

@@ -165,7 +165,7 @@ void Plane::send_location(mavlink_channel_t chan)
         current_loc.lat,                // in 1E7 degrees
         current_loc.lng,                // in 1E7 degrees
         current_loc.alt * 10UL,         // millimeters above sea level
-        relative_altitude() * 1.0e3f,    // millimeters above ground
+        relative_altitude * 1.0e3f,    // millimeters above ground
         vel.x * 100,  // X speed cm/s (+ve North)
         vel.y * 100,  // Y speed cm/s (+ve East)
         vel.z * -100, // Z speed cm/s (+ve up)

ArduPlane/Plane.h

@@ -700,6 +700,8 @@ private:
 #endif
     } target_altitude {};
 
+    float relative_altitude = 0.0f;
+
     // INS variables
     // The main loop execution time.  Seconds
     // This is the time between calls to the DCM algorithm and is the Integration time for the gyros.
@@ -829,8 +831,6 @@ private:
     void adjust_altitude_target();
     void setup_glide_slope(void);
     int32_t get_RTL_altitude();
-    float relative_altitude(void);
-    int32_t relative_altitude_abs_cm(void);
     float relative_ground_altitude(bool use_rangefinder_if_available);
     void set_target_altitude_current(void);
     void set_target_altitude_current_adjusted(void);

ArduPlane/altitude.cpp

@@ -115,23 +115,6 @@ int32_t Plane::get_RTL_altitude()
     return g.RTL_altitude_cm + home.alt;
 }
 
-
-/*
-  return relative altitude in meters (relative to home)
- */
-float Plane::relative_altitude(void)
-{
-    return (current_loc.alt - home.alt) * 0.01f;
-}
-
-/*
-  return relative altitude in centimeters, absolute value
- */
-int32_t Plane::relative_altitude_abs_cm(void)
-{
-    return labs(current_loc.alt - home.alt);
-}
-
 /*
   return relative altitude in meters (relative to terrain, if available,
   or home otherwise)
@@ -150,7 +133,7 @@ float Plane::relative_ground_altitude(bool use_rangefinder_if_available)
         return altitude;
     }
 #endif
-    return relative_altitude();
+    return relative_altitude;
 }
 
 /*
@@ -456,7 +439,7 @@ int32_t Plane::adjusted_altitude_cm(void)
  */
 int32_t Plane::adjusted_relative_altitude_cm(void)
 {
-    return adjusted_altitude_cm() - home.alt;
+    return (relative_altitude - mission_alt_offset())*100;
 }
 
 
@@ -631,7 +614,7 @@ void Plane::rangefinder_height_update(void)
     if (rangefinder_state.in_range) {
         // base correction is the difference between baro altitude and
         // rangefinder estimate
-        float correction = relative_altitude() - rangefinder_state.height_estimate;
+        float correction = relative_altitude - rangefinder_state.height_estimate;
 
 #if AP_TERRAIN_AVAILABLE
         // if we are terrain following then correction is based on terrain data

ArduPlane/commands_logic.cpp

@@ -399,7 +399,7 @@ void Plane::do_land(const AP_Mission::Mission_Command& cmd)
     // zero rangefinder state, start to accumulate good samples now
     memset(&rangefinder_state, 0, sizeof(rangefinder_state));
 
-    landing.do_land(cmd, relative_altitude());
+    landing.do_land(cmd, relative_altitude);
 
 #if GEOFENCE_ENABLED == ENABLED 
     if (g.fence_autoenable == 1) {

ArduPlane/sensors.cpp

@@ -39,7 +39,7 @@ void Plane::read_rangefinder(void)
             height = height_above_target();
         } else {
             // otherwise just use the best available baro estimate above home.
-            height = relative_altitude();
+            height = relative_altitude;
         }
         rangefinder.set_estimated_terrain_height(height);
     }

ArduPlane/servos.cpp

@@ -101,7 +101,7 @@ bool Plane::suppress_throttle(void)
         return true;
     }
     
-    if (relative_altitude_abs_cm() >= 1000) {
+    if (fabsf(relative_altitude) >= 10.0f) {
         // we're more than 10m from the home altitude
         throttle_suppressed = false;
         return false;