From b017fc2196112b896b50652432be1cac094cd573 Mon Sep 17 00:00:00 2001 From: Iampete1 Date: Tue, 10 May 2022 07:53:10 -0500 Subject: [PATCH] ArduPlane: allow FBWB alt control option in LOITER Co-authored-by: Henry Wurzburg --- ArduPlane/Attitude.cpp | 12 +++++++++++- ArduPlane/Parameters.cpp | 2 +- ArduPlane/altitude.cpp | 3 +++ ArduPlane/defines.h | 2 ++ ArduPlane/mode.h | 2 ++ ArduPlane/mode_loiter.cpp | 21 +++++++++++++++++++-- ArduPlane/navigation.cpp | 2 +- 7 files changed, 39 insertions(+), 5 deletions(-) diff --git a/ArduPlane/Attitude.cpp b/ArduPlane/Attitude.cpp index 1cdd3a50e2..77e80e61a8 100644 --- a/ArduPlane/Attitude.cpp +++ b/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); diff --git a/ArduPlane/Parameters.cpp b/ArduPlane/Parameters.cpp index 2f01551955..4458f57ef9 100644 --- a/ArduPlane/Parameters.cpp +++ b/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), diff --git a/ArduPlane/altitude.cpp b/ArduPlane/altitude.cpp index 3ad2b2dfa3..cefe32be21 100644 --- a/ArduPlane/altitude.cpp +++ b/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 diff --git a/ArduPlane/defines.h b/ArduPlane/defines.h index 8da58b7e69..2e92f68f5b 100644 --- a/ArduPlane/defines.h +++ b/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 { diff --git a/ArduPlane/mode.h b/ArduPlane/mode.h index 0dafd9a0eb..4a95f6d8d6 100644 --- a/ArduPlane/mode.h +++ b/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: diff --git a/ArduPlane/mode_loiter.cpp b/ArduPlane/mode_loiter.cpp index 45600af342..1e9b7b6175 100644 --- a/ArduPlane/mode_loiter.cpp +++ b/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); } diff --git a/ArduPlane/navigation.cpp b/ArduPlane/navigation.cpp index fadfcb62a6..42010eab8c 100644 --- a/ArduPlane/navigation.cpp +++ b/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 */