Copter: TradHeli - add virtual flybar to heli acro flight mode

ArduCopter/mode.h

@@ -258,6 +258,7 @@ public:
 
     bool init(bool ignore_checks) override;
     void run() override;
+    void virtual_flybar( float &roll_out, float &pitch_out, float &yaw_out, float pitch_leak, float roll_leak);
 
 protected:
 private:

ArduCopter/mode_acro_heli.cpp

@@ -70,7 +70,13 @@ void Copter::ModeAcro_Heli::run()
     if (!motors->has_flybar()){
         // convert the input to the desired body frame rate
         get_pilot_desired_angle_rates(channel_roll->get_control_in(), channel_pitch->get_control_in(), channel_yaw->get_control_in(), target_roll, target_pitch, target_yaw);
-
+        if (g.acro_trainer == ACRO_TRAINER_DISABLED) {
+            if (ap.land_complete) {
+                virtual_flybar(target_roll, target_pitch, target_yaw, 3.0f, 3.0f)
+            } else {
+                virtual_flybar(target_roll, target_pitch, target_yaw, g.acro_balance_pitch, g.acro_balance_roll)
+            }
+        }
         if (motors->supports_yaw_passthrough()) {
             // if the tail on a flybar heli has an external gyro then
             // also use no deadzone for the yaw control and
@@ -112,5 +118,32 @@ void Copter::ModeAcro_Heli::run()
     attitude_control->set_throttle_out(pilot_throttle_scaled, false, g.throttle_filt);
 }
 
+
+// virtual_flybar - acts like a flybar by leaking target atttitude back to current attitude
+void Copter::ModeAcro_Heli::virtual_flybar( float &roll_out, float &pitch_out, float &yaw_out, float pitch_leak, float roll_leak)
+{
+    Vector3f rate_ef_level, rate_bf_level;
+
+    // get attitude targets
+    const Vector3f att_target = attitude_control->get_att_target_euler_cd();
+
+    // Calculate Heli mode earth frame rate command for roll
+    rate_ef_level.x = -wrap_180_cd(att_target.x - ahrs.roll_sensor) * roll_leak;
+
+    // Calculate Heli  mode earth frame rate command for pitch
+    rate_ef_level.y = -wrap_180_cd(att_target.y - ahrs.pitch_sensor) * pitch_leak;
+
+    // Calculate earth frame rate command for yaw
+    rate_ef_level.z = 0;
+
+    // convert earth-frame level rates to body-frame level rates
+    attitude_control->euler_rate_to_ang_vel(attitude_control->get_att_target_euler_cd()*radians(0.01f), rate_ef_level, rate_bf_level);
+
+    // combine earth frame rate corrections with rate requests
+    roll_out += rate_bf_level.x;
+    pitch_out += rate_bf_level.y;
+    yaw_out += rate_bf_level.z;
+
+}
 #endif  //HELI_FRAME
 #endif  //MODE_ACRO_ENABLED == ENABLED