fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-22 13:43:33 +08:00
Code Issues Packages Projects Releases Wiki Activity

FW attitude control scaling fixes and cleanup (#15256)

Browse Source 
* FW attitude control scaling fixes and cleanup

This commit aligns the scaling better with the derivations in
https://dev.px4.io/master/en/flight_stack/controller_diagrams.html#airspeed-scaling

Integrator terms now scale with IAS^2 (all three axes)

To match the roll and pitch controllers:
- Yaw integrator scale is now applied during accumulation, not to
  integral value (so now FW_YR_IMAX is respected more intuitively)
- Yaw FF term now scale with IAS instead of IAS^2

Also made a number of small changes to make the three files
  (roll, pitch, yaw) 3-way diffable to be clearer about what the
  differences among them are.

* Remove unused yaw coordination method

- "Coordination method" open vs. closed code removed, since closed
  is never used and not actually implemented.
- No change to behavior

* Remove PX4_WARN messages

Co-authored-by: george <george@campsix.com>
This commit is contained in:
George Anderson
2020-07-10 15:54:39 +02:00
committed by GitHub
co-authored by george
parent f792cc7b6b
commit 94326a29bb
6 changed files with 39 additions and 97 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/fw_att_control/ecl_yaw_controller.cpp
+16 -56
View File
@@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2013-2016 Estimation and Control Library (ECL). All rights reserved.
* Copyright (c) 2013-2020 Estimation and Control Library (ECL). All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@@ -45,27 +45,7 @@
float ECL_YawController::control_attitude(const struct ECL_ControlData &ctl_data)
{
switch (_coordinated_method) {
case COORD_METHOD_OPEN:
return control_attitude_impl_openloop(ctl_data);
case COORD_METHOD_CLOSEACC:
return control_attitude_impl_accclosedloop(ctl_data);
default:
static hrt_abstime last_print = 0;
if (hrt_elapsed_time(&last_print) > 5e6) {
PX4_WARN("invalid param setting FW_YCO_METHOD");
last_print = hrt_absolute_time();
}
}
return _rate_setpoint;
}
float ECL_YawController::control_attitude_impl_openloop(const struct ECL_ControlData &ctl_data)
{
/* Do not calculate control signal with bad inputs */
if (!(PX4_ISFINITE(ctl_data.roll) &&
PX4_ISFINITE(ctl_data.pitch) &&
@@ -118,9 +98,13 @@ float ECL_YawController::control_attitude_impl_openloop(const struct ECL_Control
float ECL_YawController::control_bodyrate(const struct ECL_ControlData &ctl_data)
{
/* Do not calculate control signal with bad inputs */
if (!(PX4_ISFINITE(ctl_data.roll) && PX4_ISFINITE(ctl_data.pitch) && PX4_ISFINITE(ctl_data.body_y_rate) &&
PX4_ISFINITE(ctl_data.body_z_rate) && PX4_ISFINITE(ctl_data.pitch_rate_setpoint) &&
PX4_ISFINITE(ctl_data.airspeed_min) && PX4_ISFINITE(ctl_data.airspeed_max) &&
if (!(PX4_ISFINITE(ctl_data.roll) &&
PX4_ISFINITE(ctl_data.pitch) &&
PX4_ISFINITE(ctl_data.body_y_rate) &&
PX4_ISFINITE(ctl_data.body_z_rate) &&
PX4_ISFINITE(ctl_data.pitch_rate_setpoint) &&
PX4_ISFINITE(ctl_data.airspeed_min) &&
PX4_ISFINITE(ctl_data.airspeed_max) &&
PX4_ISFINITE(ctl_data.scaler))) {
return math::constrain(_last_output, -1.0f, 1.0f);
@@ -138,29 +122,13 @@ float ECL_YawController::control_bodyrate(const struct ECL_ControlData &ctl_data
lock_integrator = true;
}
/* input conditioning */
float airspeed = ctl_data.airspeed;
if (!PX4_ISFINITE(airspeed)) {
/* airspeed is NaN, +- INF or not available, pick center of band */
airspeed = 0.5f * (ctl_data.airspeed_min + ctl_data.airspeed_max);
} else if (airspeed < ctl_data.airspeed_min) {
airspeed = ctl_data.airspeed_min;
}
/* Close the acceleration loop if _coordinated_method wants this: change body_rate setpoint */
if (_coordinated_method == COORD_METHOD_CLOSEACC) {
// XXX lateral acceleration needs to go into integrator with a gain
//_bodyrate_setpoint -= (ctl_data.acc_body_y / (airspeed * cosf(ctl_data.pitch)));
}
/* Calculate body angular rate error */
_rate_error = _bodyrate_setpoint - ctl_data.body_z_rate; // body angular rate error
_rate_error = _bodyrate_setpoint - ctl_data.body_z_rate;
if (!lock_integrator && _k_i > 0.0f && airspeed > 0.5f * ctl_data.airspeed_min) {
if (!lock_integrator && _k_i > 0.0f) {
float id = _rate_error * dt;
/* Integral term scales with 1/IAS^2 */
float id = _rate_error * dt * ctl_data.scaler * ctl_data.scaler;
/*
* anti-windup: do not allow integrator to increase if actuator is at limit
@@ -179,21 +147,14 @@ float ECL_YawController::control_bodyrate(const struct ECL_ControlData &ctl_data
}
/* Apply PI rate controller and store non-limited output */
_last_output = (_bodyrate_setpoint * _k_ff + _rate_error * _k_p + _integrator) * ctl_data.scaler *
ctl_data.scaler; //scaler is proportional to 1/airspeed
/* FF terms scales with 1/TAS and P,I with 1/IAS^2 */
_last_output = _bodyrate_setpoint * _k_ff * ctl_data.scaler +
_rate_error * _k_p * ctl_data.scaler * ctl_data.scaler
+ _integrator;
return math::constrain(_last_output, -1.0f, 1.0f);
}
float ECL_YawController::control_attitude_impl_accclosedloop(const struct ECL_ControlData &ctl_data)
{
(void)ctl_data; // unused
/* dont set a rate setpoint */
return 0.0f;
}
float ECL_YawController::control_euler_rate(const struct ECL_ControlData &ctl_data)
{
/* Transform setpoint to body angular rates (jacobian) */
@@ -203,5 +164,4 @@ float ECL_YawController::control_euler_rate(const struct ECL_ControlData &ctl_da
set_bodyrate_setpoint(_bodyrate_setpoint);
return control_bodyrate(ctl_data);
}