From 9c617a18f4da336e2a15e5e7ab3b3d1ccce5585f Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Fri, 29 Oct 2021 21:37:52 +1100 Subject: [PATCH] Plane: suppress rate D terms in ground mode this prevents a common oscillation issue on the ground --- ArduPlane/Attitude.cpp | 10 ++++++---- ArduPlane/Parameters.cpp | 2 +- ArduPlane/Plane.h | 4 +++- ArduPlane/defines.h | 1 + ArduPlane/is_flying.cpp | 3 +++ 5 files changed, 14 insertions(+), 6 deletions(-) diff --git a/ArduPlane/Attitude.cpp b/ArduPlane/Attitude.cpp index 31233a9f57..3a73c93a0d 100644 --- a/ArduPlane/Attitude.cpp +++ b/ArduPlane/Attitude.cpp @@ -141,7 +141,8 @@ float Plane::stabilize_roll_get_roll_out(float speed_scaler) if (control_mode == &mode_stabilize && channel_roll->get_control_in() != 0) { disable_integrator = true; } - return rollController.get_servo_out(nav_roll_cd - ahrs.roll_sensor, speed_scaler, disable_integrator); + return rollController.get_servo_out(nav_roll_cd - ahrs.roll_sensor, speed_scaler, disable_integrator, + ground_mode && !(plane.g2.flight_options & FlightOptions::DISABLE_GROUND_PID_SUPPRESSION)); } /* @@ -197,7 +198,8 @@ float Plane::stabilize_pitch_get_pitch_out(float speed_scaler) demanded_pitch = landing.get_pitch_cd(); } - return pitchController.get_servo_out(demanded_pitch - ahrs.pitch_sensor, speed_scaler, disable_integrator); + return pitchController.get_servo_out(demanded_pitch - ahrs.pitch_sensor, speed_scaler, disable_integrator, + ground_mode && !(plane.g2.flight_options & FlightOptions::DISABLE_GROUND_PID_SUPPRESSION)); } /* @@ -402,7 +404,7 @@ void Plane::stabilize_acro(float speed_scaler) // 'stabilze' to true, which disables the roll integrator SRV_Channels::set_output_scaled(SRV_Channel::k_aileron, rollController.get_servo_out(roll_error_cd, speed_scaler, - true)); + true, false)); } else { /* aileron stick is non-zero, use pure rate control until the @@ -426,7 +428,7 @@ void Plane::stabilize_acro(float speed_scaler) nav_pitch_cd = acro_state.locked_pitch_cd; SRV_Channels::set_output_scaled(SRV_Channel::k_elevator, pitchController.get_servo_out(nav_pitch_cd - ahrs.pitch_sensor, speed_scaler, - false)); + false, false)); } else { /* user has non-zero pitch input, use a pure rate controller diff --git a/ArduPlane/Parameters.cpp b/ArduPlane/Parameters.cpp index ebe927e3e5..c2633de3f9 100644 --- a/ArduPlane/Parameters.cpp +++ b/ArduPlane/Parameters.cpp @@ -1075,7 +1075,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 + // @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 // @User: Advanced AP_GROUPINFO("FLIGHT_OPTIONS", 13, ParametersG2, flight_options, 0), diff --git a/ArduPlane/Plane.h b/ArduPlane/Plane.h index b1c6182430..9caf84df07 100644 --- a/ArduPlane/Plane.h +++ b/ArduPlane/Plane.h @@ -601,6 +601,9 @@ private: // time since started flying in any mode in milliseconds uint32_t started_flying_ms; + // ground mode is true when disarmed and not flying + bool ground_mode; + // Navigation control variables // The instantaneous desired bank angle. Hundredths of a degree int32_t nav_roll_cd; @@ -684,7 +687,6 @@ private: uint32_t time_max_ms; } loiter; - // Conditional command // A value used in condition commands (eg delay, change alt, etc.) // For example in a change altitude command, it is the altitude to change to. diff --git a/ArduPlane/defines.h b/ArduPlane/defines.h index fe06002708..3a3306c7aa 100644 --- a/ArduPlane/defines.h +++ b/ArduPlane/defines.h @@ -165,6 +165,7 @@ enum FlightOptions { GCS_REMOVE_TRIM_PITCH_CD = (1 << 8), OSD_REMOVE_TRIM_PITCH_CD = (1 << 9), CENTER_THROTTLE_TRIM = (1<<10), + DISABLE_GROUND_PID_SUPPRESSION = (1<<11), }; enum CrowFlapOptions { diff --git a/ArduPlane/is_flying.cpp b/ArduPlane/is_flying.cpp index 1b37e66b5e..38fa07c6a4 100644 --- a/ArduPlane/is_flying.cpp +++ b/ArduPlane/is_flying.cpp @@ -174,6 +174,9 @@ void Plane::update_is_flying_5Hz(void) // tell AHRS flying state set_likely_flying(new_is_flying); + + // conservative ground mode value for rate D suppression + ground_mode = !is_flying() && !hal.util->get_soft_armed(); } /*