mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-04-28 22:24:08 +08:00
FW attitude control scaling fixes and cleanup (#15256)
* 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
GitHub
parent
f792cc7b6b
commit
94326a29bb
@ -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);
|
||||
}
|
||||
|
||||
@ -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
|
||||
|
||||
@ -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);
|
||||
|
||||
}
|
||||
|
||||
@ -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
|
||||
|
||||
@ -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);
|
||||
|
||||
}
|
||||
|
||||
@ -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,6 +45,7 @@
|
||||
* which in turn is based on initial work of
|
||||
* Jonathan Challinger, 2012.
|
||||
*/
|
||||
|
||||
#ifndef ECL_YAW_CONTROLLER_H
|
||||
#define ECL_YAW_CONTROLLER_H
|
||||
|
||||
@ -61,32 +62,9 @@ public:
|
||||
float control_euler_rate(const struct ECL_ControlData &ctl_data) override;
|
||||
float control_bodyrate(const struct ECL_ControlData &ctl_data) override;
|
||||
|
||||
/* Additional setters */
|
||||
void set_coordinated_min_speed(float coordinated_min_speed)
|
||||
{
|
||||
_coordinated_min_speed = coordinated_min_speed;
|
||||
}
|
||||
|
||||
void set_coordinated_method(int32_t coordinated_method)
|
||||
{
|
||||
_coordinated_method = coordinated_method;
|
||||
}
|
||||
|
||||
enum {
|
||||
COORD_METHOD_OPEN = 0,
|
||||
COORD_METHOD_CLOSEACC = 1
|
||||
};
|
||||
|
||||
protected:
|
||||
float _coordinated_min_speed{1.0f};
|
||||
float _max_rate{0.0f};
|
||||
|
||||
int32_t _coordinated_method{COORD_METHOD_OPEN};
|
||||
|
||||
float control_attitude_impl_openloop(const struct ECL_ControlData &ctl_data);
|
||||
|
||||
float control_attitude_impl_accclosedloop(const struct ECL_ControlData &ctl_data);
|
||||
|
||||
};
|
||||
|
||||
#endif // ECL_YAW_CONTROLLER_H
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user