From ff9bbc110c16aa40bec756c45371ea120571f84d Mon Sep 17 00:00:00 2001 From: george Date: Sat, 4 Jul 2020 16:00:51 -0700 Subject: [PATCH 1/3] 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. --- .../fw_att_control/ecl_pitch_controller.cpp | 10 ++-- .../fw_att_control/ecl_pitch_controller.h | 2 +- .../fw_att_control/ecl_roll_controller.cpp | 24 +++++---- .../fw_att_control/ecl_roll_controller.h | 2 +- .../fw_att_control/ecl_yaw_controller.cpp | 49 +++++++++++-------- .../fw_att_control/ecl_yaw_controller.h | 3 +- 6 files changed, 52 insertions(+), 38 deletions(-) diff --git a/src/modules/fw_att_control/ecl_pitch_controller.cpp b/src/modules/fw_att_control/ecl_pitch_controller.cpp index 015cf8bbb578..5243f09dddcf 100644 --- a/src/modules/fw_att_control/ecl_pitch_controller.cpp +++ b/src/modules/fw_att_control/ecl_pitch_controller.cpp @@ -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 #include #include #include @@ -93,11 +92,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 +117,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); } diff --git a/src/modules/fw_att_control/ecl_pitch_controller.h b/src/modules/fw_att_control/ecl_pitch_controller.h index 459ef1e1e64d..7e993d4d30f7 100644 --- a/src/modules/fw_att_control/ecl_pitch_controller.h +++ b/src/modules/fw_att_control/ecl_pitch_controller.h @@ -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 diff --git a/src/modules/fw_att_control/ecl_roll_controller.cpp b/src/modules/fw_att_control/ecl_roll_controller.cpp index e6924ea35087..ef2608d19eaf 100644 --- a/src/modules/fw_att_control/ecl_roll_controller.cpp +++ b/src/modules/fw_att_control/ecl_roll_controller.cpp @@ -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,17 @@ 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))) { + + PX4_WARN("not controlling 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 +89,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 +113,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 +129,4 @@ float ECL_RollController::control_euler_rate(const struct ECL_ControlData &ctl_d set_bodyrate_setpoint(_bodyrate_setpoint); return control_bodyrate(ctl_data); - } diff --git a/src/modules/fw_att_control/ecl_roll_controller.h b/src/modules/fw_att_control/ecl_roll_controller.h index e0fe9253254e..5a85710ab2f8 100644 --- a/src/modules/fw_att_control/ecl_roll_controller.h +++ b/src/modules/fw_att_control/ecl_roll_controller.h @@ -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 diff --git a/src/modules/fw_att_control/ecl_yaw_controller.cpp b/src/modules/fw_att_control/ecl_yaw_controller.cpp index 33f54fa6b264..4c6a5790801c 100644 --- a/src/modules/fw_att_control/ecl_yaw_controller.cpp +++ b/src/modules/fw_att_control/ecl_yaw_controller.cpp @@ -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 @@ -72,6 +72,7 @@ float ECL_YawController::control_attitude_impl_openloop(const struct ECL_Control PX4_ISFINITE(ctl_data.roll_rate_setpoint) && PX4_ISFINITE(ctl_data.pitch_rate_setpoint))) { + PX4_WARN("not controlling yaw"); return _rate_setpoint; } @@ -118,9 +119,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 +143,31 @@ float ECL_YawController::control_bodyrate(const struct ECL_ControlData &ctl_data lock_integrator = true; } - /* input conditioning */ - float airspeed = ctl_data.airspeed; + /* Close the acceleration loop if _coordinated_method wants this: change body_rate setpoint */ + if (_coordinated_method == COORD_METHOD_CLOSEACC) { - 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); + /* input conditioning */ + float airspeed = ctl_data.airspeed; - } else if (airspeed < ctl_data.airspeed_min) { - airspeed = ctl_data.airspeed_min; - } + 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,9 +186,10 @@ 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); } @@ -203,5 +211,4 @@ float ECL_YawController::control_euler_rate(const struct ECL_ControlData &ctl_da set_bodyrate_setpoint(_bodyrate_setpoint); return control_bodyrate(ctl_data); - } diff --git a/src/modules/fw_att_control/ecl_yaw_controller.h b/src/modules/fw_att_control/ecl_yaw_controller.h index 5ec6cd6f26e1..f560b115adfd 100644 --- a/src/modules/fw_att_control/ecl_yaw_controller.h +++ b/src/modules/fw_att_control/ecl_yaw_controller.h @@ -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 From 53bf6ca7973a916f1aefd42813f2d4fa5c27d2f8 Mon Sep 17 00:00:00 2001 From: george Date: Sun, 5 Jul 2020 11:57:29 -0700 Subject: [PATCH 2/3] 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 --- .../fw_att_control/ecl_yaw_controller.cpp | 46 ------------------- .../fw_att_control/ecl_yaw_controller.h | 23 ---------- 2 files changed, 69 deletions(-) diff --git a/src/modules/fw_att_control/ecl_yaw_controller.cpp b/src/modules/fw_att_control/ecl_yaw_controller.cpp index 4c6a5790801c..e2338ff6497a 100644 --- a/src/modules/fw_att_control/ecl_yaw_controller.cpp +++ b/src/modules/fw_att_control/ecl_yaw_controller.cpp @@ -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) && @@ -143,24 +123,6 @@ float ECL_YawController::control_bodyrate(const struct ECL_ControlData &ctl_data lock_integrator = true; } - /* Close the acceleration loop if _coordinated_method wants this: change body_rate setpoint */ - if (_coordinated_method == COORD_METHOD_CLOSEACC) { - - /* 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; - } - - // 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; @@ -194,14 +156,6 @@ float ECL_YawController::control_bodyrate(const struct ECL_ControlData &ctl_data 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) */ diff --git a/src/modules/fw_att_control/ecl_yaw_controller.h b/src/modules/fw_att_control/ecl_yaw_controller.h index f560b115adfd..b36ca09614de 100644 --- a/src/modules/fw_att_control/ecl_yaw_controller.h +++ b/src/modules/fw_att_control/ecl_yaw_controller.h @@ -62,32 +62,9 @@ class ECL_YawController : 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 From 429021e3fe5f2f51ee844c1fc124c7af2972aed4 Mon Sep 17 00:00:00 2001 From: george Date: Tue, 7 Jul 2020 10:24:05 -0700 Subject: [PATCH 3/3] Remove PX4_WARN messages --- src/modules/fw_att_control/ecl_pitch_controller.cpp | 1 - src/modules/fw_att_control/ecl_roll_controller.cpp | 1 - src/modules/fw_att_control/ecl_yaw_controller.cpp | 1 - 3 files changed, 3 deletions(-) diff --git a/src/modules/fw_att_control/ecl_pitch_controller.cpp b/src/modules/fw_att_control/ecl_pitch_controller.cpp index 5243f09dddcf..4fcd88c20462 100644 --- a/src/modules/fw_att_control/ecl_pitch_controller.cpp +++ b/src/modules/fw_att_control/ecl_pitch_controller.cpp @@ -52,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; } diff --git a/src/modules/fw_att_control/ecl_roll_controller.cpp b/src/modules/fw_att_control/ecl_roll_controller.cpp index ef2608d19eaf..779f76e72ed1 100644 --- a/src/modules/fw_att_control/ecl_roll_controller.cpp +++ b/src/modules/fw_att_control/ecl_roll_controller.cpp @@ -49,7 +49,6 @@ float ECL_RollController::control_attitude(const struct ECL_ControlData &ctl_dat if (!(PX4_ISFINITE(ctl_data.roll_setpoint) && PX4_ISFINITE(ctl_data.roll))) { - PX4_WARN("not controlling roll"); return _rate_setpoint; } diff --git a/src/modules/fw_att_control/ecl_yaw_controller.cpp b/src/modules/fw_att_control/ecl_yaw_controller.cpp index e2338ff6497a..e4c5afd82f3a 100644 --- a/src/modules/fw_att_control/ecl_yaw_controller.cpp +++ b/src/modules/fw_att_control/ecl_yaw_controller.cpp @@ -52,7 +52,6 @@ float ECL_YawController::control_attitude(const struct ECL_ControlData &ctl_data PX4_ISFINITE(ctl_data.roll_rate_setpoint) && PX4_ISFINITE(ctl_data.pitch_rate_setpoint))) { - PX4_WARN("not controlling yaw"); return _rate_setpoint; }