diff --git a/src/modules/fw_att_control/CMakeLists.txt b/src/modules/fw_att_control/CMakeLists.txt index a1f7a7b09a..0414c9e30b 100644 --- a/src/modules/fw_att_control/CMakeLists.txt +++ b/src/modules/fw_att_control/CMakeLists.txt @@ -42,7 +42,7 @@ px4_add_module( fw_pitch_controller.cpp fw_roll_controller.cpp ecl_wheel_controller.cpp - ecl_yaw_controller.cpp + fw_yaw_controller.cpp DEPENDS px4_work_queue ) diff --git a/src/modules/fw_att_control/FixedwingAttitudeControl.cpp b/src/modules/fw_att_control/FixedwingAttitudeControl.cpp index 086c38236b..bb1c5c8c1f 100644 --- a/src/modules/fw_att_control/FixedwingAttitudeControl.cpp +++ b/src/modules/fw_att_control/FixedwingAttitudeControl.cpp @@ -345,7 +345,8 @@ void FixedwingAttitudeControl::Run() euler_angles.theta()); _pitch_ctrl.control_pitch(_att_sp.pitch_body, _yaw_ctrl.get_euler_rate_setpoint(), euler_angles.phi(), euler_angles.theta()); - _yaw_ctrl.control_attitude(dt, control_input); + _yaw_ctrl.control_yaw(_att_sp.roll_body, _pitch_ctrl.get_euler_rate_setpoint(), euler_angles.phi(), + euler_angles.theta(), get_airspeed_constrained()); if (wheel_control) { _wheel_ctrl.control_attitude(dt, control_input); diff --git a/src/modules/fw_att_control/FixedwingAttitudeControl.hpp b/src/modules/fw_att_control/FixedwingAttitudeControl.hpp index e70cf27738..320649e899 100644 --- a/src/modules/fw_att_control/FixedwingAttitudeControl.hpp +++ b/src/modules/fw_att_control/FixedwingAttitudeControl.hpp @@ -37,7 +37,7 @@ #include "fw_pitch_controller.h" #include "fw_roll_controller.h" #include "ecl_wheel_controller.h" -#include "ecl_yaw_controller.h" +#include "fw_yaw_controller.h" #include #include #include @@ -161,7 +161,7 @@ private: RollController _roll_ctrl; PitchController _pitch_ctrl; - ECL_YawController _yaw_ctrl; + YawController _yaw_ctrl; ECL_WheelController _wheel_ctrl; void parameters_update(); diff --git a/src/modules/fw_att_control/ecl_controller.cpp b/src/modules/fw_att_control/ecl_controller.cpp index fd41d36255..324ff8c376 100644 --- a/src/modules/fw_att_control/ecl_controller.cpp +++ b/src/modules/fw_att_control/ecl_controller.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2013 Estimation and Control Library (ECL). All rights reserved. + * Copyright (c) 2020-2022 PX4 Development Team. 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/fw_yaw_controller.cpp similarity index 65% rename from src/modules/fw_att_control/ecl_yaw_controller.cpp rename to src/modules/fw_att_control/fw_yaw_controller.cpp index f49b1c9edd..4f27e8a610 100644 --- a/src/modules/fw_att_control/ecl_yaw_controller.cpp +++ b/src/modules/fw_att_control/fw_yaw_controller.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2013-2020 Estimation and Control Library (ECL). All rights reserved. + * Copyright (c) 2020-2022 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -32,24 +32,24 @@ ****************************************************************************/ /** - * @file ecl_yaw_controller.cpp - * Implementation of a simple orthogonal coordinated turn yaw PID controller. - * - * Authors and acknowledgements in header. + * @file fw_yaw_controller.cpp + * Implementation of a simple coordinated turn yaw controller. */ -#include "ecl_yaw_controller.h" +#include "fw_yaw_controller.h" #include #include #include -float ECL_YawController::control_attitude(const float dt, const ECL_ControlData &ctl_data) +float YawController::control_yaw(float roll_setpoint, float euler_pitch_rate_setpoint, float roll, float pitch, + float airspeed) { /* Do not calculate control signal with bad inputs */ - if (!(PX4_ISFINITE(ctl_data.roll) && - PX4_ISFINITE(ctl_data.pitch) && - PX4_ISFINITE(ctl_data.euler_pitch_rate_setpoint) && - PX4_ISFINITE(ctl_data.airspeed_constrained))) { + if (!(PX4_ISFINITE(roll_setpoint) && + PX4_ISFINITE(roll) && + PX4_ISFINITE(pitch) && + PX4_ISFINITE(euler_pitch_rate_setpoint) && + PX4_ISFINITE(airspeed))) { return _body_rate_setpoint; } @@ -58,41 +58,40 @@ float ECL_YawController::control_attitude(const float dt, const ECL_ControlData bool inverted = false; /* roll is used as feedforward term and inverted flight needs to be considered */ - if (fabsf(ctl_data.roll) < math::radians(90.0f)) { + if (fabsf(roll) < math::radians(90.f)) { /* not inverted, but numerically still potentially close to infinity */ - constrained_roll = math::constrain(ctl_data.roll, math::radians(-80.0f), math::radians(80.0f)); + constrained_roll = math::constrain(roll, math::radians(-80.f), math::radians(80.f)); } else { inverted = true; // inverted flight, constrain on the two extremes of -pi..+pi to avoid infinity //note: the ranges are extended by 10 deg here to avoid numeric resolution effects - if (ctl_data.roll > 0.0f) { + if (roll > 0.f) { /* right hemisphere */ - constrained_roll = math::constrain(ctl_data.roll, math::radians(100.0f), math::radians(180.0f)); + constrained_roll = math::constrain(roll, math::radians(100.f), math::radians(180.f)); } else { /* left hemisphere */ - constrained_roll = math::constrain(ctl_data.roll, math::radians(-180.0f), math::radians(-100.0f)); + constrained_roll = math::constrain(roll, math::radians(-180.f), math::radians(-100.f)); } } - constrained_roll = math::constrain(constrained_roll, -fabsf(ctl_data.roll_setpoint), fabsf(ctl_data.roll_setpoint)); + constrained_roll = math::constrain(constrained_roll, -fabsf(roll_setpoint), fabsf(roll_setpoint)); if (!inverted) { /* Calculate desired yaw rate from coordinated turn constraint / (no side forces) */ - _euler_rate_setpoint = tanf(constrained_roll) * cosf(ctl_data.pitch) * CONSTANTS_ONE_G / ctl_data.airspeed_constrained; + _euler_rate_setpoint = tanf(constrained_roll) * cosf(pitch) * CONSTANTS_ONE_G / airspeed; /* Transform setpoint to body angular rates (jacobian) */ - const float yaw_body_rate_setpoint_raw = -sinf(ctl_data.roll) * ctl_data.euler_pitch_rate_setpoint + - cosf(ctl_data.roll) * cosf(ctl_data.pitch) * _euler_rate_setpoint; + const float yaw_body_rate_setpoint_raw = -sinf(roll) * euler_pitch_rate_setpoint + + cosf(roll) * cosf(pitch) * _euler_rate_setpoint; _body_rate_setpoint = math::constrain(yaw_body_rate_setpoint_raw, -_max_rate, _max_rate); } if (!PX4_ISFINITE(_body_rate_setpoint)) { - PX4_WARN("yaw rate sepoint not finite"); - _body_rate_setpoint = 0.0f; + _body_rate_setpoint = 0.f; } return _body_rate_setpoint; diff --git a/src/modules/fw_att_control/ecl_yaw_controller.h b/src/modules/fw_att_control/fw_yaw_controller.h similarity index 65% rename from src/modules/fw_att_control/ecl_yaw_controller.h rename to src/modules/fw_att_control/fw_yaw_controller.h index 08578ba083..7e97618a26 100644 --- a/src/modules/fw_att_control/ecl_yaw_controller.h +++ b/src/modules/fw_att_control/fw_yaw_controller.h @@ -32,11 +32,8 @@ ****************************************************************************/ /** - * @file ecl_yaw_controller.h - * Definition of a simple orthogonal coordinated turn yaw PID controller. - * - * @author Lorenz Meier - * @author Thomas Gubler + * @file fw_yaw_controller.h + * Definition of a simple coordinated turn controller. * * Acknowledgements: * @@ -46,27 +43,37 @@ * Jonathan Challinger, 2012. */ -#ifndef ECL_YAW_CONTROLLER_H -#define ECL_YAW_CONTROLLER_H +#ifndef FW_YAW_CONTROLLER_H +#define FW_YAW_CONTROLLER_H -#include "ecl_controller.h" - -class ECL_YawController : - public ECL_Controller +class YawController { public: - ECL_YawController() = default; - ~ECL_YawController() = default; + YawController() = default; + ~YawController() = default; /** - * @brief Calculates both euler and body yaw rate setpoints. + * @brief Calculates both euler and body yaw rate setpoints for coordinated turn based on current attitude and airspeed * - * @param dt Time step [s] - * @param ctrl_data Various control inputs (attitude, body rates, attitdue stepoints, euler rate setpoints, current speeed) - * @return Yaw body rate setpoint [rad/s] + * @param roll_setpoint roll setpoint [rad] + * @param euler_pitch_rate_setpoint euler pitch rate setpoint [rad/s] + * @param roll estimated roll [rad] + * @param pitch estimated pitch [rad] + * @param airspeed airspeed [m/s] + * @return Roll body rate setpoint [rad/s] */ - float control_attitude(const float dt, const ECL_ControlData &ctl_data) override; + float control_yaw(float roll_setpoint, float euler_pitch_rate_setpoint, float roll, float pitch, + float airspeed); + void set_max_rate(float max_rate) { _max_rate = max_rate; } + + float get_euler_rate_setpoint() { return _euler_rate_setpoint; } + float get_body_rate_setpoint() { return _body_rate_setpoint; } + +private: + float _max_rate; + float _euler_rate_setpoint; + float _body_rate_setpoint; }; -#endif // ECL_YAW_CONTROLLER_H +#endif // FW_YAW_CONTROLLER_H