AC_AttitudeControl: Tradheli support for integrator management and hover collective learning

This commit is contained in:
bnsgeyer 2020-11-17 22:38:18 -05:00 committed by Bill Geyer
parent eb3da385dd
commit 04e2cceaff
4 changed files with 21 additions and 3 deletions

View File

@ -179,6 +179,14 @@ void AC_AttitudeControl::reset_rate_controller_I_terms()
get_rate_yaw_pid().reset_I(); get_rate_yaw_pid().reset_I();
} }
// reset rate controller I terms smoothly to zero in 0.5 seconds
void AC_AttitudeControl::reset_rate_controller_I_terms_smoothly()
{
get_rate_roll_pid().reset_I_smoothly();
get_rate_pitch_pid().reset_I_smoothly();
get_rate_yaw_pid().reset_I_smoothly();
}
// The attitude controller works around the concept of the desired attitude, target attitude // The attitude controller works around the concept of the desired attitude, target attitude
// and measured attitude. The desired attitude is the attitude input into the attitude controller // and measured attitude. The desired attitude is the attitude input into the attitude controller
// that expresses where the higher level code would like the aircraft to move to. The target attitude is moved // that expresses where the higher level code would like the aircraft to move to. The target attitude is moved

View File

@ -117,6 +117,9 @@ public:
// reset rate controller I terms // reset rate controller I terms
void reset_rate_controller_I_terms(); void reset_rate_controller_I_terms();
// reset rate controller I terms smoothly to zero in 0.5 seconds
void reset_rate_controller_I_terms_smoothly();
// Sets attitude target to vehicle attitude // Sets attitude target to vehicle attitude
void set_attitude_target_to_current_attitude() { _ahrs.get_quat_body_to_ned(_attitude_target_quat); } void set_attitude_target_to_current_attitude() { _ahrs.get_quat_body_to_ned(_attitude_target_quat); }
@ -317,6 +320,10 @@ public:
// set_hover_roll_scalar - scales Hover Roll Trim parameter. To be used by vehicle code according to vehicle condition. // set_hover_roll_scalar - scales Hover Roll Trim parameter. To be used by vehicle code according to vehicle condition.
virtual void set_hover_roll_trim_scalar(float scalar) {} virtual void set_hover_roll_trim_scalar(float scalar) {}
// Return angle in centidegrees to be added to roll angle for hover collective learn. Used by heli to counteract
// tail rotor thrust in hover. Overloaded by AC_Attitude_Heli to return angle.
virtual float get_roll_trim_cd() { return 0;}
// passthrough_bf_roll_pitch_rate_yaw - roll and pitch are passed through directly, body-frame rate target for yaw // passthrough_bf_roll_pitch_rate_yaw - roll and pitch are passed through directly, body-frame rate target for yaw
virtual void passthrough_bf_roll_pitch_rate_yaw(float roll_passthrough, float pitch_passthrough, float yaw_rate_bf_cds) {}; virtual void passthrough_bf_roll_pitch_rate_yaw(float roll_passthrough, float pitch_passthrough, float yaw_rate_bf_cds) {};

View File

@ -386,13 +386,13 @@ void AC_AttitudeControl_Heli::rate_bf_to_motor_roll_pitch(const Vector3f &rate_r
if (_flags_heli.leaky_i) { if (_flags_heli.leaky_i) {
_pid_rate_roll.update_leaky_i(AC_ATTITUDE_HELI_RATE_INTEGRATOR_LEAK_RATE); _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) + _actuator_sysid.x; float roll_pid = _pid_rate_roll.update_all(rate_roll_target_rads, rate_rads.x, _motors.limit.roll) + _actuator_sysid.x;
if (_flags_heli.leaky_i) { if (_flags_heli.leaky_i) {
_pid_rate_pitch.update_leaky_i(AC_ATTITUDE_HELI_RATE_INTEGRATOR_LEAK_RATE); _pid_rate_pitch.update_leaky_i(AC_ATTITUDE_HELI_RATE_INTEGRATOR_LEAK_RATE);
} }
float pitch_pid = _pid_rate_pitch.update_all(rate_pitch_target_rads, rate_rads.y, _flags_heli.limit_pitch) + _actuator_sysid.y; float pitch_pid = _pid_rate_pitch.update_all(rate_pitch_target_rads, rate_rads.y, _motors.limit.pitch) + _actuator_sysid.y;
// use pid library to calculate ff // use pid library to calculate ff
float roll_ff = _pid_rate_roll.get_ff(); float roll_ff = _pid_rate_roll.get_ff();
@ -448,7 +448,7 @@ float AC_AttitudeControl_Heli::rate_target_to_motor_yaw(float rate_yaw_actual_ra
_pid_rate_yaw.update_leaky_i(AC_ATTITUDE_HELI_RATE_INTEGRATOR_LEAK_RATE); _pid_rate_yaw.update_leaky_i(AC_ATTITUDE_HELI_RATE_INTEGRATOR_LEAK_RATE);
} }
float pid = _pid_rate_yaw.update_all(rate_target_rads, rate_yaw_actual_rads, _flags_heli.limit_yaw) + _actuator_sysid.z; float pid = _pid_rate_yaw.update_all(rate_target_rads, rate_yaw_actual_rads, _motors.limit.yaw) + _actuator_sysid.z;
// use pid library to calculate ff // use pid library to calculate ff
float vff = _pid_rate_yaw.get_ff()*_feedforward_scalar; float vff = _pid_rate_yaw.get_ff()*_feedforward_scalar;

View File

@ -89,6 +89,9 @@ public:
// set_hover_roll_scalar - scales Hover Roll Trim parameter. To be used by vehicle code according to vehicle condition. // set_hover_roll_scalar - scales Hover Roll Trim parameter. To be used by vehicle code according to vehicle condition.
void set_hover_roll_trim_scalar(float scalar) override {_hover_roll_trim_scalar = constrain_float(scalar, 0.0f, 1.0f);} void set_hover_roll_trim_scalar(float scalar) override {_hover_roll_trim_scalar = constrain_float(scalar, 0.0f, 1.0f);}
// get_roll_trim - angle in centi-degrees to be added to roll angle for learn hover collective. Used by helicopter to counter tail rotor thrust in hover
float get_roll_trim_cd() override { return constrain_float(_hover_roll_trim_scalar * _hover_roll_trim, -1000.0f,1000.0f);}
// Set output throttle // Set output throttle
void set_throttle_out(float throttle_in, bool apply_angle_boost, float filt_cutoff) override; void set_throttle_out(float throttle_in, bool apply_angle_boost, float filt_cutoff) override;