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

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:
Leonard Hall 2016-05-26 22:40:35 +09:00 committed by Randy Mackay
parent dafc45eb26
commit 096bdd67f8
4 changed files with 16 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
libraries/AC_AttitudeControl/AC_AttitudeControl.cpp
View File

@ -87,6 +87,13 @@ const AP_Param::GroupInfo AC_AttitudeControl::var_info[] = {
// @User: Standard // @User: Standard
AP_SUBGROUPINFO(_p_angle_yaw, "ANG_YAW_", 15, AC_AttitudeControl, AC_P), 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 AP_GROUPEND
}; };

7
libraries/AC_AttitudeControl/AC_AttitudeControl.h
View File

@ -40,8 +40,8 @@
#define AC_ATTITUDE_CONTROL_RATE_BF_FF_DEFAULT 1 // body-frame rate feedforward enabled by default #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_ANGLE_LIMIT_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_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_MIN_DEFAULT 0.1f // minimum throttle mix
#define AC_ATTITUDE_CONTROL_MID_DEFAULT 0.5f // manual 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_pitch;
AC_P _p_angle_yaw; AC_P _p_angle_yaw;
// Angle limit time constant (to maintain altitude)
AP_Float _angle_limit_tc;
// Intersampling period in seconds // Intersampling period in seconds
float _dt; float _dt;

4
libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.cpp
View File

@ -235,8 +235,8 @@ void AC_AttitudeControl_Heli::rate_controller_run()
// Update Alt_Hold angle maximum // Update Alt_Hold angle maximum
void AC_AttitudeControl_Heli::update_althold_lean_angle_max(float throttle_in) 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)); 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+AC_ATTITUDE_CONTROL_ALTHOLD_LEAN_ANGLE_TC_DEFAULT))*(_throttle_in-_althold_lean_angle_max); _althold_lean_angle_max = _althold_lean_angle_max + (_dt/(_dt+_angle_limit_tc))*(_throttle_in-_althold_lean_angle_max);
} }
// //

4
libraries/AC_AttitudeControl/AC_AttitudeControl_Multi.cpp
View File

@ -159,8 +159,8 @@ void AC_AttitudeControl_Multi::update_althold_lean_angle_max(float throttle_in)
return; 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)); 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+AC_ATTITUDE_CONTROL_ALTHOLD_LEAN_ANGLE_TC_DEFAULT))*(althold_lean_angle_max-_althold_lean_angle_max); _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) void AC_AttitudeControl_Multi::set_throttle_out(float throttle_in, bool apply_angle_boost, float filter_cutoff)