From c052b58f70114e1ff3ee098bcfb4fd5e2a9f175e Mon Sep 17 00:00:00 2001 From: bnsgeyer Date: Tue, 17 Nov 2020 22:38:18 -0500 Subject: [PATCH] AC_AttitudeControl: Tradheli support for integrator management and hover collective learning --- libraries/AC_AttitudeControl/AC_AttitudeControl.cpp | 8 ++++++++ libraries/AC_AttitudeControl/AC_AttitudeControl.h | 7 +++++++ libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.cpp | 6 +++--- libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.h | 3 +++ 4 files changed, 21 insertions(+), 3 deletions(-) diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp b/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp index 245e7811d4..613da7a7f5 100644 --- a/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp +++ b/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp @@ -179,6 +179,14 @@ void AC_AttitudeControl::reset_rate_controller_I_terms() 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 // 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 diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl.h b/libraries/AC_AttitudeControl/AC_AttitudeControl.h index f02393e74c..b563be92e8 100644 --- a/libraries/AC_AttitudeControl/AC_AttitudeControl.h +++ b/libraries/AC_AttitudeControl/AC_AttitudeControl.h @@ -114,6 +114,9 @@ public: // 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 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. 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 virtual void passthrough_bf_roll_pitch_rate_yaw(float roll_passthrough, float pitch_passthrough, float yaw_rate_bf_cds) {}; diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.cpp b/libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.cpp index 19274f7568..a80e6fe379 100644 --- a/libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.cpp +++ b/libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.cpp @@ -362,13 +362,13 @@ void AC_AttitudeControl_Heli::rate_bf_to_motor_roll_pitch(const Vector3f &rate_r if (_flags_heli.leaky_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) + _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) { _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 float roll_ff = _pid_rate_roll.get_ff(); @@ -424,7 +424,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); } - 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 float vff = _pid_rate_yaw.get_ff()*_feedforward_scalar; diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.h b/libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.h index ec49f673f9..e56d9dbd92 100644 --- a/libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.h +++ b/libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.h @@ -89,6 +89,9 @@ public: // 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);} + // 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 void set_throttle_out(float throttle_in, bool apply_angle_boost, float filt_cutoff) override;