fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-07-17 05:00:35 +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 06:54:39 -07:00
committed by GitHub
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_roll_controller.cpp
+13 -10
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
@@ -46,14 +46,16 @@
float ECL_RollController::control_attitude(const struct ECL_ControlData &ctl_data)
{
/* Do not calculate control signal with bad inputs */
if (!(PX4_ISFINITE(ctl_data.roll_setpoint) && PX4_ISFINITE(ctl_data.roll))) {
if (!(PX4_ISFINITE(ctl_data.roll_setpoint) &&
PX4_ISFINITE(ctl_data.roll))) {
return _rate_setpoint;
}
/* Calculate error */
/* Calculate the error */
float roll_error = ctl_data.roll_setpoint - ctl_data.roll;
/* Apply P controller */
/* Apply P controller: rate setpoint from current error and time constant */
_rate_setpoint = roll_error / _tc;
return _rate_setpoint;
@@ -86,15 +88,16 @@ float ECL_RollController::control_bodyrate(const struct ECL_ControlData &ctl_dat
}
/* Calculate body angular rate error */
_rate_error = _bodyrate_setpoint - ctl_data.body_x_rate; //body angular rate error
_rate_error = _bodyrate_setpoint - ctl_data.body_x_rate;
if (!lock_integrator && _k_i > 0.0f) {
float id = _rate_error * dt * ctl_data.scaler;
/* 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
*/
* anti-windup: do not allow integrator to increase if actuator is at limit
*/
if (_last_output < -1.0f) {
/* only allow motion to center: increase value */
id = math::max(id, 0.0f);
@@ -109,9 +112,10 @@ float ECL_RollController::control_bodyrate(const struct ECL_ControlData &ctl_dat
}
/* Apply PI rate controller and store non-limited output */
/* 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; //scaler is proportional to 1/airspeed
+ _integrator;
return math::constrain(_last_output, -1.0f, 1.0f);
}
@@ -124,5 +128,4 @@ float ECL_RollController::control_euler_rate(const struct ECL_ControlData &ctl_d
set_bodyrate_setpoint(_bodyrate_setpoint);
return control_bodyrate(ctl_data);
}