Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 06:28:27 -04:00
Code Issues Packages Projects Releases Wiki Activity

APM_Control: ROLL and PITCH controllers

Browse Source 
These changes reduce height variation in turns and improve
robustness. the specific changes are:

1) Linked roll and pitch integrator protection to the final output
value so that if final output is on upper limit, the integrator is
prevented from increasing and vice-versa. This improves wind-up
protection.

2) Modified rate feedback in roll and pitch controllers to use body
rates rather than Euler or earth rates.

3) Changed the roll to pitch compensation to use measured roll angle
and estimated airspeed to calculate the component of turn rate
(assuming a level coordinated turn) around the pitch axis. This a
mathematically correct calculation and will work over a range of bank
angles and aircraft with minimal (if any) tuning required.

4) The integrator in the roll and pitch loop is clamped when the
estimated speed is below the minimum FBW speed

5) The noise filter in the pitch and roll loop has been changed to use
a FOH discretisation. This gives improved noise rejection and less
phase loss when compared to the previous filter that used a ZOH or
equivalent discretisation.

This has been flown on the rascal in the SITL and on a X-8 with
limited flight testing. Initial results have been encouraging with
reduced height variation in turns.  Compare to standard PIDS, the
revised pitch and roll controllers allow the use of rate feedback
(effectively the same as the old D term) without beating the servos to
death.  The bank angle compensation in the pitch loop works
effectively over a much larger range of bank angles and requires
minimal tuning compared to the old calculation.

YAW CONTROLLER

Currently testing the a 3-loop acceleration autopilot topology for the
yaw loop with feed forward yaw rate for turn compensation. This 3-loop
topology is commonly used in tactical skid to to turn missiles and is
easy to tune. The following block diagram shows the general signal
flow

Note that the acceleration measurement has to pass through an
integrator before it gets to the actuator. This is a important feature
as it eliminates problems of high frequency noise and potential
coupling with structural modes associated with direct feedback of
measured acceleration to actuator.

The high pass filter has been inserted to compensate for airspeed and
bank angle measurement errors which will cause steady state errors in
the calculation of the turn yaw rate.

The yaw controller flies SITL well, but hasn't been flight tested
yet. It can be configured either as a simple yaw damper, or the
acceleration and integral term can be turned on to allow feedback
control of lateral acceleration/sideslip.

TO DO:

Need to reduce number of tuning parameters and provide consistent
naming Need to provide guidance on tuning these loops with definitions
for all the gain terms.  Need to check signs and units into and out of
lateral loops.

DESIGN DECISIONS PENDING:

1) Can we remove the noise filters? Provided the mpu6k noise filter is
running they are of limited benefit given the 25Hz Nyquist frequency

2) If we do remove them and rely on the mpu6k noise filter, what is
the apprporiate default cutoff frequency for plane use. 20Hz is
probably OK for most setups, but some noisy/high vibration setups
would require as low as 10Hz

3) The inverted flight logic looks like a crash waiting to
happen. It's problematic to test and even if implemented correctly
would still crash a plane with poor inverted flight capability. We
should either implement it properly and fully tested or delete it.
This commit is contained in:
Paul Riseborough 2013-04-23 21:02:18 +10:00 committed by Andrew Tridgell
parent 3663426550
commit 10ecffce01
6 changed files with 178 additions and 105 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

78
libraries/APM_Control/AP_PitchController.cpp
View File

@ -1,7 +1,7 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*- // -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*-
// Code by Jon Challinger // Initial Code by Jon Challinger
// // Modified by Paul Riseborough
// This library is free software; you can redistribute it and / or // This library is free software; you can redistribute it and / or
// modify it under the terms of the GNU Lesser General Public // modify it under the terms of the GNU Lesser General Public
// License as published by the Free Software Foundation; either // License as published by the Free Software Foundation; either
@ -15,18 +15,26 @@
extern const AP_HAL::HAL& hal; extern const AP_HAL::HAL& hal;
const AP_Param::GroupInfo AP_PitchController::var_info[] PROGMEM = { const AP_Param::GroupInfo AP_PitchController::var_info[] PROGMEM = {
AP_GROUPINFO("AP", 0, AP_PitchController, _kp_angle, 1.0), AP_GROUPINFO("OMEGA", 0, AP_PitchController, _kp_angle, 1.0),
AP_GROUPINFO("FF", 1, AP_PitchController, _kp_ff, 0.4), AP_GROUPINFO("K_P", 1, AP_PitchController, _kp_ff, 0.4),
AP_GROUPINFO("RP", 2, AP_PitchController, _kp_rate, 0.0), AP_GROUPINFO("K_D", 2, AP_PitchController, _kp_rate, 0.0),
AP_GROUPINFO("RI", 3, AP_PitchController, _ki_rate, 0.0), AP_GROUPINFO("K_I", 3, AP_PitchController, _ki_rate, 0.0),
AP_GROUPINFO("RMAX_U", 4, AP_PitchController, _max_rate_pos, 0.0), AP_GROUPINFO("RMAX_U", 4, AP_PitchController, _max_rate_pos, 0.0),
AP_GROUPINFO("RMAX_D", 5, AP_PitchController, _max_rate_neg, 0.0), AP_GROUPINFO("RMAX_D", 5, AP_PitchController, _max_rate_neg, 0.0),
AP_GROUPINFO("RLL_FF", 6, AP_PitchController, _roll_ff, 0.0), AP_GROUPINFO("K_RLL", 6, AP_PitchController, _roll_ff, 1.0),
AP_GROUPINFO("STAB_GAIN", 7, AP_PitchController, _stabilize_gain, 1),
AP_GROUPEND AP_GROUPEND
}; };
int32_t AP_PitchController::get_servo_out(int32_t angle, float scaler, bool stabilize) // Function returns an equivalent elevator deflection in centi-degrees in the range from -4500 to 4500
// A positive demand is up
// Inputs are:
// 1) demanded pitch angle in centi-degrees
// 2) control gain scaler = scaling_speed / aspeed
// 3) boolean which is true when stabilise mode is active
// 4) minimum FBW airspeed (metres/sec)
// 5) maximum FBW airspeed (metres/sec)
//
int32_t AP_PitchController::get_servo_out(int32_t angle, float scaler, bool stabilize, int16_t aspd_min, int16_t aspd_max)
{ {
uint32_t tnow = hal.scheduler->millis(); uint32_t tnow = hal.scheduler->millis();
uint32_t dt = tnow - _last_t; uint32_t dt = tnow - _last_t;
@ -53,23 +61,15 @@ int32_t AP_PitchController::get_servo_out(int32_t angle, float scaler, bool stab
// If no airspeed available use average of min and max // If no airspeed available use average of min and max
aspeed = 0.5f*(float(aspd_min) + float(aspd_max)); aspeed = 0.5f*(float(aspd_min) + float(aspd_max));
} }
rate_offset = fabsf(ToDeg((9.807f / max(aspeed , float(aspd_min))) * tanf(bank_angle) * sinf(bank_angle))) * _roll_ff; rate_offset = fabsf(ToDeg((9.807f / constrain(aspeed , float(aspd_min), float(aspd_max))) * tanf(bank_angle) * sinf(bank_angle))) * _roll_ff;
//Calculate pitch angle error in centi-degrees //Calculate pitch angle error in centi-degrees
int32_t angle_err = angle - _ahrs->pitch_sensor; int32_t angle_err = angle - _ahrs->pitch_sensor;
angle_err *= inv;
// Calculate the desired pitch rate (deg/sec) from the angle error
float rate = _ahrs->get_pitch_rate_earth(); float desired_rate = angle_err * 0.01f * _kp_angle;
float RC = 1/(2*PI*_fCut);
rate = _last_rate +
(delta_time / (RC + delta_time)) * (rate - _last_rate);
_last_rate = rate;
float roll_scaler = 1/constrain_float(cosf(_ahrs->roll),0.33f,1);
int32_t desired_rate = angle_err * _kp_angle;
// limit the maximum pitch rate demand
if (_max_rate_neg && desired_rate < -_max_rate_neg) { if (_max_rate_neg && desired_rate < -_max_rate_neg) {
desired_rate = -_max_rate_neg; desired_rate = -_max_rate_neg;
} else if (_max_rate_pos && desired_rate > _max_rate_pos) { } else if (_max_rate_pos && desired_rate > _max_rate_pos) {
@ -82,23 +82,43 @@ int32_t AP_PitchController::get_servo_out(int32_t angle, float scaler, bool stab
// Get body rate vector (radians/sec) // Get body rate vector (radians/sec)
float omega_y = _ahrs->get_gyro().y; float omega_y = _ahrs->get_gyro().y;
// Apply a first order lowpass filter with a 20Hz cut-off
// Coefficients derived using a first order hold discretisation method
// Use of FOH discretisation increases high frequency noise rejection
// and reduces phase loss compared to other methods
float rate = 0.0810026f * _last_rate_out + 0.6343426f * omega_y + 0.2846549f * _last_rate_in;
_last_rate_out = rate;
_last_rate_in = omega_y;
// Calculate the pitch rate error (deg/sec) and scale // Calculate the pitch rate error (deg/sec) and scale
float rate_error = (desired_rate - ToDeg(omega_y)) * scaler; float rate_error = (desired_rate - ToDeg(rate)) * scaler;
// Multiply pitch rate error by _ki_rate and integrate // Multiply pitch rate error by _ki_rate and integrate
// Don't integrate if in stabilise mode as the integrator will wind up against the pilots inputs // Don't integrate if in stabilise mode as the integrator will wind up against the pilots inputs
if (!stabilize) { if (!stabilize) {
if ((fabsf(_ki_rate) > 0) && (dt > 0)) //only integrate if gain and time step are positive and airspeed above min value.
if ((fabsf(_ki_rate) > 0) && (dt > 0) && (aspeed > float(aspd_min)))
{ {
_integrator += (rate_error * _ki_rate) * scaler * delta_time; float integrator_delta = rate_error * _ki_rate * scaler * delta_time;
if (_integrator < -4500-out) _integrator = -4500-out; // prevent the integrator from increasing if surface defln demand is above the upper limit
else if (_integrator > 4500-out) _integrator = 4500-out; if (_last_out < -45) integrator_delta = max(integrator_delta , 0);
// prevent the integrator from decreasing if surface defln demand is below the lower limit
else if (_last_out > 45) integrator_delta = min(integrator_delta , 0);
_integrator += integrator_delta;
} }
} else { } else {
_integrator = 0; _integrator = 0;
} }
return out + _integrator; // Calculate the demanded control surface deflection
// Note the scaler is applied again. We want a 1/speed scaler applied to the feed-forward
// path, but want a 1/speed^2 scaler applied to the rate error path.
// This is because acceleration scales with speed^2, but rate scales with speed.
float _last_out = ( (rate_error * _kp_rate) + _integrator + (desired_rate * _kp_ff) ) * scaler;
// Convert to centi-degrees and constrain
float out = constrain(_last_out * 100, -4500, 4500);
return out;
} }
void AP_PitchController::reset_I() void AP_PitchController::reset_I()

5
libraries/APM_Control/AP_PitchController.h
View File

@ -15,7 +15,7 @@ public:
void set_ahrs(AP_AHRS *ahrs) { _ahrs = ahrs; } void set_ahrs(AP_AHRS *ahrs) { _ahrs = ahrs; }
int32_t get_servo_out(int32_t angle, float scaler = 1.0, bool stabilize = false); int32_t get_servo_out(int32_t angle, float scaler = 1.0, bool stabilize = false, int16_t aspd_min = 0, int16_t aspd_max = 0);
void reset_I(); void reset_I();
@ -29,8 +29,9 @@ private:
AP_Int16 _max_rate_pos; AP_Int16 _max_rate_pos;
AP_Int16 _max_rate_neg; AP_Int16 _max_rate_neg;
AP_Float _roll_ff; AP_Float _roll_ff;
AP_Float _stabilize_gain;
uint32_t _last_t; uint32_t _last_t;
float _last_rate_in;
float _last_rate_out;
float _last_out; float _last_out;
float _integrator; float _integrator;

81
libraries/APM_Control/AP_RollController.cpp
View File

@ -1,6 +1,7 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*- // -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*-
// Code by Jon Challinger // Code by Jon Challinger
// Modified by Paul Riseborough
// //
// This library is free software; you can redistribute it and / or // This library is free software; you can redistribute it and / or
// modify it under the terms of the GNU Lesser General Public // modify it under the terms of the GNU Lesser General Public
@ -15,16 +16,23 @@
extern const AP_HAL::HAL& hal; extern const AP_HAL::HAL& hal;
const AP_Param::GroupInfo AP_RollController::var_info[] PROGMEM = { const AP_Param::GroupInfo AP_RollController::var_info[] PROGMEM = {
AP_GROUPINFO("AP", 0, AP_RollController, _kp_angle, 1.0), AP_GROUPINFO("OMEGA", 0, AP_RollController, _kp_angle, 1.0),
AP_GROUPINFO("FF", 1, AP_RollController, _kp_ff, 0.4), AP_GROUPINFO("K_P", 1, AP_RollController, _kp_ff, 0.4),
AP_GROUPINFO("RP", 2, AP_RollController, _kp_rate, 0.0), AP_GROUPINFO("K_D", 2, AP_RollController, _kp_rate, 0.0),
AP_GROUPINFO("RI", 3, AP_RollController, _ki_rate, 0.0), AP_GROUPINFO("K_I", 3, AP_RollController, _ki_rate, 0.0),
AP_GROUPINFO("RMAX", 4, AP_RollController, _max_rate, 0), AP_GROUPINFO("RMAX", 4, AP_RollController, _max_rate, 60),
AP_GROUPINFO("STAB_GAIN", 5, AP_RollController, _stabilize_gain, 1),
AP_GROUPEND AP_GROUPEND
}; };
int32_t AP_RollController::get_servo_out(int32_t angle, float scaler, bool stabilize) // Function returns an equivalent elevator deflection in centi-degrees in the range from -4500 to 4500
// A positive demand is up
// Inputs are:
// 1) demanded bank angle in centi-degrees
// 2) control gain scaler = scaling_speed / aspeed
// 3) boolean which is true when stabilise mode is active
// 4) minimum FBW airspeed (metres/sec)
//
int32_t AP_RollController::get_servo_out(int32_t angle, float scaler, bool stabilize, int16_t aspd_min)
{ {
uint32_t tnow = hal.scheduler->millis(); uint32_t tnow = hal.scheduler->millis();
uint32_t dt = tnow - _last_t; uint32_t dt = tnow - _last_t;
@ -36,41 +44,60 @@ int32_t AP_RollController::get_servo_out(int32_t angle, float scaler, bool stabi
if(_ahrs == NULL) return 0; // can't control without a reference if(_ahrs == NULL) return 0; // can't control without a reference
float delta_time = (float)dt / 1000.0f; float delta_time = (float)dt / 1000.0f;
// Calculate bank angle error in centi-degrees
int32_t angle_err = angle - _ahrs->roll_sensor; int32_t angle_err = angle - _ahrs->roll_sensor;
// Calculate the desired roll rate (deg/sec) from the angle error
float desired_rate = angle_err * 0.01f * _kp_angle;
float rate = _ahrs->get_roll_rate_earth(); // Limit the demanded roll rate
float RC = 1/(2*PI*_fCut);
rate = _last_rate +
(delta_time / (RC + delta_time)) * (rate - _last_rate);
_last_rate = rate;
int32_t desired_rate = angle_err * _kp_angle;
if (_max_rate && desired_rate < -_max_rate) desired_rate = -_max_rate; if (_max_rate && desired_rate < -_max_rate) desired_rate = -_max_rate;
else if (_max_rate && desired_rate > _max_rate) desired_rate = _max_rate; else if (_max_rate && desired_rate > _max_rate) desired_rate = _max_rate;
// Get body rate vector (radians/sec) // Get body rate vector (radians/sec)
float omega_x = _ahrs->get_gyro().x; float omega_x = _ahrs->get_gyro().x;
// Apply a first order lowpass filter with a 20Hz cut-off
// Coefficients derived using a first order hold discretisation method
// Use of FOH discretisation increases high frequency noise rejection
// and reduces phase loss compared to other methods
float rate = 0.0810026f * _last_rate_out + 0.6343426f * omega_x + 0.2846549f * _last_rate_in;
_last_rate_out = rate;
_last_rate_in = omega_x;
// Calculate the roll rate error (deg/sec) and apply gain scaler // Calculate the roll rate error (deg/sec) and apply gain scaler
float rate_error = (desired_rate - ToDeg(omega_x)) * scaler; float rate_error = (desired_rate - ToDeg(rate)) * scaler;
float out = (rate_error * _kp_rate + desired_rate * _kp_ff) * scaler; // Get an airspeed estimate - default to zero if none available
float aspeed;
//rate integrator if (!_ahrs->airspeed_estimate(&aspeed)) aspeed = 0.0f;
if (!stabilize) {
if ((fabsf(_ki_rate) > 0) && (dt > 0)) // Multiply roll rate error by _ki_rate and integrate
// Don't integrate if in stabilise mode as the integrator will wind up against the pilots inputs
if (!stabilize) {
//only integrate if gain and time step are positive and airspeed above min value.
if ((fabsf(_ki_rate) > 0) && (dt > 0) && (aspeed > float(aspd_min)))
{ {
_integrator += (rate_error * _ki_rate) * scaler * delta_time; float integrator_delta = rate_error * _ki_rate * scaler * delta_time;
if (_integrator < -4500-out) _integrator = -4500-out; // prevent the integrator from increasing if surface defln demand is above the upper limit
else if (_integrator > 4500-out) _integrator = 4500-out; if (_last_out < -45) integrator_delta = max(integrator_delta , 0);
// prevent the integrator from decreasing if surface defln demand is below the lower limit
else if (_last_out > 45) integrator_delta = min(integrator_delta , 0);
_integrator += integrator_delta;
} }
} else { } else {
_integrator = 0; _integrator = 0;
} }
return out + _integrator; // Calculate the demanded control surface deflection
// Note the scaler is applied again. We want a 1/speed scaler applied to the feed-forward
// path, but want a 1/speed^2 scaler applied to the rate error path.
// This is because acceleration scales with speed^2, but rate scales with speed.
float _last_out = ( (rate_error * _kp_rate) + _integrator + (desired_rate * _kp_ff) ) * scaler;
// Convert to centi-degrees and constrain
float out = constrain(_last_out * 100, -4500, 4500);
return out;
} }
void AP_RollController::reset_I() void AP_RollController::reset_I()

5
libraries/APM_Control/AP_RollController.h
View File

@ -15,7 +15,7 @@ public:
void set_ahrs(AP_AHRS *ahrs) { _ahrs = ahrs; } void set_ahrs(AP_AHRS *ahrs) { _ahrs = ahrs; }
int32_t get_servo_out(int32_t angle, float scaler=1.0, bool stabilize=false); int32_t get_servo_out(int32_t angle, float scaler=1.0, bool stabilize=false, int16_t aspd_min = 0);
void reset_I(); void reset_I();
@ -26,9 +26,10 @@ private:
AP_Float _kp_ff; AP_Float _kp_ff;
AP_Float _kp_rate; AP_Float _kp_rate;
AP_Float _ki_rate; AP_Float _ki_rate;
AP_Float _stabilize_gain;
AP_Int16 _max_rate; AP_Int16 _max_rate;
uint32_t _last_t; uint32_t _last_t;
float _last_rate_in;
float _last_rate_out;
float _last_out; float _last_out;
float _integrator; float _integrator;

99
libraries/APM_Control/AP_YawController.cpp
View File

@ -1,6 +1,8 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*- // -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*-
// Code by Jon Challinger // Code by Jon Challinger
// Modified by Paul Riseborough to implement a three loop autopilot
// topology
// //
// This library is free software; you can redistribute it and / or // This library is free software; you can redistribute it and / or
// modify it under the terms of the GNU Lesser General Public // modify it under the terms of the GNU Lesser General Public
@ -14,18 +16,14 @@
extern const AP_HAL::HAL& hal; extern const AP_HAL::HAL& hal;
const AP_Param::GroupInfo AP_YawController::var_info[] PROGMEM = { const AP_Param::GroupInfo AP_YawController::var_info[] PROGMEM = {
AP_GROUPINFO("P", 0, AP_YawController, _kp, 0), AP_GROUPINFO("K_A", 0, AP_YawController, _K_A, 0),
AP_GROUPINFO("I", 1, AP_YawController, _ki, 0), AP_GROUPINFO("K_I", 1, AP_YawController, _K_I, 0),
AP_GROUPINFO("IMAX", 2, AP_YawController, _imax, 0), AP_GROUPINFO("K_D", 2, AP_YawController, _K_D, 0),
AP_GROUPINFO("K_RLL", 3, AP_YawController, _K_FF, 1),
AP_GROUPEND AP_GROUPEND
}; };
// Low pass filter cut frequency for derivative calculation. int32_t AP_YawController::get_servo_out(float scaler, bool stabilize, int16_t aspd_min, int16_t aspd_max)
// FCUT macro computes a frequency cut based on an acceptable delay.
#define FCUT(d) (1 / ( 2 * 3.14f * (d) ) )
const float AP_YawController::_fCut = FCUT(0.5f);
int32_t AP_YawController::get_servo_out(float scaler, bool stick_movement)
{ {
uint32_t tnow = hal.scheduler->millis(); uint32_t tnow = hal.scheduler->millis();
uint32_t dt = tnow - _last_t; uint32_t dt = tnow - _last_t;
@ -40,23 +38,37 @@ int32_t AP_YawController::get_servo_out(float scaler, bool stick_movement)
float delta_time = (float) dt / 1000.0f; float delta_time = (float) dt / 1000.0f;
if(stick_movement) { // Calculate yaw rate required to keep up with a constant height coordinated turn
if(!_stick_movement) { float aspeed;
_stick_movement_begin = tnow; float rate_offset;
} else { float bank_angle = _ahrs->roll;
if(_stick_movement_begin < tnow-333) { // limit bank angle between +- 80 deg if right way up
_freeze_start_time = tnow; if (fabsf(bank_angle) < 1.5707964f) {
} bank_angle = constrain(bank_angle,-1.3962634,1.3962634f);
}
} }
rate_offset = (9.807f / max(aspeed , float(aspd_min))) * tanf(bank_angle) * cosf(bank_angle) * _K_FF; if (!_ahrs->airspeed_estimate(&aspeed)) {
// If no airspeed available use average of min and max
aspeed = 0.5f*(float(aspd_min) + float(aspd_max));
}
rate_offset = (9.807f / constrain(aspeed , float(aspd_min), float(aspd_max))) * tanf(bank_angle) * cosf(bank_angle) * _K_FF;
// Get body rate vector (radians/sec) // Get body rate vector (radians/sec)
float omega_z = _ahrs->get_gyro().z; float omega_z = _ahrs->get_gyro();
// Apply a first order lowpass filter with a 20Hz cut-off
// Coefficients derived using a first order hold discretisation method
// Use of FOH discretisation increases high frequency noise rejection
// and reduces phase loss compared to other methods
float rate = 0.0810026f * _last_rate_out + 0.6343426f * omega_z + 0.2846549f * _last_rate_in;
_last_rate_out = rate;
_last_rate_in = omega_z;
// Get the accln vector (m/s^2)
float accel_y = _ins->get_accel().y;
// Subtract the steady turn component of rate from the measured rate // Subtract the steady turn component of rate from the measured rate
// to calculate the rate relative to the turn requirement in degrees/sec // to calculate the rate relative to the turn requirement in degrees/sec
float rate_hp_in = ToDeg(omega_z - rate_offset); float rate_hp_in = ToDeg(rate - rate_offset);
// Apply a high-pass filter to the rate to washout any steady state error // Apply a high-pass filter to the rate to washout any steady state error
// due to bias errors in rate_offset // due to bias errors in rate_offset
@ -66,29 +78,42 @@ int32_t AP_YawController::get_servo_out(float scaler, bool stick_movement)
_last_rate_hp_out = rate_hp_out; _last_rate_hp_out = rate_hp_out;
_last_rate_hp_in = rate_hp_in; _last_rate_hp_in = rate_hp_in;
// Get the accln vector (m/s^2) //Calculate input to integrator
Vector3f accel = _ins->get_accel(); float integ_in = - _K_I * (_K_A * accel_y + rate_hp_out);
// Calculate input to integrator
float integ_in = - _K_I * (_K_A * accel.y + rate_hp_out);
// strongly filter the error // Apply integrator, but clamp input to prevent control saturation and freeze integrator below min FBW speed
float RC = 1/(2*PI*_fCut); // Don't integrate if in stabilise mode as the integrator will wind up against the pilots inputs
error = _last_error + // Don't integrate if _K_D is zero as integrator will keep winding up
(delta_time / (RC + delta_time)) * (error - _last_error); if (!stabilize && _K_D > 0) {
_last_error = error; //only integrate if airspeed above min value
// integrator if (aspeed > float(aspd_min))
if(_freeze_start_time < (tnow - 2000)) { {
if ((fabsf(_ki) > 0) && (dt > 0)) { // prevent the integrator from increasing if surface defln demand is above the upper limit
_integrator += (error * _ki) * scaler * delta_time; if (_last_out < -45) _integrator += max(integ_in * delta_time , 0);
if (_integrator < -_imax) _integrator = -_imax; // prevent the integrator from decreasing if surface defln demand is below the lower limit
else if (_integrator > _imax) _integrator = _imax; else if (_last_out > 45) _integrator += min(integ_in * delta_time , 0);
else _integrator += integ_in * delta_time;
} }
} else { } else {
_integrator = 0; _integrator = 0;
} }
return (error * _kp * scaler) + _integrator; // Protect against increases to _K_D during in-flight tuning from creating large control transients
// due to stored integrator values
if (_K_D > _K_D_last && _K_D > 0) {
_integrator = _K_D_last/_K_D * _integrator;
}
_K_D_last = _K_D;
// Calculate demanded rudder deflection, +Ve deflection yaws nose right
// Save to last value before application of limiter so that integrator limiting
// can detect exceedance next frame
// Scale using inverse dynamic pressure (1/V^2)
_last_out = _K_D * (_integrator - rate_hp_out) * scaler * scaler;
// Convert to centi-degrees and constrain
float out = constrain(_last_out * 100, -4500, 4500);
return out;
} }
void AP_YawController::reset_I() void AP_YawController::reset_I()

15
libraries/APM_Control/AP_YawController.h
View File

@ -19,32 +19,31 @@ public:
_ins = _ahrs->get_ins(); _ins = _ahrs->get_ins();
} }
int32_t get_servo_out(float scaler = 1.0, bool stick_movement = false); int32_t get_servo_out(float scaler = 1.0, bool stabilize = false, int16_t aspd_min = 0, int16_t aspd_max = 0);
void reset_I(); void reset_I();
static const struct AP_Param::GroupInfo var_info[]; static const struct AP_Param::GroupInfo var_info[];
private: private:
AP_Float _kp; AP_Float _K_A;
AP_Float _ki; AP_Float _K_I;
AP_Int16 _imax; AP_Float _K_D;
AP_Float _K_FF;
uint32_t _last_t; uint32_t _last_t;
float _last_error; float _last_error;
float _last_rate_in;
float _last_rate_out;
float _last_out; float _last_out;
float _last_rate_hp_out; float _last_rate_hp_out;
float _last_rate_hp_in; float _last_rate_hp_in;
float _K_D_last; float _K_D_last;
float _integrator; float _integrator;
bool _stick_movement;
uint32_t _stick_movement_begin;
uint32_t _freeze_start_time;
AP_AHRS *_ahrs; AP_AHRS *_ahrs;
AP_InertialSensor *_ins; AP_InertialSensor *_ins;
static const float _fCut;
}; };
#endif // __AP_YAW_CONTROLLER_H__ #endif // __AP_YAW_CONTROLLER_H__