diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.cpp b/libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.cpp index 1909712953..1b2f817557 100644 --- a/libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.cpp +++ b/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 (_flags_heli.flybar_passthrough){ - passthrough_to_motor_roll_pitch(); - }else{ + // if using a flybar passthrough roll and pitch directly to motors + if (_flags_heli.flybar_passthrough) { + _motors.set_roll(_passthrough_roll); + _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) { diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.h b/libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.h index c9f9a4ea1d..d9c4db6c85 100644 --- a/libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.h +++ b/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, - RC_Channel& rc_roll, RC_Channel& rc_pitch + AC_HELI_PID& pid_rate_roll, AC_HELI_PID& pid_rate_pitch, AC_HELI_PID& pid_rate_yaw ) : 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), - _rc_pitch(rc_pitch) + _passthrough_roll(0), _passthrough_pitch(0) { 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