diff --git a/ArduPlane/Attitude.cpp b/ArduPlane/Attitude.cpp index 06a1acc020..38bba9eb4a 100644 --- a/ArduPlane/Attitude.cpp +++ b/ArduPlane/Attitude.cpp @@ -128,7 +128,8 @@ void Plane::stabilize_roll(float speed_scaler) */ rollController.decay_I(); } else { - roll_out = rollController.get_servo_out(nav_roll_cd - ahrs.roll_sensor, speed_scaler, disable_integrator); + roll_out = 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)); } SRV_Channels::set_output_scaled(SRV_Channel::k_aileron, roll_out); } @@ -172,7 +173,8 @@ void Plane::stabilize_pitch(float speed_scaler) demanded_pitch = landing.get_pitch_cd(); } - pitch_out = pitchController.get_servo_out(demanded_pitch - ahrs.pitch_sensor, speed_scaler, disable_integrator); + pitch_out = pitchController.get_servo_out(demanded_pitch - ahrs.pitch_sensor, speed_scaler, disable_integrator, + ground_mode && !(plane.g2.flight_options & FlightOptions::DISABLE_GROUND_PID_SUPPRESSION)); } SRV_Channels::set_output_scaled(SRV_Channel::k_elevator, pitch_out); } @@ -372,7 +374,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 @@ -396,7 +398,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 7dd79001d0..a5a097ed21 100644 --- a/ArduPlane/Parameters.cpp +++ b/ArduPlane/Parameters.cpp @@ -1105,7 +1105,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 + // @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, 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 ead7e30978..198b1c5890 100644 --- a/ArduPlane/Plane.h +++ b/ArduPlane/Plane.h @@ -597,6 +597,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; @@ -680,7 +683,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 1bd9ade168..80f9dd9086 100644 --- a/ArduPlane/defines.h +++ b/ArduPlane/defines.h @@ -161,6 +161,7 @@ enum FlightOptions { ACRO_YAW_DAMPER = (1 << 5), SURPRESS_TKOFF_SCALING = (1<<6), ENABLE_DEFAULT_AIRSPEED = (1<<7), + DISABLE_GROUND_PID_SUPPRESSION = (1<<11), }; enum CrowFlapOptions { diff --git a/ArduPlane/is_flying.cpp b/ArduPlane/is_flying.cpp index 2ddad79c5d..e22a6e903f 100644 --- a/ArduPlane/is_flying.cpp +++ b/ArduPlane/is_flying.cpp @@ -172,6 +172,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(); } /*