From b8ce23970dbd01a66a2236c70ab72b5de4703fc6 Mon Sep 17 00:00:00 2001 From: Robert Lefebvre Date: Mon, 21 Sep 2015 22:35:53 -0400 Subject: [PATCH] AC_AttitudeControl_Heli: Implement Pirouette Compensation --- .../AC_AttitudeControl_Heli.cpp | 28 +++++++++++++------ .../AC_AttitudeControl_Heli.h | 8 ++++++ 2 files changed, 27 insertions(+), 9 deletions(-) diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.cpp b/libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.cpp index 7676df7f1b..ba76841ced 100644 --- a/libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.cpp +++ b/libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.cpp @@ -8,6 +8,13 @@ const AP_Param::GroupInfo AC_AttitudeControl_Heli::var_info[] = { // parameters from parent vehicle AP_NESTEDGROUPINFO(AC_AttitudeControl, 0), + // @Param: PIRO_COMP + // @DisplayName: Piro Comp Enable + // @Description: Pirouette compensation enabled + // @Range: 0:Disabled 1:Enabled + // @User: Advanced + AP_GROUPINFO("PIRO_COMP", 0, AC_AttitudeControl_Heli, _piro_comp_enabled, 0), + AP_GROUPEND }; @@ -237,28 +244,31 @@ static LowPassFilterFloat rate_dynamics_filter; // Rate Dynamics filter last_roll_output = cc_roll_output; last_pitch_output = cc_pitch_output; # endif // HELI_CC_COMP +*/ -#if AC_ATTITUDE_HELI_PIRO_COMP == ENABLED - if (control_mode <= ACRO){ + // Piro-Comp, or Pirouette Compensation is a pre-compensation calculation, which basically rotates the Roll and Pitch Rate I-terms as the + // helicopter rotates in yaw. Much of the built-up I-term is needed to tip the disk into the incoming wind. Fast yawing can create an instability + // as the built-up I-term in one axis must be reduced, while the other increases. This helps solve that by rotating the I-terms before the error occurs. + // It does assume that the rotor aerodynamics and mechanics are essentially symmetrical about the main shaft, which is a generally valid assumption. + if (_piro_comp_enabled){ - int32_t piro_roll_i, piro_pitch_i; // used to hold i term while doing prio comp + int32_t piro_roll_i, piro_pitch_i; // used to hold I-terms while doing piro comp piro_roll_i = roll_i; piro_pitch_i = pitch_i; Vector2f yawratevector; - yawratevector.x = cosf(-omega.z/100.0f); - yawratevector.y = sinf(-omega.z/100.0f); + yawratevector.x = cosf(-_ahrs.get_gyro().z * _dt); + yawratevector.y = sinf(-_ahrs.get_gyro().z * _dt); yawratevector.normalize(); roll_i = piro_roll_i * yawratevector.x - piro_pitch_i * yawratevector.y; pitch_i = piro_pitch_i * yawratevector.x + piro_roll_i * yawratevector.y; - g.pid_rate_pitch.set_integrator(pitch_i); - g.pid_rate_roll.set_integrator(roll_i); + _pid_rate_pitch.set_integrator(pitch_i); + _pid_rate_roll.set_integrator(roll_i); } -#endif //HELI_PIRO_COMP -*/ + } // rate_bf_to_motor_yaw - ask the rate controller to calculate the motor outputs to achieve the target rate in centi-degrees / second diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.h b/libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.h index 6f9f48e4cf..e867cbbb53 100644 --- a/libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.h +++ b/libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.h @@ -42,6 +42,7 @@ public: _flags_heli.leaky_i = true; _flags_heli.flybar_passthrough = false; _flags_heli.tail_passthrough = false; + _flags_heli.do_piro_comp = false; } // passthrough_bf_roll_pitch_rate_yaw - roll and pitch are passed through directly, body-frame rate target for yaw @@ -64,6 +65,9 @@ public: _flags_heli.tail_passthrough = tail_passthrough; } + // do_piro_comp - controls whether piro-comp is active or not + void do_piro_comp(bool piro_comp) { _flags_heli.do_piro_comp = piro_comp; } + // user settable parameters static const struct AP_Param::GroupInfo var_info[]; @@ -77,6 +81,7 @@ 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 roll & pitch input directly to swash-plate uint8_t tail_passthrough : 1; // 1 if we should pass through pilots yaw input to tail + uint8_t do_piro_comp : 1; // 1 if we should do pirouette compensation on roll/pitch } _flags_heli; // @@ -100,6 +105,9 @@ private: // pass through for yaw if tail_passthrough is set int16_t _passthrough_yaw; + + // parameters + AP_Int8 _piro_comp_enabled; // Flybar present or not. Affects attitude controller used during ACRO flight mode // LPF filters to act on Rate Feedforward terms to linearize output. // Due to complicated aerodynamic effects, feedforwards acting too fast can lead