Plane: move adjust_altitude_target functianlity to per mode functions

ArduPlane/altitude.cpp

@@ -25,17 +25,14 @@
  */
 void Plane::adjust_altitude_target()
 {
-    Location target_location;
+    control_mode->update_target_altitude();
+}
 
-    if (control_mode == &mode_fbwb ||
+// to be moved to mode_guided.cpp in a future PR, kept here to minimise diff
-        control_mode == &mode_cruise) {
+void ModeGuided::update_target_altitude()
-        return;
+{
-    }
-    if ((control_mode == &mode_loiter) && plane.stick_mixing_enabled() && (plane.g2.flight_options & FlightOptions::ENABLE_LOITER_ALT_CONTROL)) {
-       return;
-    }
 #if OFFBOARD_GUIDED == ENABLED
-    if (control_mode == &mode_guided && ((plane.guided_state.target_alt_time_ms != 0) || plane.guided_state.target_alt > -0.001 )) { // target_alt now defaults to -1, and _time_ms defaults to zero.
+    if (((plane.guided_state.target_alt_time_ms != 0) || plane.guided_state.target_alt > -0.001 )) { // target_alt now defaults to -1, and _time_ms defaults to zero.
         // offboard altitude demanded
         uint32_t now = AP_HAL::millis();
         float delta = 1e-3f * (now - plane.guided_state.target_alt_time_ms);
@@ -53,11 +50,20 @@ void Plane::adjust_altitude_target()
         }
         plane.guided_state.last_target_alt = temp.alt;
         plane.set_target_altitude_location(temp);
+        plane.altitude_error_cm = plane.calc_altitude_error_cm();
     } else 
 #endif // OFFBOARD_GUIDED == ENABLED
-      if (control_mode->update_target_altitude()) {
+        {
-        // handled in mode specific code
+        Mode::update_target_altitude();
-    } else if (plane.landing.is_flaring()) {
+    }
+}
+
+// to be moved to mode.cpp in a future PR, kept here to minimise diff
+void Mode::update_target_altitude()
+{
+    Location target_location;
+
+    if (plane.landing.is_flaring()) {
         // during a landing flare, use TECS_LAND_SINK as a target sink
         // rate, and ignores the target altitude
         plane.set_target_altitude_location(plane.next_WP_loc);

ArduPlane/mode.h

@@ -115,7 +115,7 @@ public:
     virtual bool does_auto_throttle() const { return false; }
 
     // method for mode specific target altitude profiles
-    virtual bool update_target_altitude() { return false; }
+    virtual void update_target_altitude();
 
     // handle a guided target request from GCS
     virtual bool handle_guided_request(Location target_loc) { return false; }
@@ -226,6 +226,8 @@ public:
 
     void set_radius_and_direction(const float radius, const bool direction_is_ccw);
 
+    void update_target_altitude() override;
+
 protected:
 
     bool _enter() override;
@@ -278,6 +280,8 @@ public:
 
     bool allows_terrain_disable() const override { return true; }
 
+    void update_target_altitude() override;
+
 protected:
 
     bool _enter() override;
@@ -422,6 +426,8 @@ public:
 
     bool does_auto_throttle() const override { return true; }
 
+    void update_target_altitude() override {};
+
 protected:
 
     bool _enter() override;
@@ -448,6 +454,8 @@ public:
 
     bool does_auto_throttle() const override { return true; }
 
+    void update_target_altitude() override {};
+
 protected:
 
     bool _enter() override;
@@ -596,7 +604,7 @@ public:
 
     bool does_auto_throttle() const override { return true; }
 
-    bool update_target_altitude() override;
+    void update_target_altitude() override;
 
     bool allows_throttle_nudging() const override;
 

ArduPlane/mode_loiter.cpp

@@ -110,3 +110,10 @@ void ModeLoiter::navigate()
     plane.update_loiter(0);
 }
 
+void ModeLoiter::update_target_altitude()
+{
+    if (plane.stick_mixing_enabled() && (plane.g2.flight_options & FlightOptions::ENABLE_LOITER_ALT_CONTROL)) {
+        return;
+    }
+    Mode::update_target_altitude();
+}

ArduPlane/mode_qrtl.cpp

@@ -132,13 +132,14 @@ void ModeQRTL::run()
 /*
   update target altitude for QRTL profile
  */
-bool ModeQRTL::update_target_altitude()
+void ModeQRTL::update_target_altitude()
 {
     /*
       update height target in approach
      */
     if ((submode != SubMode::RTL) || (plane.quadplane.poscontrol.get_state() != QuadPlane::QPOS_APPROACH)) {
-        return false;
+        Mode::update_target_altitude();
+        return;
     }
 
     /*
@@ -158,7 +159,7 @@ bool ModeQRTL::update_target_altitude()
     Location loc = plane.next_WP_loc;
     loc.alt += alt*100;
     plane.set_target_altitude_location(loc);
-    return true;
+    plane.altitude_error_cm = plane.calc_altitude_error_cm();
 }
 
 // only nudge during approach