diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp b/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp index 7c3ac76375..5f67859028 100644 --- a/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp +++ b/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp @@ -110,6 +110,9 @@ void AC_AttitudeControl::angle_ef_roll_pitch_rate_ef_yaw_smooth(float roll_angle // sanity check smoothing gain smoothing_gain = constrain_float(smoothing_gain,1.0f,50.0f); + // add roll trim to compensate tail rotor thrust in heli (should return zero for multirotors) + roll_angle_ef += get_roll_trim(); + // if accel limiting and feed forward enabled if ((_accel_roll_max > 0.0f) && _rate_bf_ff_enabled) { rate_change_limit = _accel_roll_max * _dt; @@ -202,6 +205,9 @@ void AC_AttitudeControl::angle_ef_roll_pitch_rate_ef_yaw(float roll_angle_ef, fl { Vector3f angle_ef_error; // earth frame angle errors + // add roll trim to compensate tail rotor thrust in heli (should return zero for multirotors) + roll_angle_ef += get_roll_trim(); + // set earth-frame angle targets for roll and pitch and calculate angle error _angle_ef_target.x = constrain_float(roll_angle_ef, -_aparm.angle_max, _aparm.angle_max); angle_ef_error.x = wrap_180_cd_float(_angle_ef_target.x - _ahrs.roll_sensor); @@ -247,6 +253,9 @@ void AC_AttitudeControl::angle_ef_roll_pitch_yaw(float roll_angle_ef, float pitc { Vector3f angle_ef_error; + // add roll trim to compensate tail rotor thrust in heli (should return zero for multirotors) + roll_angle_ef += get_roll_trim(); + // set earth-frame angle targets _angle_ef_target.x = constrain_float(roll_angle_ef, -_aparm.angle_max, _aparm.angle_max); _angle_ef_target.y = constrain_float(pitch_angle_ef, -_aparm.angle_max, _aparm.angle_max); diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl.h b/libraries/AC_AttitudeControl/AC_AttitudeControl.h index 489ef8e6f4..b53c1cd15f 100644 --- a/libraries/AC_AttitudeControl/AC_AttitudeControl.h +++ b/libraries/AC_AttitudeControl/AC_AttitudeControl.h @@ -271,6 +271,10 @@ protected: // calculate total body frame throttle required to produce the given earth frame throttle virtual float get_boosted_throttle(float throttle_in) = 0; + // get_roll_trim - angle in centi-degrees to be added to roll angle. Used by helicopter to counter tail rotor thrust in hover + // Overloaded by AC_Attitude_Heli to return angle. Should be left to return zero for multirotors. + virtual int16_t get_roll_trim() { return 0;} + // references to external libraries const AP_AHRS& _ahrs; const AP_Vehicle::MultiCopter &_aparm; diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.cpp b/libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.cpp index a1e09991fa..516f78a833 100644 --- a/libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.cpp +++ b/libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.cpp @@ -15,6 +15,14 @@ const AP_Param::GroupInfo AC_AttitudeControl_Heli::var_info[] = { // @User: Advanced AP_GROUPINFO("PIRO_COMP", 0, AC_AttitudeControl_Heli, _piro_comp_enabled, 0), + // @Param: HOVR_ROL_TRM + // @DisplayName: Hover Roll Trim + // @Description: Trim the hover roll angle to counter tail rotor thrust in a hover + // @Units: Centi-Degrees + // @Range: 0 1000 + // @User: Advanced + AP_GROUPINFO("HOVR_ROL_TRM", 1, AC_AttitudeControl_Heli, _hover_roll_trim, AC_ATTITUDE_HELI_HOVER_ROLL_TRIM_DEFAULT), + AP_GROUPEND }; diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.h b/libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.h index e867cbbb53..257f8fd973 100644 --- a/libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.h +++ b/libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.h @@ -15,6 +15,7 @@ #define AC_ATTITUDE_HELI_RATE_RP_FF_FILTER 10.0f #define AC_ATTITUDE_HELI_RATE_Y_VFF_FILTER 10.0f #define AC_ATTITUDE_HELI_RATE_Y_AFF_FILTER 10.0f +#define AC_ATTITUDE_HELI_HOVER_ROLL_TRIM_DEFAULT 300 class AC_AttitudeControl_Heli : public AC_AttitudeControl { public: @@ -106,8 +107,12 @@ private: // pass through for yaw if tail_passthrough is set int16_t _passthrough_yaw; + // get_roll_trim - angle in centi-degrees to be added to roll angle. Used by helicopter to counter tail rotor thrust in hover + int16_t get_roll_trim() { return constrain_int16(_hover_roll_trim, -1000, 1000);} + // parameters AP_Int8 _piro_comp_enabled; // Flybar present or not. Affects attitude controller used during ACRO flight mode + AP_Int16 _hover_roll_trim; // Angle in centi-degrees used to counter tail rotor thrust in hover // LPF filters to act on Rate Feedforward terms to linearize output. // Due to complicated aerodynamic effects, feedforwards acting too fast can lead