AC_AttControlHeli: add passthrough_bf_roll_pitch_rate_yaw

libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.cpp

@@ -61,6 +61,64 @@ const AP_Param::GroupInfo AC_AttitudeControl_Heli::var_info[] PROGMEM = {
     AP_GROUPEND
 };
 
+// passthrough_bf_roll_pitch_rate_yaw - passthrough the pilots roll and pitch inputs directly to swashplate for flybar acro mode
+void AC_AttitudeControl_Heli::passthrough_bf_roll_pitch_rate_yaw(float roll_passthrough, float pitch_passthrough, float yaw_rate_bf)
+{
+    // store roll and pitch passthroughs
+    _passthrough_roll = roll_passthrough;
+    _passthrough_pitch = pitch_passthrough;
+
+    // set rate controller to use pass through
+    _flags_heli.flybar_passthrough = true;
+
+    // set bf rate targets to current body frame rates (i.e. relax and be ready for vehicle to switch out of acro)
+    _rate_bf_desired.x = _ahrs.get_gyro().x * AC_ATTITUDE_CONTROL_DEGX100;
+    _rate_bf_desired.y = _ahrs.get_gyro().y * AC_ATTITUDE_CONTROL_DEGX100;
+
+    // accel limit desired yaw rate
+    if (_accel_y_max > 0.0f) {
+        float rate_change_limit = _accel_y_max * _dt;
+        float rate_change = yaw_rate_bf - _rate_bf_desired.z;
+        rate_change = constrain_float(rate_change, -rate_change_limit, rate_change_limit);
+        _rate_bf_desired.z += rate_change;
+    } else {
+        _rate_bf_desired.z = yaw_rate_bf;
+    }
+
+    integrate_bf_rate_error_to_angle_errors();
+    _angle_bf_error.x = 0;
+    _angle_bf_error.y = 0;
+
+    // update our earth-frame angle targets
+    Vector3f angle_ef_error;
+    frame_conversion_bf_to_ef(_angle_bf_error, angle_ef_error);
+    _angle_ef_target.x = wrap_180_cd_float(angle_ef_error.x + _ahrs.roll_sensor);
+    _angle_ef_target.y = wrap_180_cd_float(angle_ef_error.y + _ahrs.pitch_sensor);
+    _angle_ef_target.z = wrap_360_cd_float(angle_ef_error.z + _ahrs.yaw_sensor);
+
+    // handle flipping over pitch axis
+    if (_angle_ef_target.y > 9000.0f) {
+        _angle_ef_target.x = wrap_180_cd_float(_angle_ef_target.x + 18000.0f);
+        _angle_ef_target.y = wrap_180_cd_float(18000.0f - _angle_ef_target.x);
+        _angle_ef_target.z = wrap_360_cd_float(_angle_ef_target.z + 18000.0f);
+    }
+    if (_angle_ef_target.y < -9000.0f) {
+        _angle_ef_target.x = wrap_180_cd_float(_angle_ef_target.x + 18000.0f);
+        _angle_ef_target.y = wrap_180_cd_float(-18000.0f - _angle_ef_target.x);
+        _angle_ef_target.z = wrap_360_cd_float(_angle_ef_target.z + 18000.0f);
+    }
+
+    // convert body-frame angle errors to body-frame rate targets
+    update_rate_bf_targets();
+
+    // set body-frame roll/pitch rate target to current desired rates which are the vehicle's actual rates
+    _rate_bf_target.x = _rate_bf_desired.x;
+    _rate_bf_target.y = _rate_bf_desired.y;
+
+    // add desired target to yaw
+    _rate_bf_target.z += _rate_bf_desired.z;
+}
+
 //
 // rate controller (body-frame) methods
 //
@@ -70,10 +128,11 @@ const AP_Param::GroupInfo AC_AttitudeControl_Heli::var_info[] PROGMEM = {
 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?
+    // if using a flybar passthrough roll and pitch directly to motors
-    if (_flags_heli.flybar_passthrough){
+    if (_flags_heli.flybar_passthrough) {
-        passthrough_to_motor_roll_pitch();
+        _motors.set_roll(_passthrough_roll);
-    }else{
+        _motors.set_pitch(_passthrough_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));
@@ -245,14 +304,6 @@ 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)
 {

libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.h

@@ -22,18 +22,19 @@ 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),
-        _rc_roll(rc_roll),
+        _passthrough_roll(0), _passthrough_pitch(0)
-        _rc_pitch(rc_pitch)
 		{
             AP_Param::setup_object_defaults(this, var_info);
 		}
 
+    // passthrough_bf_roll_pitch_rate_yaw - roll and pitch are passed through directly, body-frame rate target for yaw
+    void passthrough_bf_roll_pitch_rate_yaw(float roll_passthrough, float pitch_passthrough, float yaw_rate_bf);
+
 	// rate_controller_run - run lowest level body-frame rate controller and send outputs to the motors
 	// should be called at 100hz or more
 	virtual void rate_controller_run();
@@ -57,12 +58,8 @@ private:
         uint8_t limit_pitch         :   1;  // 1 if we have requested larger pitch angle than swash can physically move
         uint8_t limit_yaw           :   1;  // 1 if we have requested larger yaw angle than tail servo can physically move
         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
+        uint8_t flybar_passthrough  :   1;  // 1 if we should pass through pilots roll & pitch input directly to swash-plate
     } _flags_heli;
-    
-    // references to external libraries
-    RC_Channel& _rc_roll;
-    RC_Channel& _rc_pitch;
 
     //
     // body-frame rate controller
@@ -79,8 +76,6 @@ 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
@@ -89,6 +84,9 @@ private:
     LowPassFilterInt32 roll_feedforward_filter;
     LowPassFilterInt32 yaw_feedforward_filter;
 
+    // pass through for roll and pitch
+    int16_t _passthrough_roll;
+    int16_t _passthrough_pitch;
 };
 
 #endif //AC_ATTITUDECONTROL_HELI_H