ArduPlane: allow FBWB alt control option in LOITER

Co-authored-by: Henry Wurzburg <hwurzburg@yahoo.com>

ArduPlane/Attitude.cpp

@@ -237,6 +237,11 @@ void Plane::stabilize_stick_mixing_direct()
     aileron = channel_roll->stick_mixing(aileron);
     SRV_Channels::set_output_scaled(SRV_Channel::k_aileron, aileron);
 
+    if ((control_mode == &mode_loiter) && (plane.g2.flight_options & FlightOptions::ENABLE_LOITER_ALT_CONTROL)) {
+        // loiter is using altitude control based on the pitch stick, don't use it again here
+        return;
+    }
+
     float elevator = SRV_Channels::get_output_scaled(SRV_Channel::k_elevator);
     elevator = channel_pitch->stick_mixing(elevator);
     SRV_Channels::set_output_scaled(SRV_Channel::k_elevator, elevator);
@@ -281,7 +286,12 @@ void Plane::stabilize_stick_mixing_fbw()
     }
     nav_roll_cd += roll_input * roll_limit_cd;
     nav_roll_cd = constrain_int32(nav_roll_cd, -roll_limit_cd, roll_limit_cd);
-    
+
+    if ((control_mode == &mode_loiter) && (plane.g2.flight_options & FlightOptions::ENABLE_LOITER_ALT_CONTROL)) {
+        // loiter is using altitude control based on the pitch stick, don't use it again here
+        return;
+    }
+
     float pitch_input = channel_pitch->norm_input();
     if (pitch_input > 0.5f) {
         pitch_input = (3*pitch_input - 1);

ArduPlane/Parameters.cpp

@@ -1082,7 +1082,7 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = {
     // @Param: FLIGHT_OPTIONS
     // @DisplayName: Flight mode options
     // @Description: Flight mode specific options
-    // @Bitmask: 0:Rudder mixing in direct flight modes only (Manual / Stabilize / Acro),1:Use centered throttle in Cruise or FBWB to indicate trim airspeed, 2:Disable attitude check for takeoff arming, 3:Force target airspeed to trim airspeed in Cruise or FBWB, 4: Climb to ALT_HOLD_RTL before turning for RTL, 5: Enable yaw damper in acro mode, 6: Surpress speed scaling during auto takeoffs to be 1 or less to prevent oscillations without airpseed sensor., 7:EnableDefaultAirspeed for takeoff, 8: Remove the TRIM_PITCH_CD on the GCS horizon, 9: Remove the TRIM_PITCH_CD on the OSD horizon, 10: Adjust mid-throttle to be TRIM_THROTTLE in non-auto throttle modes except MANUAL, 11:Disable suppression of fixed wing rate gains in ground mode
+    // @Bitmask: 0:Rudder mixing in direct flight modes only (Manual / Stabilize / Acro),1:Use centered throttle in Cruise or FBWB to indicate trim airspeed, 2:Disable attitude check for takeoff arming, 3:Force target airspeed to trim airspeed in Cruise or FBWB, 4: Climb to ALT_HOLD_RTL before turning for RTL, 5: Enable yaw damper in acro mode, 6: Surpress speed scaling during auto takeoffs to be 1 or less to prevent oscillations without airpseed sensor., 7:EnableDefaultAirspeed for takeoff, 8: Remove the TRIM_PITCH_CD on the GCS horizon, 9: Remove the TRIM_PITCH_CD on the OSD horizon, 10: Adjust mid-throttle to be TRIM_THROTTLE in non-auto throttle modes except MANUAL, 11:Disable suppression of fixed wing rate gains in ground mode, 12: Enable FBWB style loiter altitude control
     // @User: Advanced
     AP_GROUPINFO("FLIGHT_OPTIONS", 13, ParametersG2, flight_options, 0),
 

ArduPlane/altitude.cpp

@@ -31,6 +31,9 @@ void Plane::adjust_altitude_target()
         control_mode == &mode_cruise) {
         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 && ((guided_state.target_alt_time_ms != 0) || guided_state.target_alt > -0.001 )) { // target_alt now defaults to -1, and _time_ms defaults to zero.
         // offboard altitude demanded

ArduPlane/defines.h

@@ -162,6 +162,8 @@ enum FlightOptions {
     OSD_REMOVE_TRIM_PITCH_CD = (1 << 9),
     CENTER_THROTTLE_TRIM = (1<<10),
     DISABLE_GROUND_PID_SUPPRESSION = (1<<11),
+    ENABLE_LOITER_ALT_CONTROL = (1<<12),
+
 };
 
 enum CrowFlapOptions {

ArduPlane/mode.h

@@ -271,6 +271,8 @@ public:
     bool does_auto_navigation() const override { return true; }
 
     bool does_auto_throttle() const override { return true; }
+    
+    bool allows_terrain_disable() const override { return true; }
 
 protected:
 

ArduPlane/mode_loiter.cpp

@@ -5,6 +5,14 @@ bool ModeLoiter::_enter()
 {
     plane.do_loiter_at_location();
     plane.setup_terrain_target_alt(plane.next_WP_loc);
+
+    // make sure the local target altitude is the same as the nav target used for loiter nav
+    // this allows us to do FBWB style stick control
+    /*IGNORE_RETURN(plane.next_WP_loc.get_alt_cm(Location::AltFrame::ABSOLUTE, plane.target_altitude.amsl_cm));*/
+    if (plane.stick_mixing_enabled() && (plane.g2.flight_options & FlightOptions::ENABLE_LOITER_ALT_CONTROL)) {
+        plane.set_target_altitude_current();
+    }
+
     plane.loiter_angle_reset();
 
     return true;
@@ -13,8 +21,12 @@ bool ModeLoiter::_enter()
 void ModeLoiter::update()
 {
     plane.calc_nav_roll();
-    plane.calc_nav_pitch();
-    plane.calc_throttle();
+    if (plane.stick_mixing_enabled() && (plane.g2.flight_options & FlightOptions::ENABLE_LOITER_ALT_CONTROL)) {
+        plane.update_fbwb_speed_height();
+    } else {
+        plane.calc_nav_pitch();
+        plane.calc_throttle();
+    }
 }
 
 bool ModeLoiter::isHeadingLinedUp(const Location loiterCenterLoc, const Location targetLoc)
@@ -74,6 +86,11 @@ bool ModeLoiter::isHeadingLinedUp_cd(const int32_t bearing_cd)
 
 void ModeLoiter::navigate()
 {
+    if (plane.g2.flight_options & FlightOptions::ENABLE_LOITER_ALT_CONTROL) {
+        // update the WP alt from the global target adjusted by update_fbwb_speed_height
+        plane.next_WP_loc.set_alt_cm(plane.target_altitude.amsl_cm, Location::AltFrame::ABSOLUTE);
+    }
+
     // Zero indicates to use WP_LOITER_RAD
     plane.update_loiter(0);
 }

ArduPlane/navigation.cpp

@@ -365,7 +365,7 @@ void Plane::update_loiter(uint16_t radius)
 }
 
 /*
-  handle speed and height control in FBWB or CRUISE mode.
+  handle speed and height control in FBWB, CRUISE, and optionally, LOITER mode.
   In this mode the elevator is used to change target altitude. The
   throttle is used to change target airspeed or throttle
  */