fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-22 12:03: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_pitch_controller.cpp
+6 -5
View File
@@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2013 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
@@ -39,7 +39,6 @@
*/
#include "ecl_pitch_controller.h"
#include <math.h>
#include <float.h>
#include <lib/ecl/geo/geo.h>
#include <mathlib/mathlib.h>
@@ -53,7 +52,6 @@ float ECL_PitchController::control_attitude(const struct ECL_ControlData &ctl_da
PX4_ISFINITE(ctl_data.pitch) &&
PX4_ISFINITE(ctl_data.airspeed))) {
PX4_WARN("not controlling pitch");
return _rate_setpoint;
}
@@ -93,11 +91,13 @@ float ECL_PitchController::control_bodyrate(const struct ECL_ControlData &ctl_da
lock_integrator = true;
}
/* Calculate body angular rate error */
_rate_error = _bodyrate_setpoint - ctl_data.body_y_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
@@ -116,9 +116,10 @@ float ECL_PitchController::control_bodyrate(const struct ECL_ControlData &ctl_da
}
/* 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);
}