zrzk
/
zr-v4
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

AC_AttitudeControl_Heli: support for updates to AC_PID library 

includes rename get_leaky_i to update_leaky_i
master
Leonard Hall 6 years ago committed by Randy Mackay
parent
bbe33e38f3
commit
30746267ec
2 changed files with 48 additions and 113 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 134
      libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.cpp
  2. 19
      libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.h

134
libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.cpp
Unescape View File

@ -157,13 +157,13 @@ const AP_Param::GroupInfo AC_AttitudeControl_Heli::var_info[] = { @@ -157,13 +157,13 @@ const AP_Param::GroupInfo AC_AttitudeControl_Heli::var_info[] = {
void AC_AttitudeControl_Heli::passthrough_bf_roll_pitch_rate_yaw(float roll_passthrough, float pitch_passthrough, float yaw_rate_bf_cds)
{
// convert from centidegrees on public interface to radians
float yaw_rate_bf_rads = radians(yaw_rate_bf_cds*0.01f);
float yaw_rate_bf_rads = radians(yaw_rate_bf_cds * 0.01f);
// store roll, pitch and passthroughs
// NOTE: this abuses yaw_rate_bf_rads
_passthrough_roll = roll_passthrough;
_passthrough_pitch = pitch_passthrough;
_passthrough_yaw = degrees(yaw_rate_bf_rads)*100.0f;
_passthrough_yaw = degrees(yaw_rate_bf_rads) * 100.0f;
// set rate controller to use pass through
_flags_heli.flybar_passthrough = true;
@ -191,19 +191,19 @@ void AC_AttitudeControl_Heli::passthrough_bf_roll_pitch_rate_yaw(float roll_pass @@ -191,19 +191,19 @@ void AC_AttitudeControl_Heli::passthrough_bf_roll_pitch_rate_yaw(float roll_pass
// convert angle error rotation vector into 321-intrinsic euler angle difference
// NOTE: this results an an approximation linearized about the vehicle's attitude
if (ang_vel_to_euler_rate(Vector3f(_ahrs.roll,_ahrs.pitch,_ahrs.yaw), _att_error_rot_vec_rad, att_error_euler_rad)) {
if (ang_vel_to_euler_rate(Vector3f(_ahrs.roll, _ahrs.pitch, _ahrs.yaw), _att_error_rot_vec_rad, att_error_euler_rad)) {
_attitude_target_euler_angle.x = wrap_PI(att_error_euler_rad.x + _ahrs.roll);
_attitude_target_euler_angle.y = wrap_PI(att_error_euler_rad.y + _ahrs.pitch);
_attitude_target_euler_angle.z = wrap_2PI(att_error_euler_rad.z + _ahrs.yaw);
}
// handle flipping over pitch axis
if (_attitude_target_euler_angle.y > M_PI/2.0f) {
if (_attitude_target_euler_angle.y > M_PI / 2.0f) {
_attitude_target_euler_angle.x = wrap_PI(_attitude_target_euler_angle.x + M_PI);
_attitude_target_euler_angle.y = wrap_PI(M_PI - _attitude_target_euler_angle.x);
_attitude_target_euler_angle.z = wrap_2PI(_attitude_target_euler_angle.z + M_PI);
}
if (_attitude_target_euler_angle.y < -M_PI/2.0f) {
if (_attitude_target_euler_angle.y < -M_PI / 2.0f) {
_attitude_target_euler_angle.x = wrap_PI(_attitude_target_euler_angle.x + M_PI);
_attitude_target_euler_angle.y = wrap_PI(-M_PI - _attitude_target_euler_angle.x);
_attitude_target_euler_angle.z = wrap_2PI(_attitude_target_euler_angle.z + M_PI);
@ -253,13 +253,13 @@ void AC_AttitudeControl_Heli::rate_controller_run() @@ -253,13 +253,13 @@ void AC_AttitudeControl_Heli::rate_controller_run()
// call rate controllers and send output to motors object
// if using a flybar passthrough roll and pitch directly to motors
if (_flags_heli.flybar_passthrough) {
_motors.set_roll(_passthrough_roll/4500.0f);
_motors.set_pitch(_passthrough_pitch/4500.0f);
_motors.set_roll(_passthrough_roll / 4500.0f);
_motors.set_pitch(_passthrough_pitch / 4500.0f);
} else {
rate_bf_to_motor_roll_pitch(gyro_latest, _rate_target_ang_vel.x, _rate_target_ang_vel.y);
}
if (_flags_heli.tail_passthrough) {
_motors.set_yaw(_passthrough_yaw/4500.0f);
_motors.set_yaw(_passthrough_yaw / 4500.0f);
} else {
_motors.set_yaw(rate_target_to_motor_yaw(gyro_latest.z, _rate_target_ang_vel.z));
}
@ -268,8 +268,8 @@ void AC_AttitudeControl_Heli::rate_controller_run() @@ -268,8 +268,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 = acosf(constrain_float(_throttle_in/AC_ATTITUDE_HELI_ANGLE_LIMIT_THROTTLE_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);
float althold_lean_angle_max = acosf(constrain_float(_throttle_in / AC_ATTITUDE_HELI_ANGLE_LIMIT_THROTTLE_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);
}
//
@ -283,68 +283,37 @@ void AC_AttitudeControl_Heli::update_althold_lean_angle_max(float throttle_in) @@ -283,68 +283,37 @@ void AC_AttitudeControl_Heli::update_althold_lean_angle_max(float throttle_in)
// rate_bf_to_motor_roll_pitch - ask the rate controller to calculate the motor outputs to achieve the target rate in radians/second
void AC_AttitudeControl_Heli::rate_bf_to_motor_roll_pitch(const Vector3f &rate_rads, float rate_roll_target_rads, float rate_pitch_target_rads)
{
float roll_pd, roll_i, roll_ff; // used to capture pid values
float pitch_pd, pitch_i, pitch_ff; // used to capture pid values
float rate_roll_error_rads, rate_pitch_error_rads; // simply target_rate - current_rate
float roll_out, pitch_out;
// calculate error
rate_roll_error_rads = rate_roll_target_rads - rate_rads.x;
rate_pitch_error_rads = rate_pitch_target_rads - rate_rads.y;
// pass error to PID controller
_pid_rate_roll.set_input_filter_all(rate_roll_error_rads);
_pid_rate_roll.set_desired_rate(rate_roll_target_rads);
_pid_rate_pitch.set_input_filter_all(rate_pitch_error_rads);
_pid_rate_pitch.set_desired_rate(rate_pitch_target_rads);
// call p and d controllers
roll_pd = _pid_rate_roll.get_p() + _pid_rate_roll.get_d();
pitch_pd = _pid_rate_pitch.get_p() + _pid_rate_pitch.get_d();
// get roll i term
roll_i = _pid_rate_roll.get_integrator();
// update i term as long as we haven't breached the limits or the I term will certainly reduce
if (!_flags_heli.limit_roll || ((roll_i>0&&rate_roll_error_rads<0)||(roll_i<0&&rate_roll_error_rads>0))){
if (_flags_heli.leaky_i){
roll_i = _pid_rate_roll.get_leaky_i(AC_ATTITUDE_HELI_RATE_INTEGRATOR_LEAK_RATE);
}else{
roll_i = _pid_rate_roll.get_i();
}
}
// get pitch i term
pitch_i = _pid_rate_pitch.get_integrator();
// update i term as long as we haven't breached the limits or the I term will certainly reduce
if (!_flags_heli.limit_pitch || ((pitch_i>0&&rate_pitch_error_rads<0)||(pitch_i<0&&rate_pitch_error_rads>0))){
if (_flags_heli.leaky_i) {
pitch_i = _pid_rate_pitch.get_leaky_i(AC_ATTITUDE_HELI_RATE_INTEGRATOR_LEAK_RATE);
}else{
pitch_i = _pid_rate_pitch.get_i();
_pid_rate_roll.update_leaky_i(AC_ATTITUDE_HELI_RATE_INTEGRATOR_LEAK_RATE);
}
float roll_pid = _pid_rate_roll.update_all(rate_roll_target_rads, rate_rads.x, _flags_heli.limit_roll);
if (_flags_heli.leaky_i) {
_pid_rate_pitch.update_leaky_i(AC_ATTITUDE_HELI_RATE_INTEGRATOR_LEAK_RATE);
}
// For legacy reasons, we convert to centi-degrees before inputting to the feedforward
roll_ff = roll_feedforward_filter.apply(_pid_rate_roll.get_ff(rate_roll_target_rads), _dt);
pitch_ff = pitch_feedforward_filter.apply(_pid_rate_pitch.get_ff(rate_pitch_target_rads), _dt);
float pitch_pid = _pid_rate_pitch.update_all(rate_pitch_target_rads, rate_rads.y, _flags_heli.limit_pitch);
// use pid library to calculate ff
float roll_ff = _pid_rate_roll.get_ff();
float pitch_ff = _pid_rate_pitch.get_ff();
// add feed forward and final output
roll_out = roll_pd + roll_i + roll_ff;
pitch_out = pitch_pd + pitch_i + pitch_ff;
float roll_out = roll_pid + roll_ff;
float pitch_out = pitch_pid + pitch_ff;
// constrain output and update limit flags
if (fabsf(roll_out) > AC_ATTITUDE_RATE_RP_CONTROLLER_OUT_MAX) {
roll_out = constrain_float(roll_out,-AC_ATTITUDE_RATE_RP_CONTROLLER_OUT_MAX,AC_ATTITUDE_RATE_RP_CONTROLLER_OUT_MAX);
roll_out = constrain_float(roll_out, -AC_ATTITUDE_RATE_RP_CONTROLLER_OUT_MAX, AC_ATTITUDE_RATE_RP_CONTROLLER_OUT_MAX);
_flags_heli.limit_roll = true;
}else{
} else {
_flags_heli.limit_roll = false;
}
if (fabsf(pitch_out) > AC_ATTITUDE_RATE_RP_CONTROLLER_OUT_MAX) {
pitch_out = constrain_float(pitch_out,-AC_ATTITUDE_RATE_RP_CONTROLLER_OUT_MAX,AC_ATTITUDE_RATE_RP_CONTROLLER_OUT_MAX);
pitch_out = constrain_float(pitch_out, -AC_ATTITUDE_RATE_RP_CONTROLLER_OUT_MAX, AC_ATTITUDE_RATE_RP_CONTROLLER_OUT_MAX);
_flags_heli.limit_pitch = true;
}else{
} else {
_flags_heli.limit_pitch = false;
}
@ -356,22 +325,19 @@ void AC_AttitudeControl_Heli::rate_bf_to_motor_roll_pitch(const Vector3f &rate_r @@ -356,22 +325,19 @@ void AC_AttitudeControl_Heli::rate_bf_to_motor_roll_pitch(const Vector3f &rate_r
// helicopter rotates in yaw. Much of the built-up I-term is needed to tip the disk into the incoming wind. Fast yawing can create an instability
// as the built-up I-term in one axis must be reduced, while the other increases. This helps solve that by rotating the I-terms before the error occurs.
// It does assume that the rotor aerodynamics and mechanics are essentially symmetrical about the main shaft, which is a generally valid assumption.
if (_piro_comp_enabled){
if (_piro_comp_enabled) {
// used to hold current I-terms while doing piro comp:
const float piro_roll_i = roll_i;
const float piro_pitch_i = pitch_i;
const float piro_roll_i = _pid_rate_roll.get_i();
const float piro_pitch_i = _pid_rate_pitch.get_i();
Vector2f yawratevector;
yawratevector.x = cosf(-rate_rads.z * _dt);
yawratevector.y = sinf(-rate_rads.z * _dt);
yawratevector.normalize();
roll_i = piro_roll_i * yawratevector.x - piro_pitch_i * yawratevector.y;
pitch_i = piro_pitch_i * yawratevector.x + piro_roll_i * yawratevector.y;
_pid_rate_pitch.set_integrator(pitch_i);
_pid_rate_roll.set_integrator(roll_i);
_pid_rate_roll.set_integrator(piro_roll_i * yawratevector.x - piro_pitch_i * yawratevector.y);
_pid_rate_pitch.set_integrator(piro_pitch_i * yawratevector.x + piro_roll_i * yawratevector.y);
}
}
@ -379,43 +345,23 @@ void AC_AttitudeControl_Heli::rate_bf_to_motor_roll_pitch(const Vector3f &rate_r @@ -379,43 +345,23 @@ void AC_AttitudeControl_Heli::rate_bf_to_motor_roll_pitch(const Vector3f &rate_r
// rate_bf_to_motor_yaw - ask the rate controller to calculate the motor outputs to achieve the target rate in radians/second
float AC_AttitudeControl_Heli::rate_target_to_motor_yaw(float rate_yaw_actual_rads, float rate_target_rads)
{
float pd,i,vff; // used to capture pid values for logging
float rate_error_rads; // simply target_rate - current_rate
float yaw_out;
// calculate error and call pid controller
rate_error_rads = rate_target_rads - rate_yaw_actual_rads;
// pass error to PID controller
_pid_rate_yaw.set_input_filter_all(rate_error_rads);
_pid_rate_yaw.set_desired_rate(rate_target_rads);
// get p and d
pd = _pid_rate_yaw.get_p() + _pid_rate_yaw.get_d();
// get i term
i = _pid_rate_yaw.get_integrator();
// update i term as long as we haven't breached the limits or the I term will certainly reduce
if (!_flags_heli.limit_yaw || ((i>0&&rate_error_rads<0)||(i<0&&rate_error_rads>0))) {
if (((AP_MotorsHeli&)_motors).rotor_runup_complete()) {
i = _pid_rate_yaw.get_i();
} else {
i = ((AC_HELI_PID&)_pid_rate_yaw).get_leaky_i(AC_ATTITUDE_HELI_RATE_INTEGRATOR_LEAK_RATE); // If motor is not running use leaky I-term to avoid excessive build-up
}
if (!((AP_MotorsHeli&)_motors).rotor_runup_complete()) {
_pid_rate_yaw.update_leaky_i(AC_ATTITUDE_HELI_RATE_INTEGRATOR_LEAK_RATE);
}
// For legacy reasons, we convert to centi-degrees before inputting to the feedforward
vff = yaw_velocity_feedforward_filter.apply(_pid_rate_yaw.get_ff(rate_target_rads), _dt);
float pid = _pid_rate_yaw.update_all(rate_target_rads, rate_yaw_actual_rads, _flags_heli.limit_yaw);
// use pid library to calculate ff
float vff = _pid_rate_yaw.get_ff();
// add feed forward
yaw_out = pd + i + vff;
float yaw_out = pid + vff;
// constrain output and update limit flag
if (fabsf(yaw_out) > AC_ATTITUDE_RATE_YAW_CONTROLLER_OUT_MAX) {
yaw_out = constrain_float(yaw_out,-AC_ATTITUDE_RATE_YAW_CONTROLLER_OUT_MAX,AC_ATTITUDE_RATE_YAW_CONTROLLER_OUT_MAX);
yaw_out = constrain_float(yaw_out, -AC_ATTITUDE_RATE_YAW_CONTROLLER_OUT_MAX, AC_ATTITUDE_RATE_YAW_CONTROLLER_OUT_MAX);
_flags_heli.limit_yaw = true;
}else{
} else {
_flags_heli.limit_yaw = false;
}

19
libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.h
Unescape View File

@ -36,12 +36,9 @@ public: @@ -36,12 +36,9 @@ public:
AP_MotorsHeli& motors,
float dt) :
AC_AttitudeControl(ahrs, aparm, motors, dt),
_pid_rate_roll(AC_ATC_HELI_RATE_RP_P, AC_ATC_HELI_RATE_RP_I, AC_ATC_HELI_RATE_RP_D, AC_ATC_HELI_RATE_RP_IMAX, AC_ATC_HELI_RATE_RP_FILT_HZ, dt, AC_ATC_HELI_RATE_RP_FF),
_pid_rate_pitch(AC_ATC_HELI_RATE_RP_P, AC_ATC_HELI_RATE_RP_I, AC_ATC_HELI_RATE_RP_D, AC_ATC_HELI_RATE_RP_IMAX, AC_ATC_HELI_RATE_RP_FILT_HZ, dt, AC_ATC_HELI_RATE_RP_FF),
_pid_rate_yaw(AC_ATC_HELI_RATE_YAW_P, AC_ATC_HELI_RATE_YAW_I, AC_ATC_HELI_RATE_YAW_D, AC_ATC_HELI_RATE_YAW_IMAX, AC_ATC_HELI_RATE_YAW_FILT_HZ, dt, AC_ATC_HELI_RATE_YAW_FF),
pitch_feedforward_filter(AC_ATTITUDE_HELI_RATE_RP_FF_FILTER),
roll_feedforward_filter(AC_ATTITUDE_HELI_RATE_RP_FF_FILTER),
yaw_velocity_feedforward_filter(AC_ATTITUDE_HELI_RATE_Y_VFF_FILTER)
_pid_rate_roll(AC_ATC_HELI_RATE_RP_P, AC_ATC_HELI_RATE_RP_I, AC_ATC_HELI_RATE_RP_D, AC_ATC_HELI_RATE_RP_FF, AC_ATC_HELI_RATE_RP_IMAX, AC_ATTITUDE_HELI_RATE_RP_FF_FILTER, AC_ATC_HELI_RATE_RP_FILT_HZ, 0.0f, dt),
_pid_rate_pitch(AC_ATC_HELI_RATE_RP_P, AC_ATC_HELI_RATE_RP_I, AC_ATC_HELI_RATE_RP_D, AC_ATC_HELI_RATE_RP_FF, AC_ATC_HELI_RATE_RP_IMAX, AC_ATTITUDE_HELI_RATE_RP_FF_FILTER, AC_ATC_HELI_RATE_RP_FILT_HZ, 0.0f, dt),
_pid_rate_yaw(AC_ATC_HELI_RATE_YAW_P, AC_ATC_HELI_RATE_YAW_I, AC_ATC_HELI_RATE_YAW_D, AC_ATC_HELI_RATE_YAW_FF, AC_ATC_HELI_RATE_YAW_IMAX, AC_ATTITUDE_HELI_RATE_Y_VFF_FILTER, AC_ATC_HELI_RATE_YAW_FILT_HZ, 0.0f, dt)
{
AP_Param::setup_object_defaults(this, var_info);
@ -128,7 +125,7 @@ private: @@ -128,7 +125,7 @@ private:
// rate_bf_to_motor_roll_pitch - ask the rate controller to calculate the motor outputs to achieve the target body-frame rate (in radians/sec) for roll, pitch and yaw
// outputs are sent directly to motor class
void rate_bf_to_motor_roll_pitch(const Vector3f &rate_rads, float rate_roll_target_rads, float rate_pitch_target_rads);
float rate_target_to_motor_yaw(float rate_yaw_actual_rads, float rate_yaw_rads) override;
float rate_target_to_motor_yaw(float rate_yaw_actual_rads, float rate_yaw_rads);
//
// throttle methods
@ -160,12 +157,4 @@ private: @@ -160,12 +157,4 @@ private:
AC_HELI_PID _pid_rate_pitch;
AC_HELI_PID _pid_rate_yaw;
// LPF filters to act on Rate Feedforward terms to linearize output.
// Due to complicated aerodynamic effects, feedforwards acting too fast can lead
// to jerks on rate change requests.
LowPassFilterFloat pitch_feedforward_filter;
LowPassFilterFloat roll_feedforward_filter;
LowPassFilterFloat yaw_velocity_feedforward_filter;
LowPassFilterFloat yaw_acceleration_feedforward_filter;
};

Write Preview
Loading…
Cancel
Save