aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AC_AttitudeControl: add TC for Alt_Hold angle limit

Browse Source
This commit is contained in:
Leonard Hall 2016-05-24 00:42:14 +09:30 committed by Randy Mackay
parent 7ff0fcb25d
commit 3d27ecca92
6 changed files with 32 additions and 14 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
libraries/AC_AttitudeControl/AC_AttitudeControl.cpp
View File

@ -595,7 +595,6 @@ void AC_AttitudeControl::accel_limiting(bool enable_limits)
void AC_AttitudeControl::set_throttle_out_unstabilized(float throttle_in, bool reset_attitude_control, float filter_cutoff)
{
_throttle_in = throttle_in;
_throttle_in_filt.apply(throttle_in, _dt);
_motors.set_throttle_filter_cutoff(filter_cutoff);
if (reset_attitude_control) {
relax_bf_rate_controller();

12
libraries/AC_AttitudeControl/AC_AttitudeControl.h
View File

@ -40,7 +40,8 @@
#define AC_ATTITUDE_CONTROL_RATE_BF_FF_DEFAULT 1 // body-frame rate feedforward enabled by default
#define AC_ATTITUDE_CONTROL_ALTHOLD_LEANANGLE_FILT_HZ 1.0f // filter (in hz) of throttle filter used to limit lean angle so that vehicle does not lose altitude
#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_MIN_DEFAULT 0.1f // minimum throttle mix
#define AC_ATTITUDE_CONTROL_MID_DEFAULT 0.5f // manual throttle mix
@ -60,7 +61,6 @@ public:
_dt(dt),
_angle_boost(0),
_att_ctrl_use_accel_limit(true),
_throttle_in_filt(AC_ATTITUDE_CONTROL_ALTHOLD_LEANANGLE_FILT_HZ),
_throttle_rpy_mix_desired(AC_ATTITUDE_CONTROL_THR_MIX_DEFAULT),
_throttle_rpy_mix(AC_ATTITUDE_CONTROL_THR_MIX_DEFAULT),
_ahrs(ahrs),
@ -198,6 +198,9 @@ public:
// Enable or disable body-frame feed forward
void accel_limiting(bool enable_or_disable);
// Update Alt_Hold angle maximum
virtual void update_althold_lean_angle_max(float throttle_in) = 0;
// Set output throttle
virtual void set_throttle_out(float throttle_in, bool apply_angle_boost, float filt_cutoff) = 0;
@ -341,7 +344,6 @@ protected:
Vector3f _ang_vel_target_rads;
// throttle provided as input to attitude controller. This does not include angle boost.
// Used only for logging.
float _throttle_in = 0.0f;
// This represents the throttle increase applied for tilt compensation.
@ -351,8 +353,8 @@ protected:
// Specifies whether the attitude controller should use the acceleration limit
bool _att_ctrl_use_accel_limit;
// Filtered throttle input - used to limit lean angle when throttle is saturated
LowPassFilterFloat _throttle_in_filt;
// Filtered Alt_Hold lean angle max - used to limit lean angle when throttle is saturated using Alt_Hold
float _althold_lean_angle_max;
float _throttle_rpy_mix_desired; // desired throttle_low_comp value, actual throttle_low_comp is slewed towards this value over 1~2 seconds
float _throttle_rpy_mix; // mix between throttle and hover throttle for 0 to 1 and ratio above hover throttle for >1

13
libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.cpp
View File

@ -235,11 +235,15 @@ void AC_AttitudeControl_Heli::rate_controller_run()
// get lean angle max for pilot input that prioritises altitude hold over lean angle
float AC_AttitudeControl_Heli::get_althold_lean_angle_max() const
{
// calc maximum tilt angle based on throttle
float ret = acosf(constrain_float(_throttle_in_filt.get()/0.9f, 0.0f, 1.0f));
// TEMP: convert to centi-degrees for public interface
return degrees(ret) * 100.0f;
return degrees(_althold_lean_angle_max) * 100.0f;
}
// 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);
}
//
@ -407,7 +411,6 @@ float AC_AttitudeControl_Heli::rate_bf_to_motor_yaw(float rate_target_rads)
void AC_AttitudeControl_Heli::set_throttle_out(float throttle_in, bool apply_angle_boost, float filter_cutoff)
{
_throttle_in = throttle_in;
_throttle_in_filt.apply(throttle_in, _dt);
_motors.set_throttle_filter_cutoff(filter_cutoff);
_motors.set_throttle(throttle_in);
// Clear angle_boost for logging purposes

3
libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.h
View File

@ -79,6 +79,9 @@ public:
// NOTE: returns centi-degrees
float get_althold_lean_angle_max() const;
// Update Alt_Hold angle maximum
void update_althold_lean_angle_max(float throttle_in) override;
// use_leaky_i - controls whether we use leaky i term for body-frame to motor output stage
void use_leaky_i(bool leaky_i) { _flags_heli.leaky_i = leaky_i; }

14
libraries/AC_AttitudeControl/AC_AttitudeControl_Multi.cpp
View File

@ -149,22 +149,30 @@ AC_AttitudeControl_Multi::AC_AttitudeControl_Multi(AP_AHRS &ahrs, const AP_Vehic
// get lean angle max for pilot input that prioritises altitude hold over lean angle
float AC_AttitudeControl_Multi::get_althold_lean_angle_max() const
{
return ToDeg(_althold_lean_angle_max) * 100.0f;
}
// Update Alt_Hold angle maximum
void AC_AttitudeControl_Multi::update_althold_lean_angle_max(float throttle_in)
{
// calc maximum tilt angle based on throttle
float thr_max = _motors_multi.get_throttle_thrust_max();
// divide by zero check
if (is_zero(thr_max)) {
return 0.0f;
_althold_lean_angle_max = 0.0f;
return;
}
return ToDeg(acos(constrain_float(_throttle_in_filt.get()/(0.9f * thr_max), 0.0f, 1.0f))) * 100.0f;
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))*(_throttle_in-_althold_lean_angle_max);
}
void AC_AttitudeControl_Multi::set_throttle_out(float throttle_in, bool apply_angle_boost, float filter_cutoff)
{
_throttle_in = throttle_in;
_throttle_in_filt.apply(throttle_in, _dt);
update_althold_lean_angle_max(throttle_in);
_motors.set_throttle_filter_cutoff(filter_cutoff);
if (apply_angle_boost) {
// Apply angle boost

3
libraries/AC_AttitudeControl/AC_AttitudeControl_Multi.h
View File

@ -55,6 +55,9 @@ public:
// get lean angle max for pilot input that prioritizes altitude hold over lean angle
float get_althold_lean_angle_max() const;
// Update Alt_Hold angle maximum
void update_althold_lean_angle_max(float throttle_in) override;
// Set output throttle
void set_throttle_out(float throttle_in, bool apply_angle_boost, float filt_cutoff) override;