Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Plane: suppress rate D terms in ground mode 

this prevents a common oscillation issue on the ground
This commit is contained in:
Andrew Tridgell 2021-10-30 18:52:06 +11:00 committed by Randy Mackay
parent c7d89b7288
commit 352046d7f9
5 changed files with 14 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
ArduPlane/Attitude.cpp
View File

@ -128,7 +128,8 @@ void Plane::stabilize_roll(float speed_scaler)
*/ */
rollController.decay_I(); rollController.decay_I();
} else { } 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); 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(); 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); 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 // 'stabilze' to true, which disables the roll integrator
SRV_Channels::set_output_scaled(SRV_Channel::k_aileron, rollController.get_servo_out(roll_error_cd, SRV_Channels::set_output_scaled(SRV_Channel::k_aileron, rollController.get_servo_out(roll_error_cd,
speed_scaler, speed_scaler,
true)); true, false));
} else { } else {
/* /*
aileron stick is non-zero, use pure rate control until the 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; 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, SRV_Channels::set_output_scaled(SRV_Channel::k_elevator, pitchController.get_servo_out(nav_pitch_cd - ahrs.pitch_sensor,
speed_scaler, speed_scaler,
false)); false, false));
} else { } else {
/* /*
user has non-zero pitch input, use a pure rate controller user has non-zero pitch input, use a pure rate controller

2
ArduPlane/Parameters.cpp
View File

@ -1105,7 +1105,7 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = {
// @Param: FLIGHT_OPTIONS // @Param: FLIGHT_OPTIONS
// @DisplayName: Flight mode options // @DisplayName: Flight mode options
// @Description: Flight mode specific 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 // @User: Advanced
AP_GROUPINFO("FLIGHT_OPTIONS", 13, ParametersG2, flight_options, 0), AP_GROUPINFO("FLIGHT_OPTIONS", 13, ParametersG2, flight_options, 0),

4
ArduPlane/Plane.h
View File

@ -597,6 +597,9 @@ private:
// time since started flying in any mode in milliseconds // time since started flying in any mode in milliseconds
uint32_t started_flying_ms; uint32_t started_flying_ms;
// ground mode is true when disarmed and not flying
bool ground_mode;
// Navigation control variables // Navigation control variables
// The instantaneous desired bank angle. Hundredths of a degree // The instantaneous desired bank angle. Hundredths of a degree
int32_t nav_roll_cd; int32_t nav_roll_cd;
@ -680,7 +683,6 @@ private:
uint32_t time_max_ms; uint32_t time_max_ms;
} loiter; } loiter;
// Conditional command // Conditional command
// A value used in condition commands (eg delay, change alt, etc.) // 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. // For example in a change altitude command, it is the altitude to change to.

1
ArduPlane/defines.h
View File

@ -161,6 +161,7 @@ enum FlightOptions {
ACRO_YAW_DAMPER = (1 << 5), ACRO_YAW_DAMPER = (1 << 5),
SURPRESS_TKOFF_SCALING = (1<<6), SURPRESS_TKOFF_SCALING = (1<<6),
ENABLE_DEFAULT_AIRSPEED = (1<<7), ENABLE_DEFAULT_AIRSPEED = (1<<7),
DISABLE_GROUND_PID_SUPPRESSION = (1<<11),
}; };
enum CrowFlapOptions { enum CrowFlapOptions {

3
ArduPlane/is_flying.cpp
View File

@ -172,6 +172,9 @@ void Plane::update_is_flying_5Hz(void)
// tell AHRS flying state // tell AHRS flying state
set_likely_flying(new_is_flying); set_likely_flying(new_is_flying);
// conservative ground mode value for rate D suppression
ground_mode = !is_flying() && !hal.util->get_soft_armed();
} }
/* /*