mirror of https://github.com/ArduPilot/ardupilot
AC_AttitudeControl: add ATC_ANG_LIM_TC parameter
This allow adjusting the reponse to limit lean angles to reduce altitude loss
This commit is contained in:
parent
dafc45eb26
commit
096bdd67f8
|
@ -87,6 +87,13 @@ const AP_Param::GroupInfo AC_AttitudeControl::var_info[] = {
|
|||
// @User: Standard
|
||||
AP_SUBGROUPINFO(_p_angle_yaw, "ANG_YAW_", 15, AC_AttitudeControl, AC_P),
|
||||
|
||||
// @Param: ANG_LIM_TC
|
||||
// @DisplayName: Angle Limit (to maintain altitude) Time Constant
|
||||
// @Description: Angle Limit (to maintain altitude) Time Constant
|
||||
// @Range: 0.5 10.0
|
||||
// @User: Advanced
|
||||
AP_GROUPINFO("ANG_LIM_TC", 16, AC_AttitudeControl, _angle_limit_tc, AC_ATTITUDE_CONTROL_ANGLE_LIMIT_TC_DEFAULT),
|
||||
|
||||
AP_GROUPEND
|
||||
};
|
||||
|
||||
|
|
|
@ -40,8 +40,8 @@
|
|||
|
||||
#define AC_ATTITUDE_CONTROL_RATE_BF_FF_DEFAULT 1 // body-frame rate feedforward enabled by default
|
||||
|
||||
#define AC_ATTITUDE_CONTROL_ALTHOLD_LEAN_ANGLE_TC_DEFAULT 1.0f // Time constant used to limit lean angle so that vehicle does not lose altitude
|
||||
#define AC_ATTITUDE_CONTROL_ALTHOLD_LEAN_ANGLE_THROTTLE_MAX 0.8f // Max throttle used to limit lean angle so that vehicle does not lose altitude
|
||||
#define AC_ATTITUDE_CONTROL_ANGLE_LIMIT_TC_DEFAULT 1.0f // Time constant used to limit lean angle so that vehicle does not lose altitude
|
||||
#define AC_ATTITUDE_CONTROL_ANGLE_LIMIT_THROTTLE_MAX 0.8f // Max throttle used to limit lean angle so that vehicle does not lose altitude
|
||||
|
||||
#define AC_ATTITUDE_CONTROL_MIN_DEFAULT 0.1f // minimum throttle mix
|
||||
#define AC_ATTITUDE_CONTROL_MID_DEFAULT 0.5f // manual throttle mix
|
||||
|
@ -316,6 +316,9 @@ protected:
|
|||
AC_P _p_angle_pitch;
|
||||
AC_P _p_angle_yaw;
|
||||
|
||||
// Angle limit time constant (to maintain altitude)
|
||||
AP_Float _angle_limit_tc;
|
||||
|
||||
// Intersampling period in seconds
|
||||
float _dt;
|
||||
|
||||
|
|
|
@ -235,8 +235,8 @@ void AC_AttitudeControl_Heli::rate_controller_run()
|
|||
// Update Alt_Hold angle maximum
|
||||
void AC_AttitudeControl_Heli::update_althold_lean_angle_max(float throttle_in)
|
||||
{
|
||||
float althold_lean_angle_max = acos(constrain_float(_throttle_in/AC_ATTITUDE_CONTROL_ALTHOLD_LEAN_ANGLE_THROTTLE_MAX, 0.0f, 1.0f));
|
||||
_althold_lean_angle_max = _althold_lean_angle_max + (_dt/(_dt+AC_ATTITUDE_CONTROL_ALTHOLD_LEAN_ANGLE_TC_DEFAULT))*(_throttle_in-_althold_lean_angle_max);
|
||||
float althold_lean_angle_max = acos(constrain_float(_throttle_in/AC_ATTITUDE_CONTROL_ANGLE_LIMIT_THROTTLE_MAX, 0.0f, 1.0f));
|
||||
_althold_lean_angle_max = _althold_lean_angle_max + (_dt/(_dt+_angle_limit_tc))*(_throttle_in-_althold_lean_angle_max);
|
||||
}
|
||||
|
||||
//
|
||||
|
|
|
@ -159,8 +159,8 @@ void AC_AttitudeControl_Multi::update_althold_lean_angle_max(float throttle_in)
|
|||
return;
|
||||
}
|
||||
|
||||
float althold_lean_angle_max = acos(constrain_float(_throttle_in/(AC_ATTITUDE_CONTROL_ALTHOLD_LEAN_ANGLE_THROTTLE_MAX * thr_max), 0.0f, 1.0f));
|
||||
_althold_lean_angle_max = _althold_lean_angle_max + (_dt/(_dt+AC_ATTITUDE_CONTROL_ALTHOLD_LEAN_ANGLE_TC_DEFAULT))*(althold_lean_angle_max-_althold_lean_angle_max);
|
||||
float althold_lean_angle_max = acos(constrain_float(_throttle_in/(AC_ATTITUDE_CONTROL_ANGLE_LIMIT_THROTTLE_MAX * thr_max), 0.0f, 1.0f));
|
||||
_althold_lean_angle_max = _althold_lean_angle_max + (_dt/(_dt+_angle_limit_tc))*(althold_lean_angle_max-_althold_lean_angle_max);
|
||||
}
|
||||
|
||||
void AC_AttitudeControl_Multi::set_throttle_out(float throttle_in, bool apply_angle_boost, float filter_cutoff)
|
||||
|
|
Loading…
Reference in New Issue