AC_AttitudeControl: Add Hover Roll Trim functionality for helicopters.

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);

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;

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
 };
 

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