From 9b1d9e3cf072d06221a41cbbdb98dd558e637554 Mon Sep 17 00:00:00 2001
From: Robert Lefebvre <robert.lefebvre@gmail.com>
Date: Thu, 3 Jul 2014 15:49:48 -0400
Subject: [PATCH] AC_AttitudeControl_Heli: Add passthrough_to_motor_roll_pitch
 function.

---
 .../AC_AttitudeControl/AC_AttitudeControl_Heli.cpp | 14 +++++++++++++-
 .../AC_AttitudeControl/AC_AttitudeControl_Heli.h   | 14 ++++++++++++--
 2 files changed, 25 insertions(+), 3 deletions(-)

diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.cpp b/libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.cpp
index f9ab49a53..190971295 100644
--- a/libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.cpp
+++ b/libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.cpp
@@ -71,7 +71,11 @@ void AC_AttitudeControl_Heli::rate_controller_run()
 {	
     // call rate controllers and send output to motors object
     // To-Do: should the outputs from get_rate_roll, pitch, yaw be int16_t which is the input to the motors library?
-    rate_bf_to_motor_roll_pitch(_rate_bf_target.x, _rate_bf_target.y);
+    if (_flags_heli.flybar_passthrough){
+        passthrough_to_motor_roll_pitch();
+    }else{
+        rate_bf_to_motor_roll_pitch(_rate_bf_target.x, _rate_bf_target.y);
+    }
     _motors.set_yaw(rate_bf_to_motor_yaw(_rate_bf_target.z));
 }
 
@@ -241,6 +245,14 @@ static LowPassFilterFloat rate_dynamics_filter;     // Rate Dynamics filter
 */
 }
 
+// passthrough_to_motor_roll_pitch - passthrough the pilots roll and pitch inputs directly to swashplate for flybar acro mode
+void AC_AttitudeControl_Heli::passthrough_to_motor_roll_pitch()
+{
+    // output to motors
+    _motors.set_roll(_rc_roll.control_in);
+    _motors.set_pitch(_rc_pitch.control_in);
+}
+
 // rate_bf_to_motor_yaw - ask the rate controller to calculate the motor outputs to achieve the target rate in centi-degrees / second
 float AC_AttitudeControl_Heli::rate_bf_to_motor_yaw(float rate_target_cds)
 {
diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.h b/libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.h
index 445fb4fbf..4642b804e 100644
--- a/libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.h
+++ b/libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.h
@@ -22,11 +22,14 @@ public:
                         const AP_Vehicle::MultiCopter &aparm,
                         AP_MotorsHeli& motors,
                         AC_P& p_angle_roll, AC_P& p_angle_pitch, AC_P& p_angle_yaw,
-                        AC_HELI_PID& pid_rate_roll, AC_HELI_PID& pid_rate_pitch, AC_HELI_PID& pid_rate_yaw
+                        AC_HELI_PID& pid_rate_roll, AC_HELI_PID& pid_rate_pitch, AC_HELI_PID& pid_rate_yaw,
+                        RC_Channel& rc_roll, RC_Channel& rc_pitch
                         ) :
         AC_AttitudeControl(ahrs, aparm, motors,
                            p_angle_roll, p_angle_pitch, p_angle_yaw,
-                           pid_rate_roll, pid_rate_pitch, pid_rate_yaw)
+                           pid_rate_roll, pid_rate_pitch, pid_rate_yaw),
+        _rc_roll(rc_roll),
+        _rc_pitch(rc_pitch)
 		{
             AP_Param::setup_object_defaults(this, var_info);
 		}
@@ -53,6 +56,10 @@ private:
         uint8_t leaky_i             :   1;  // 1 if we should use leaky i term for body-frame rate to motor stage
         uint8_t flybar_passthrough  :   1;  // 1 if we should pass through pilots input directly to swash-plate
     } _flags_heli;
+    
+    // references to external libraries
+    RC_Channel& _rc_roll;
+    RC_Channel& _rc_pitch;
 
     //
     // body-frame rate controller
@@ -69,6 +76,9 @@ private:
     // get_angle_boost - calculate total body frame throttle required to produce the given earth frame throttle
     virtual int16_t get_angle_boost(int16_t throttle_pwm);
     
+    // roll and pitch inputs are sent directly to motor outputs (servos) direct flybar control.
+    void passthrough_to_motor_roll_pitch();
+    
     // LPF filters to act on Rate Feedforward terms to linearize output.
     // Due to complicated aerodynamic effects, feedforwards acting too fast can lead
     // to jerks on rate change requests.
-- 
GitLab