diff --git a/src/modules/fw_att_control/ecl_pitch_controller.cpp b/src/modules/fw_att_control/ecl_pitch_controller.cpp index 015cf8bbb5..4fcd88c204 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 @@ -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); } diff --git a/src/modules/fw_att_control/ecl_pitch_controller.h b/src/modules/fw_att_control/ecl_pitch_controller.h index 459ef1e1e6..7e993d4d30 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 e6924ea350..779f76e72e 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,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); - } diff --git a/src/modules/fw_att_control/ecl_roll_controller.h b/src/modules/fw_att_control/ecl_roll_controller.h index e0fe925325..5a85710ab2 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 33f54fa6b2..e4c5afd82f 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 @@ -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); - } diff --git a/src/modules/fw_att_control/ecl_yaw_controller.h b/src/modules/fw_att_control/ecl_yaw_controller.h index 5ec6cd6f26..b36ca09614 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 @@ -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