From 7c9249de93aa49be71c7eaf248c523002a5521c3 Mon Sep 17 00:00:00 2001 From: Robert Lefebvre Date: Fri, 2 May 2014 20:42:43 -0400 Subject: [PATCH] AC_AttitudeControl_Heli: Change to use AC_HELI_PID class instead of AC_PID. Remove FF parameters from class. --- .../AC_AttitudeControl_Heli.cpp | 47 ++++++------------- .../AC_AttitudeControl_Heli.h | 7 +-- 2 files changed, 16 insertions(+), 38 deletions(-) diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.cpp b/libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.cpp index 6c47b6d6e7..e102336296 100644 --- a/libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.cpp +++ b/libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.cpp @@ -53,30 +53,6 @@ const AP_Param::GroupInfo AC_AttitudeControl_Heli::var_info[] PROGMEM = { // @User: Advanced AP_GROUPINFO("ACCEL_Y_MAX", 4, AC_AttitudeControl_Heli, _accel_y_max, AC_ATTITUDE_CONTROL_ACCEL_Y_MAX_DEFAULT), - // @Param: RATE_RLL_FF - // @DisplayName: Rate Roll Feed Forward - // @Description: Rate Roll Feed Forward (for TradHeli Only) - // @Range: 0 10 - // @Increment: 0.01 - // @User: Standard - AP_GROUPINFO("RATE_RLL_FF", 5, AC_AttitudeControl_Heli, _heli_roll_ff, AC_ATTITUDE_HELI_ROLL_FF), - - // @Param: RATE_PIT_FF - // @DisplayName: Rate Pitch Feed Forward - // @Description: Rate Pitch Feed Forward (for TradHeli Only) - // @Range: 0 10 - // @Increment: 0.01 - // @User: Standard - AP_GROUPINFO("RATE_PIT_FF", 6, AC_AttitudeControl_Heli, _heli_pitch_ff, AC_ATTITUDE_HELI_ROLL_FF), - - // @Param: RATE_YAW_FF - // @DisplayName: Rate Yaw Feed Forward - // @Description: Rate Yaw Feed Forward (for TradHeli Only) - // @Range: 0 10 - // @Increment: 0.01 - // @User: Standard - AP_GROUPINFO("RATE_YAW_FF", 7, AC_AttitudeControl_Heli, _heli_yaw_ff, AC_ATTITUDE_HELI_YAW_FF), - AP_GROUPEND }; @@ -106,8 +82,8 @@ void AC_AttitudeControl_Heli::rate_controller_run() // rate_bf_to_motor_roll_pitch - ask the rate controller to calculate the motor outputs to achieve the target rate in centi-degrees / second void AC_AttitudeControl_Heli::rate_bf_to_motor_roll_pitch(float rate_roll_target_cds, float rate_pitch_target_cds) { - float roll_pd, roll_i; // used to capture pid values - float pitch_pd, pitch_i; // used to capture pid values + float roll_pd, roll_i, roll_ff; // used to capture pid values + float pitch_pd, pitch_i, pitch_ff; // used to capture pid values float rate_roll_error, rate_pitch_error; // simply target_rate - current_rate float roll_out, pitch_out; const Vector3f& gyro = _ins.get_gyro(); // get current rates @@ -131,7 +107,7 @@ void AC_AttitudeControl_Heli::rate_bf_to_motor_roll_pitch(float rate_roll_target } }else{ if (_flags_heli.leaky_i){ - roll_i = _pid_rate_roll.get_leaky_i(rate_roll_error, _dt, AC_ATTITUDE_HELI_RATE_INTEGRATOR_LEAK_RATE); + roll_i = ((AC_HELI_PID&)_pid_rate_roll).get_leaky_i(rate_roll_error, _dt, AC_ATTITUDE_HELI_RATE_INTEGRATOR_LEAK_RATE); }else{ roll_i = _pid_rate_roll.get_i(rate_roll_error, _dt); } @@ -149,16 +125,19 @@ void AC_AttitudeControl_Heli::rate_bf_to_motor_roll_pitch(float rate_roll_target } }else{ if (_flags_heli.leaky_i) { - pitch_i = _pid_rate_pitch.get_leaky_i(rate_pitch_error, _dt, AC_ATTITUDE_HELI_RATE_INTEGRATOR_LEAK_RATE); + pitch_i = ((AC_HELI_PID&)_pid_rate_pitch).get_leaky_i(rate_pitch_error, _dt, AC_ATTITUDE_HELI_RATE_INTEGRATOR_LEAK_RATE); }else{ pitch_i = _pid_rate_pitch.get_i(rate_pitch_error, _dt); } } } + + roll_ff = ((AC_HELI_PID&)_pid_rate_roll).get_ff(rate_roll_target_cds); + pitch_ff = ((AC_HELI_PID&)_pid_rate_pitch).get_ff(rate_pitch_target_cds); // add feed forward and final output - roll_out = (_heli_roll_ff * rate_roll_target_cds) + roll_pd + roll_i; - pitch_out = (_heli_pitch_ff * rate_pitch_target_cds) + pitch_pd + pitch_i; + roll_out = roll_pd + roll_i + roll_ff; + pitch_out = pitch_pd + pitch_i + pitch_ff; // constrain output and update limit flags if (fabs(roll_out) > AC_ATTITUDE_RATE_RP_CONTROLLER_OUT_MAX) { @@ -261,7 +240,7 @@ static LowPassFilterFloat rate_dynamics_filter; // Rate Dynamics filter // 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) { - float pd,i; // used to capture pid values for logging + float pd,i,ff; // used to capture pid values for logging float current_rate; // this iteration's rate float rate_error; // simply target_rate - current_rate float yaw_out; @@ -282,12 +261,14 @@ float AC_AttitudeControl_Heli::rate_bf_to_motor_yaw(float rate_target_cds) if (((AP_MotorsHeli&)_motors).motor_runup_complete()) { i = _pid_rate_yaw.get_i(rate_error, _dt); } else { - i = _pid_rate_yaw.get_leaky_i(rate_error, _dt, AC_ATTITUDE_HELI_RATE_INTEGRATOR_LEAK_RATE); // If motor is not running use leaky I-term to avoid excessive build-up + i = ((AC_HELI_PID&)_pid_rate_yaw).get_leaky_i(rate_error, _dt, AC_ATTITUDE_HELI_RATE_INTEGRATOR_LEAK_RATE); // If motor is not running use leaky I-term to avoid excessive build-up } } + ff = ((AC_HELI_PID&)_pid_rate_yaw).get_ff(rate_target_cds); + // add feed forward - yaw_out = (_heli_yaw_ff*rate_target_cds) + pd + i; + yaw_out = pd + i + ff; // constrain output and update limit flag if (fabs(yaw_out) > AC_ATTITUDE_RATE_YAW_CONTROLLER_OUT_MAX) { diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.h b/libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.h index d80ab1cd5d..91323e7516 100644 --- a/libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.h +++ b/libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.h @@ -7,6 +7,7 @@ #define AC_ATTITUDECONTROL_HELI_H #include +#include #define AC_ATTITUDE_HELI_ROLL_FF 0.0f #define AC_ATTITUDE_HELI_PITCH_FF 0.0f @@ -20,7 +21,7 @@ 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_PID& pid_rate_roll, AC_PID& pid_rate_pitch, AC_PID& pid_rate_yaw + AC_HELI_PID& pid_rate_roll, AC_HELI_PID& pid_rate_pitch, AC_HELI_PID& pid_rate_yaw ) : AC_AttitudeControl(ahrs, ins, aparm, motors, p_angle_roll, p_angle_pitch, p_angle_yaw, @@ -64,10 +65,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); - // parameters - AP_Float _heli_roll_ff; // body-frame roll rate to motor output feed forward - AP_Float _heli_pitch_ff; // body-frame pitch rate to motor output feed forward - AP_Float _heli_yaw_ff; // body-frame yaw rate to motor output feed forward }; #endif //AC_ATTITUDECONTROL_HELI_H