zrzk
/
ardupilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

AC_AttitudeControl_Heli: Change to use AC_HELI_PID class instead of AC_PID. Remove FF parameters from class.

mission-4.1.18
Robert Lefebvre 11 years ago committed by Randy Mackay
parent
5ee87f7561
commit
7c9249de93
2 changed files with 16 additions and 38 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 47
      libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.cpp
  2. 7
      libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.h

47
libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.cpp
Unescape View File

@ -53,30 +53,6 @@ const AP_Param::GroupInfo AC_AttitudeControl_Heli::var_info[] PROGMEM = { @@ -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() @@ -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 @@ -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 @@ -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 @@ -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) @@ -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) {

7
libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.h
Unescape View File

@ -7,6 +7,7 @@ @@ -7,6 +7,7 @@
#define AC_ATTITUDECONTROL_HELI_H
#include <AC_AttitudeControl.h>
#include <AC_HELI_PID.h>
#define AC_ATTITUDE_HELI_ROLL_FF 0.0f
#define AC_ATTITUDE_HELI_PITCH_FF 0.0f
@ -20,7 +21,7 @@ public: @@ -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: @@ -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

Write Preview
Loading…
Cancel
Save