diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.cpp b/libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.cpp index f9ab49a534..1909712953 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 445fb4fbfc..4642b804e3 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.