Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

APM_Control: fixed code style of plane rate controllers

This commit is contained in:
Andrew Tridgell 2021-11-25 16:23:42 +11:00 committed by Peter Barker
parent c974863d29
commit 5a996f308b
8 changed files with 262 additions and 245 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
libraries/APM_Control/AP_AutoTune.cpp
View File

@ -57,7 +57,7 @@ AP_AutoTune::AP_AutoTune(ATGains &_gains, ATType _type,
#include <stdio.h>
# define Debug(fmt, args ...) do {::printf("%s:%d: " fmt "\n", __FUNCTION__, __LINE__, ## args); } while(0)
#else
# define Debug(fmt, args ...)
# define Debug(fmt, args ...)
#endif
/*
@ -337,7 +337,7 @@ void AP_AutoTune::update(AP_Logger::PID_Info &pinfo, float scaler, float angle_e
D_limit = D;
D_set_ms = now;
action = Action::LOWER_D;
GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "%sD: %.4f", type==AUTOTUNE_ROLL?"Roll":"Pitch", D_limit);
GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "%sD: %.4f", type==AUTOTUNE_ROLL?"Roll":"Pitch", D_limit);
}
} else if (min_Dmod < 1.0) {
// oscillation, with D_limit set
@ -370,7 +370,7 @@ void AP_AutoTune::update(AP_Logger::PID_Info &pinfo, float scaler, float angle_e
}
} else if (ff_count < 4) {
// we don't have a good FF estimate yet, keep going
} else if (!is_positive(D_limit)) {
/* we haven't detected D oscillation yet, keep raising D */
D *= 1.3;

13
libraries/APM_Control/AP_AutoTune.h
View File

@ -6,7 +6,8 @@
#include <AP_Vehicle/AP_Vehicle.h>
#include <AC_PID/AC_PID.h>
class AP_AutoTune {
class AP_AutoTune
{
public:
struct ATGains {
AP_Float tau;
@ -56,7 +57,7 @@ public:
// are we running?
bool running;
private:
// the current gains
ATGains &current;
@ -65,7 +66,7 @@ private:
// what type of autotune is this
ATType type;
const AP_Vehicle::FixedWing &aparm;
const AP_Vehicle::FixedWing &aparm;
// values to restore if we leave autotune mode
ATGains restore;
@ -77,7 +78,8 @@ private:
// the demanded/achieved state
enum class ATState {IDLE,
DEMAND_POS,
DEMAND_NEG};
DEMAND_NEG
};
ATState state;
// the demanded/achieved state
@ -90,7 +92,8 @@ private:
RAISE_D,
RAISE_P,
LOWER_D,
LOWER_P};
LOWER_P
};
Action action;
// when we entered the current state

138
libraries/APM_Control/AP_PitchController.cpp
View File

@ -3,12 +3,12 @@
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.
This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
You should have received a copy of the GNU General Public License
along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
@ -25,39 +25,39 @@ extern const AP_HAL::HAL& hal;
const AP_Param::GroupInfo AP_PitchController::var_info[] = {
// @Param: 2SRV_TCONST
// @DisplayName: Pitch Time Constant
// @Description: Time constant in seconds from demanded to achieved pitch angle. Most models respond well to 0.5. May be reduced for faster responses, but setting lower than a model can achieve will not help.
// @Range: 0.4 1.0
// @Units: s
// @Increment: 0.1
// @User: Advanced
// @DisplayName: Pitch Time Constant
// @Description: Time constant in seconds from demanded to achieved pitch angle. Most models respond well to 0.5. May be reduced for faster responses, but setting lower than a model can achieve will not help.
// @Range: 0.4 1.0
// @Units: s
// @Increment: 0.1
// @User: Advanced
AP_GROUPINFO("2SRV_TCONST", 0, AP_PitchController, gains.tau, 0.5f),
// index 1 to 3 reserved for old PID values
// @Param: 2SRV_RMAX_UP
// @DisplayName: Pitch up max rate
// @Description: Maximum pitch up rate that the pitch controller demands (degrees/sec) in ACRO mode.
// @Range: 0 100
// @Units: deg/s
// @Increment: 1
// @User: Advanced
// @DisplayName: Pitch up max rate
// @Description: Maximum pitch up rate that the pitch controller demands (degrees/sec) in ACRO mode.
// @Range: 0 100
// @Units: deg/s
// @Increment: 1
// @User: Advanced
AP_GROUPINFO("2SRV_RMAX_UP", 4, AP_PitchController, gains.rmax_pos, 0.0f),
// @Param: 2SRV_RMAX_DN
// @DisplayName: Pitch down max rate
// @Description: This sets the maximum nose down pitch rate that the controller will demand (degrees/sec). Setting it to zero disables the limit.
// @Range: 0 100
// @Units: deg/s
// @Increment: 1
// @User: Advanced
// @DisplayName: Pitch down max rate
// @Description: This sets the maximum nose down pitch rate that the controller will demand (degrees/sec). Setting it to zero disables the limit.
// @Range: 0 100
// @Units: deg/s
// @Increment: 1
// @User: Advanced
AP_GROUPINFO("2SRV_RMAX_DN", 5, AP_PitchController, gains.rmax_neg, 0.0f),
// @Param: 2SRV_RLL
// @DisplayName: Roll compensation
// @Description: Gain added to pitch to keep aircraft from descending or ascending in turns. Increase in increments of 0.05 to reduce altitude loss. Decrease for altitude gain.
// @Range: 0.7 1.5
// @Increment: 0.05
// @DisplayName: Roll compensation
// @Description: Gain added to pitch to keep aircraft from descending or ascending in turns. Increase in increments of 0.05 to reduce altitude loss. Decrease for altitude gain.
// @Range: 0.7 1.5
// @Increment: 0.05
// @User: Standard
AP_GROUPINFO("2SRV_RLL", 6, AP_PitchController, _roll_ff, 1.0f),
@ -130,7 +130,7 @@ const AP_Param::GroupInfo AP_PitchController::var_info[] = {
// @User: Advanced
AP_SUBGROUPINFO(rate_pid, "_RATE_", 11, AP_PitchController, AC_PID),
AP_GROUPEND
};
@ -173,7 +173,7 @@ float AP_PitchController::_get_rate_out(float desired_rate, float scaler, bool d
// when underspeed we lock the integrator
rate_pid.set_integrator(old_I);
}
// FF should be scaled by scaler/eas2tas, but since we have scaled
// the AC_PID target above by scaler*scaler we need to instead
// divide by scaler*eas2tas to get the right scaling
@ -208,10 +208,10 @@ float AP_PitchController::_get_rate_out(float desired_rate, float scaler, bool d
_last_out = out;
if (autotune != nullptr && autotune->running && aspeed > aparm.airspeed_min) {
// let autotune have a go at the values
// let autotune have a go at the values
autotune->update(pinfo, scaler, angle_err_deg);
}
// output is scaled to notional centidegrees of deflection
return constrain_float(out * 100, -4500, 4500);
}
@ -219,7 +219,7 @@ float AP_PitchController::_get_rate_out(float desired_rate, float scaler, bool d
/*
Function returns an equivalent elevator deflection in centi-degrees in the range from -4500 to 4500
A positive demand is up
Inputs are:
Inputs are:
1) demanded pitch rate in degrees/second
2) control gain scaler = scaling_speed / aspeed
3) boolean which is true when stabilise mode is active
@ -229,57 +229,57 @@ float AP_PitchController::_get_rate_out(float desired_rate, float scaler, bool d
float AP_PitchController::get_rate_out(float desired_rate, float scaler)
{
float aspeed;
if (!AP::ahrs().airspeed_estimate(aspeed)) {
// If no airspeed available use average of min and max
if (!AP::ahrs().airspeed_estimate(aspeed)) {
// If no airspeed available use average of min and max
aspeed = 0.5f*(float(aparm.airspeed_min) + float(aparm.airspeed_max));
}
}
return _get_rate_out(desired_rate, scaler, false, aspeed, false);
}
/*
get the rate offset in degrees/second needed for pitch in body frame
to maintain height in a coordinated turn.
Also returns the inverted flag and the estimated airspeed in m/s for
use by the rest of the pitch controller
*/
float AP_PitchController::_get_coordination_rate_offset(float &aspeed, bool &inverted) const
{
float rate_offset;
float bank_angle = AP::ahrs().roll;
float rate_offset;
float bank_angle = AP::ahrs().roll;
// limit bank angle between +- 80 deg if right way up
if (fabsf(bank_angle) < radians(90)) {
bank_angle = constrain_float(bank_angle,-radians(80),radians(80));
// limit bank angle between +- 80 deg if right way up
if (fabsf(bank_angle) < radians(90)) {
bank_angle = constrain_float(bank_angle,-radians(80),radians(80));
inverted = false;
} else {
inverted = true;
if (bank_angle > 0.0f) {
bank_angle = constrain_float(bank_angle,radians(100),radians(180));
} else {
bank_angle = constrain_float(bank_angle,-radians(180),-radians(100));
}
}
} else {
inverted = true;
if (bank_angle > 0.0f) {
bank_angle = constrain_float(bank_angle,radians(100),radians(180));
} else {
bank_angle = constrain_float(bank_angle,-radians(180),-radians(100));
}
}
const AP_AHRS &_ahrs = AP::ahrs();
if (!_ahrs.airspeed_estimate(aspeed)) {
// If no airspeed available use average of min and max
if (!_ahrs.airspeed_estimate(aspeed)) {
// If no airspeed available use average of min and max
aspeed = 0.5f*(float(aparm.airspeed_min) + float(aparm.airspeed_max));
}
}
if (abs(_ahrs.pitch_sensor) > 7000) {
// don't do turn coordination handling when at very high pitch angles
rate_offset = 0;
} else {
rate_offset = cosf(_ahrs.pitch)*fabsf(ToDeg((GRAVITY_MSS / MAX((aspeed * _ahrs.get_EAS2TAS()) , MAX(aparm.airspeed_min, 1))) * tanf(bank_angle) * sinf(bank_angle))) * _roll_ff;
rate_offset = cosf(_ahrs.pitch)*fabsf(ToDeg((GRAVITY_MSS / MAX((aspeed * _ahrs.get_EAS2TAS()), MAX(aparm.airspeed_min, 1))) * tanf(bank_angle) * sinf(bank_angle))) * _roll_ff;
}
if (inverted) {
rate_offset = -rate_offset;
}
if (inverted) {
rate_offset = -rate_offset;
}
return rate_offset;
}
// Function returns an equivalent elevator deflection in centi-degrees in the range from -4500 to 4500
// A positive demand is up
// Inputs are:
// 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
@ -288,37 +288,37 @@ float AP_PitchController::_get_coordination_rate_offset(float &aspeed, bool &inv
//
float AP_PitchController::get_servo_out(int32_t angle_err, float scaler, bool disable_integrator, bool ground_mode)
{
// Calculate offset to pitch rate demand required to maintain pitch angle whilst banking
// Calculate ideal turn rate from bank angle and airspeed assuming a level coordinated turn
// Pitch rate offset is the component of turn rate about the pitch axis
float aspeed;
float rate_offset;
bool inverted;
// Calculate offset to pitch rate demand required to maintain pitch angle whilst banking
// Calculate ideal turn rate from bank angle and airspeed assuming a level coordinated turn
// Pitch rate offset is the component of turn rate about the pitch axis
float aspeed;
float rate_offset;
bool inverted;
if (gains.tau < 0.05f) {
gains.tau.set(0.05f);
}
rate_offset = _get_coordination_rate_offset(aspeed, inverted);
// Calculate the desired pitch rate (deg/sec) from the angle error
// Calculate the desired pitch rate (deg/sec) from the angle error
angle_err_deg = angle_err * 0.01;
float desired_rate = angle_err_deg / gains.tau;
// limit the maximum pitch rate demand. Don't apply when inverted
// as the rates will be tuned when upright, and it is common that
// much higher rates are needed inverted
if (!inverted) {
// limit the maximum pitch rate demand. Don't apply when inverted
// as the rates will be tuned when upright, and it is common that
// much higher rates are needed inverted
if (!inverted) {
desired_rate += rate_offset;
if (gains.rmax_neg && desired_rate < -gains.rmax_neg) {
desired_rate = -gains.rmax_neg;
} else if (gains.rmax_pos && desired_rate > gains.rmax_pos) {
desired_rate = gains.rmax_pos;
}
}
} else {
// Make sure not to invert the turn coordination offset
desired_rate = -desired_rate + rate_offset;
}
}
/*
when we are past the users defined roll limit for the aircraft

25
libraries/APM_Control/AP_PitchController.h
View File

@ -7,7 +7,8 @@
#include <AP_Math/AP_Math.h>
#include <AC_PID/AC_PID.h>
class AP_PitchController {
class AP_PitchController
{
public:
AP_PitchController(const AP_Vehicle::FixedWing &parms);
@ -15,26 +16,30 @@ public:
AP_PitchController(const AP_PitchController &other) = delete;
AP_PitchController &operator=(const AP_PitchController&) = delete;
float get_rate_out(float desired_rate, float scaler);
float get_rate_out(float desired_rate, float scaler);
float get_servo_out(int32_t angle_err, float scaler, bool disable_integrator, bool ground_mode);
void reset_I();
void reset_I();
/*
reduce the integrator, used when we have a low scale factor in a quadplane hover
*/
void decay_I() {
void decay_I()
{
// this reduces integrator by 95% over 2s
_pid_info.I *= 0.995f;
rate_pid.set_integrator(rate_pid.get_i() * 0.995);
}
void autotune_start(void);
void autotune_restore(void);
const AP_Logger::PID_Info& get_pid_info(void) const { return _pid_info; }
const AP_Logger::PID_Info& get_pid_info(void) const
{
return _pid_info;
}
static const struct AP_Param::GroupInfo var_info[];
static const struct AP_Param::GroupInfo var_info[];
AP_Float &kP(void) { return rate_pid.kP(); }
AP_Float &kI(void) { return rate_pid.kI(); }
@ -48,8 +53,8 @@ private:
AP_AutoTune::ATGains gains;
AP_AutoTune *autotune;
bool failed_autotune_alloc;
AP_Int16 _max_rate_neg;
AP_Float _roll_ff;
AP_Int16 _max_rate_neg;
AP_Float _roll_ff;
float _last_out;
AC_PID rate_pid{0.04, 0.15, 0, 0.345, 0.666, 3, 0, 12, 0.02, 150, 1};
float angle_err_deg;
@ -57,5 +62,5 @@ private:
AP_Logger::PID_Info _pid_info;
float _get_rate_out(float desired_rate, float scaler, bool disable_integrator, float aspeed, bool ground_mode);
float _get_coordination_rate_offset(float &aspeed, bool &inverted) const;
float _get_coordination_rate_offset(float &aspeed, bool &inverted) const;
};

46
libraries/APM_Control/AP_RollController.cpp
View File

@ -3,12 +3,12 @@
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.
This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
You should have received a copy of the GNU General Public License
along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
@ -25,23 +25,23 @@ extern const AP_HAL::HAL& hal;
const AP_Param::GroupInfo AP_RollController::var_info[] = {
// @Param: 2SRV_TCONST
// @DisplayName: Roll Time Constant
// @Description: Time constant in seconds from demanded to achieved roll angle. Most models respond well to 0.5. May be reduced for faster responses, but setting lower than a model can achieve will not help.
// @Range: 0.4 1.0
// @Units: s
// @Increment: 0.1
// @User: Advanced
// @DisplayName: Roll Time Constant
// @Description: Time constant in seconds from demanded to achieved roll angle. Most models respond well to 0.5. May be reduced for faster responses, but setting lower than a model can achieve will not help.
// @Range: 0.4 1.0
// @Units: s
// @Increment: 0.1
// @User: Advanced
AP_GROUPINFO("2SRV_TCONST", 0, AP_RollController, gains.tau, 0.5f),
// index 1 to 3 reserved for old PID values
// @Param: 2SRV_RMAX
// @DisplayName: Maximum Roll Rate
// @Description: Maximum roll rate that the roll controller demands (degrees/sec) in ACRO mode.
// @Range: 0 180
// @Units: deg/s
// @Increment: 1
// @User: Advanced
// @DisplayName: Maximum Roll Rate
// @Description: Maximum roll rate that the roll controller demands (degrees/sec) in ACRO mode.
// @Range: 0 180
// @Units: deg/s
// @Increment: 1
// @User: Advanced
AP_GROUPINFO("2SRV_RMAX", 4, AP_RollController, gains.rmax_pos, 0),
// index 5, 6 reserved for old IMAX, FF
@ -113,13 +113,13 @@ const AP_Param::GroupInfo AP_RollController::var_info[] = {
// @User: Advanced
AP_SUBGROUPINFO(rate_pid, "_RATE_", 9, AP_RollController, AC_PID),
AP_GROUPEND
};
// constructor
AP_RollController::AP_RollController(const AP_Vehicle::FixedWing &parms)
: aparm(parms)
: aparm(parms)
{
AP_Param::setup_object_defaults(this, var_info);
rate_pid.set_slew_limit_scale(45);
@ -161,7 +161,7 @@ float AP_RollController::_get_rate_out(float desired_rate, float scaler, bool di
// when underspeed we lock the integrator
rate_pid.set_integrator(old_I);
}
// FF should be scaled by scaler/eas2tas, but since we have scaled
// the AC_PID target above by scaler*scaler we need to instead
// divide by scaler*eas2tas to get the right scaling
@ -196,10 +196,10 @@ float AP_RollController::_get_rate_out(float desired_rate, float scaler, bool di
_last_out = out;
if (autotune != nullptr && autotune->running && aspeed > aparm.airspeed_min) {
// let autotune have a go at the values
// let autotune have a go at the values
autotune->update(pinfo, scaler, angle_err_deg);
}
// output is scaled to notional centidegrees of deflection
return constrain_float(out * 100, -4500, 4500);
}
@ -207,7 +207,7 @@ float AP_RollController::_get_rate_out(float desired_rate, float scaler, bool di
/*
Function returns an equivalent elevator deflection in centi-degrees in the range from -4500 to 4500
A positive demand is up
Inputs are:
Inputs are:
1) desired roll rate in degrees/sec
2) control gain scaler = scaling_speed / aspeed
*/
@ -219,7 +219,7 @@ float AP_RollController::get_rate_out(float desired_rate, float scaler)
/*
Function returns an equivalent aileron deflection in centi-degrees in the range from -4500 to 4500
A positive demand is up
Inputs are:
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
@ -230,7 +230,7 @@ float AP_RollController::get_servo_out(int32_t angle_err, float scaler, bool dis
if (gains.tau < 0.05f) {
gains.tau.set(0.05f);
}
// Calculate the desired roll rate (deg/sec) from the angle error
angle_err_deg = angle_err * 0.01;
float desired_rate = angle_err_deg/ gains.tau;
@ -247,7 +247,7 @@ float AP_RollController::get_servo_out(int32_t angle_err, float scaler, bool dis
void AP_RollController::reset_I()
{
_pid_info.I = 0;
_pid_info.I = 0;
rate_pid.reset_I();
}

21
libraries/APM_Control/AP_RollController.h
View File

@ -7,7 +7,8 @@
#include <AP_Math/AP_Math.h>
#include <AC_PID/AC_PID.h>
class AP_RollController {
class AP_RollController
{
public:
AP_RollController(const AP_Vehicle::FixedWing &parms);
@ -15,24 +16,28 @@ public:
AP_RollController(const AP_RollController &other) = delete;
AP_RollController &operator=(const AP_RollController&) = delete;
float get_rate_out(float desired_rate, float scaler);
float get_rate_out(float desired_rate, float scaler);
float get_servo_out(int32_t angle_err, float scaler, bool disable_integrator, bool ground_mode);
void reset_I();
void reset_I();
/*
reduce the integrator, used when we have a low scale factor in a quadplane hover
*/
void decay_I() {
void decay_I()
{
// this reduces integrator by 95% over 2s
_pid_info.I *= 0.995f;
rate_pid.set_integrator(rate_pid.get_i() * 0.995);
}
void autotune_start(void);
void autotune_restore(void);
const AP_Logger::PID_Info& get_pid_info(void) const { return _pid_info; }
const AP_Logger::PID_Info& get_pid_info(void) const
{
return _pid_info;
}
static const struct AP_Param::GroupInfo var_info[];
@ -41,7 +46,7 @@ public:
void kP(float v) { rate_pid.kP().set(v); }
void kI(float v) { rate_pid.kI().set(v); }
void kD(float v) { rate_pid.kD().set(v); }
void kFF(float v) { rate_pid.ff().set(v); }
void kFF(float v) {rate_pid.ff().set(v); }
AP_Float &kP(void) { return rate_pid.kP(); }
AP_Float &kI(void) { return rate_pid.kI(); }
@ -55,7 +60,7 @@ private:
AP_AutoTune::ATGains gains;
AP_AutoTune *autotune;
bool failed_autotune_alloc;
float _last_out;
float _last_out;
AC_PID rate_pid{0.08, 0.15, 0, 0.345, 0.666, 3, 0, 12, 0.02, 150, 1};
float angle_err_deg;

217
libraries/APM_Control/AP_YawController.cpp
View File

@ -3,12 +3,12 @@
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.
This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
You should have received a copy of the GNU General Public License
along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
@ -25,37 +25,37 @@ extern const AP_HAL::HAL& hal;
const AP_Param::GroupInfo AP_YawController::var_info[] = {
// @Param: SLIP
// @DisplayName: Sideslip control gain
// @Description: Gain from lateral acceleration to demanded yaw rate for aircraft with enough fuselage area to detect lateral acceleration and sideslips. Do not enable for flying wings and gliders. Actively coordinates flight more than just yaw damping. Set after YAW2SRV_DAMP and YAW2SRV_INT are tuned.
// @Range: 0 4
// @Increment: 0.25
// @Param: SLIP
// @DisplayName: Sideslip control gain
// @Description: Gain from lateral acceleration to demanded yaw rate for aircraft with enough fuselage area to detect lateral acceleration and sideslips. Do not enable for flying wings and gliders. Actively coordinates flight more than just yaw damping. Set after YAW2SRV_DAMP and YAW2SRV_INT are tuned.
// @Range: 0 4
// @Increment: 0.25
// @User: Advanced
AP_GROUPINFO("SLIP", 0, AP_YawController, _K_A, 0),
AP_GROUPINFO("SLIP", 0, AP_YawController, _K_A, 0),
// @Param: INT
// @DisplayName: Sideslip control integrator
// @Description: Integral gain from lateral acceleration error. Effectively trims rudder to eliminate long-term sideslip.
// @Range: 0 2
// @Increment: 0.25
// @Param: INT
// @DisplayName: Sideslip control integrator
// @Description: Integral gain from lateral acceleration error. Effectively trims rudder to eliminate long-term sideslip.
// @Range: 0 2
// @Increment: 0.25
// @User: Advanced
AP_GROUPINFO("INT", 1, AP_YawController, _K_I, 0),
AP_GROUPINFO("INT", 1, AP_YawController, _K_I, 0),
// @Param: DAMP
// @DisplayName: Yaw damping
// @Description: Gain from yaw rate to rudder. Most effective at yaw damping and should be tuned after KFF_RDDRMIX. Also disables YAW2SRV_INT if set to 0.
// @Range: 0 2
// @Increment: 0.25
// @Param: DAMP
// @DisplayName: Yaw damping
// @Description: Gain from yaw rate to rudder. Most effective at yaw damping and should be tuned after KFF_RDDRMIX. Also disables YAW2SRV_INT if set to 0.
// @Range: 0 2
// @Increment: 0.25
// @User: Advanced
AP_GROUPINFO("DAMP", 2, AP_YawController, _K_D, 0),
AP_GROUPINFO("DAMP", 2, AP_YawController, _K_D, 0),
// @Param: RLL
// @DisplayName: Yaw coordination gain
// @Description: Gain to the yaw rate required to keep it consistent with the turn rate in a coordinated turn. Corrects for yaw tendencies after the turn is established. Increase yaw into the turn by raising. Increase yaw out of the turn by decreasing. Values outside of 0.9-1.1 range indicate airspeed calibration problems.
// @Range: 0.8 1.2
// @Increment: 0.05
// @Param: RLL
// @DisplayName: Yaw coordination gain
// @Description: Gain to the yaw rate required to keep it consistent with the turn rate in a coordinated turn. Corrects for yaw tendencies after the turn is established. Increase yaw into the turn by raising. Increase yaw out of the turn by decreasing. Values outside of 0.9-1.1 range indicate airspeed calibration problems.
// @Range: 0.8 1.2
// @Increment: 0.05
// @User: Advanced
AP_GROUPINFO("RLL", 3, AP_YawController, _K_FF, 1),
AP_GROUPINFO("RLL", 3, AP_YawController, _K_FF, 1),
/*
Note: index 4 should not be used - it was used for an incorrect
@ -63,122 +63,121 @@ const AP_Param::GroupInfo AP_YawController::var_info[] = {
*/
// @Param: IMAX
// @DisplayName: Integrator limit
// @Description: Limit of yaw integrator gain in centi-degrees of servo travel. Servos are assumed to have +/- 4500 centi-degrees of travel, so a value of 1500 allows trim of up to 1/3 of servo travel range.
// @Range: 0 4500
// @Increment: 1
// @User: Advanced
AP_GROUPINFO("IMAX", 5, AP_YawController, _imax, 1500),
// @Param: IMAX
// @DisplayName: Integrator limit
// @Description: Limit of yaw integrator gain in centi-degrees of servo travel. Servos are assumed to have +/- 4500 centi-degrees of travel, so a value of 1500 allows trim of up to 1/3 of servo travel range.
// @Range: 0 4500
// @Increment: 1
// @User: Advanced
AP_GROUPINFO("IMAX", 5, AP_YawController, _imax, 1500),
AP_GROUPEND
AP_GROUPEND
};
int32_t AP_YawController::get_servo_out(float scaler, bool disable_integrator)
{
uint32_t tnow = AP_HAL::millis();
uint32_t dt = tnow - _last_t;
if (_last_t == 0 || dt > 1000) {
dt = 0;
}
_last_t = tnow;
uint32_t tnow = AP_HAL::millis();
uint32_t dt = tnow - _last_t;
if (_last_t == 0 || dt > 1000) {
dt = 0;
}
_last_t = tnow;
int16_t aspd_min = aparm.airspeed_min;
if (aspd_min < 1) {
aspd_min = 1;
}
float delta_time = (float) dt / 1000.0f;
// Calculate yaw rate required to keep up with a constant height coordinated turn
float aspeed;
float rate_offset;
float bank_angle = AP::ahrs().roll;
// limit bank angle between +- 80 deg if right way up
if (fabsf(bank_angle) < 1.5707964f) {
bank_angle = constrain_float(bank_angle,-1.3962634f,1.3962634f);
}
float delta_time = (float) dt / 1000.0f;
// Calculate yaw rate required to keep up with a constant height coordinated turn
float aspeed;
float rate_offset;
float bank_angle = AP::ahrs().roll;
// limit bank angle between +- 80 deg if right way up
if (fabsf(bank_angle) < 1.5707964f) {
bank_angle = constrain_float(bank_angle,-1.3962634f,1.3962634f);
}
const AP_AHRS &_ahrs = AP::ahrs();
if (!_ahrs.airspeed_estimate(aspeed)) {
// If no airspeed available use average of min and max
if (!_ahrs.airspeed_estimate(aspeed)) {
// If no airspeed available use average of min and max
aspeed = 0.5f*(float(aspd_min) + float(aparm.airspeed_max));
}
rate_offset = (GRAVITY_MSS / MAX(aspeed , float(aspd_min))) * sinf(bank_angle) * _K_FF;
}
rate_offset = (GRAVITY_MSS / MAX(aspeed, float(aspd_min))) * sinf(bank_angle) * _K_FF;
// Get body rate vector (radians/sec)
float omega_z = _ahrs.get_gyro().z;
// Get the accln vector (m/s^2)
float accel_y = AP::ins().get_accel().y;
float omega_z = _ahrs.get_gyro().z;
// Subtract the steady turn component of rate from the measured rate
// to calculate the rate relative to the turn requirement in degrees/sec
float rate_hp_in = ToDeg(omega_z - rate_offset);
// Apply a high-pass filter to the rate to washout any steady state error
// due to bias errors in rate_offset
// Use a cut-off frequency of omega = 0.2 rad/sec
// Could make this adjustable by replacing 0.9960080 with (1 - omega * dt)
float rate_hp_out = 0.9960080f * _last_rate_hp_out + rate_hp_in - _last_rate_hp_in;
_last_rate_hp_out = rate_hp_out;
_last_rate_hp_in = rate_hp_in;
// Get the accln vector (m/s^2)
float accel_y = AP::ins().get_accel().y;
//Calculate input to integrator
float integ_in = - _K_I * (_K_A * accel_y + rate_hp_out);
// Apply integrator, but clamp input to prevent control saturation and freeze integrator below min FBW speed
// Don't integrate if in stabilise mode as the integrator will wind up against the pilots inputs
// Don't integrate if _K_D is zero as integrator will keep winding up
if (!disable_integrator && _K_D > 0) {
//only integrate if airspeed above min value
if (aspeed > float(aspd_min))
{
// prevent the integrator from increasing if surface defln demand is above the upper limit
if (_last_out < -45) {
_integrator += MAX(integ_in * delta_time , 0);
// Subtract the steady turn component of rate from the measured rate
// to calculate the rate relative to the turn requirement in degrees/sec
float rate_hp_in = ToDeg(omega_z - rate_offset);
// Apply a high-pass filter to the rate to washout any steady state error
// due to bias errors in rate_offset
// Use a cut-off frequency of omega = 0.2 rad/sec
// Could make this adjustable by replacing 0.9960080 with (1 - omega * dt)
float rate_hp_out = 0.9960080f * _last_rate_hp_out + rate_hp_in - _last_rate_hp_in;
_last_rate_hp_out = rate_hp_out;
_last_rate_hp_in = rate_hp_in;
//Calculate input to integrator
float integ_in = - _K_I * (_K_A * accel_y + rate_hp_out);
// Apply integrator, but clamp input to prevent control saturation and freeze integrator below min FBW speed
// Don't integrate if in stabilise mode as the integrator will wind up against the pilots inputs
// Don't integrate if _K_D is zero as integrator will keep winding up
if (!disable_integrator && _K_D > 0) {
//only integrate if airspeed above min value
if (aspeed > float(aspd_min)) {
// prevent the integrator from increasing if surface defln demand is above the upper limit
if (_last_out < -45) {
_integrator += MAX(integ_in * delta_time, 0);
} else if (_last_out > 45) {
// prevent the integrator from decreasing if surface defln demand is below the lower limit
_integrator += MIN(integ_in * delta_time , 0);
} else {
_integrator += MIN(integ_in * delta_time, 0);
} else {
_integrator += integ_in * delta_time;
}
}
} else {
_integrator = 0;
}
}
} else {
_integrator = 0;
}
if (_K_D < 0.0001f) {
// yaw damping is disabled, and the integrator is scaled by damping, so return 0
return 0;
}
// Scale the integration limit
float intLimScaled = _imax * 0.01f / (_K_D * scaler * scaler);
// Constrain the integrator state
_integrator = constrain_float(_integrator, -intLimScaled, intLimScaled);
// 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)
_pid_info.I = _K_D * _integrator * scaler * scaler;
_pid_info.D = _K_D * (-rate_hp_out) * scaler * scaler;
_last_out = _pid_info.I + _pid_info.D;
// Convert to centi-degrees and constrain
return constrain_float(_last_out * 100, -4500, 4500);
// 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)
_pid_info.I = _K_D * _integrator * scaler * scaler;
_pid_info.D = _K_D * (-rate_hp_out) * scaler * scaler;
_last_out = _pid_info.I + _pid_info.D;
// Convert to centi-degrees and constrain
return constrain_float(_last_out * 100, -4500, 4500);
}
void AP_YawController::reset_I()
{
_integrator = 0;
_integrator = 0;
}

41
libraries/APM_Control/AP_YawController.h
View File

@ -5,7 +5,8 @@
#include <AP_Logger/AP_Logger.h>
#include <cmath>
class AP_YawController {
class AP_YawController
{
public:
AP_YawController(const AP_Vehicle::FixedWing &parms)
: aparm(parms)
@ -20,36 +21,40 @@ public:
AP_YawController(const AP_YawController &other) = delete;
AP_YawController &operator=(const AP_YawController&) = delete;
int32_t get_servo_out(float scaler, bool disable_integrator);
int32_t get_servo_out(float scaler, bool disable_integrator);
void reset_I();
void reset_I();
/*
reduce the integrator, used when we have a low scale factor in a quadplane hover
*/
void decay_I() {
void decay_I()
{
// this reduces integrator by 95% over 2s
_pid_info.I *= 0.995f;
}
const AP_Logger::PID_Info& get_pid_info(void) const {return _pid_info; }
static const struct AP_Param::GroupInfo var_info[];
const AP_Logger::PID_Info& get_pid_info(void) const
{
return _pid_info;
}
static const struct AP_Param::GroupInfo var_info[];
private:
const AP_Vehicle::FixedWing &aparm;
AP_Float _K_A;
AP_Float _K_I;
AP_Float _K_D;
AP_Float _K_FF;
AP_Float _K_A;
AP_Float _K_I;
AP_Float _K_D;
AP_Float _K_FF;
AP_Int16 _imax;
uint32_t _last_t;
float _last_out;
float _last_rate_hp_out;
float _last_rate_hp_in;
float _K_D_last;
uint32_t _last_t;
float _last_out;
float _last_rate_hp_out;
float _last_rate_hp_in;
float _K_D_last;
float _integrator;
float _integrator;
AP_Logger::PID_Info _pid_info;
AP_Logger::PID_Info _pid_info;
};