From 7e467f7121cf354165e6202f87f1db5d3893eef5 Mon Sep 17 00:00:00 2001 From: Silvan Fuhrer Date: Thu, 4 May 2023 21:29:11 +0200 Subject: [PATCH] FW att controller: roll controller: seperate from ecl_controller Signed-off-by: Silvan Fuhrer --- src/modules/fw_att_control/CMakeLists.txt | 2 +- .../FixedwingAttitudeControl.cpp | 3 +- .../FixedwingAttitudeControl.hpp | 4 +- ..._controller.cpp => fw_roll_controller.cpp} | 29 +++++------ ...roll_controller.h => fw_roll_controller.h} | 51 ++++++++++--------- 5 files changed, 43 insertions(+), 46 deletions(-) rename src/modules/fw_att_control/{ecl_roll_controller.cpp => fw_roll_controller.cpp} (74%) rename src/modules/fw_att_control/{ecl_roll_controller.h => fw_roll_controller.h} (65%) diff --git a/src/modules/fw_att_control/CMakeLists.txt b/src/modules/fw_att_control/CMakeLists.txt index a183dd8a95..cdd5ba5870 100644 --- a/src/modules/fw_att_control/CMakeLists.txt +++ b/src/modules/fw_att_control/CMakeLists.txt @@ -40,7 +40,7 @@ px4_add_module( ecl_controller.cpp ecl_pitch_controller.cpp - ecl_roll_controller.cpp + fw_roll_controller.cpp ecl_wheel_controller.cpp ecl_yaw_controller.cpp DEPENDS diff --git a/src/modules/fw_att_control/FixedwingAttitudeControl.cpp b/src/modules/fw_att_control/FixedwingAttitudeControl.cpp index 3f62b98fa9..eb442c670c 100644 --- a/src/modules/fw_att_control/FixedwingAttitudeControl.cpp +++ b/src/modules/fw_att_control/FixedwingAttitudeControl.cpp @@ -341,7 +341,8 @@ void FixedwingAttitudeControl::Run() if (_vcontrol_mode.flag_control_attitude_enabled && _in_fw_or_transition_wo_tailsitter_transition) { if (PX4_ISFINITE(_att_sp.roll_body) && PX4_ISFINITE(_att_sp.pitch_body)) { - _roll_ctrl.control_attitude(dt, control_input); + _roll_ctrl.control_roll(_att_sp.roll_body, _yaw_ctrl.get_euler_rate_setpoint(), euler_angles.phi(), + euler_angles.theta()); _pitch_ctrl.control_attitude(dt, control_input); _yaw_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 eba981d624..14d1452c72 100644 --- a/src/modules/fw_att_control/FixedwingAttitudeControl.hpp +++ b/src/modules/fw_att_control/FixedwingAttitudeControl.hpp @@ -35,7 +35,7 @@ #include #include "ecl_pitch_controller.h" -#include "ecl_roll_controller.h" +#include "fw_roll_controller.h" #include "ecl_wheel_controller.h" #include "ecl_yaw_controller.h" #include @@ -159,7 +159,7 @@ private: ) - ECL_RollController _roll_ctrl; + RollController _roll_ctrl; ECL_PitchController _pitch_ctrl; ECL_YawController _yaw_ctrl; ECL_WheelController _wheel_ctrl; diff --git a/src/modules/fw_att_control/ecl_roll_controller.cpp b/src/modules/fw_att_control/fw_roll_controller.cpp similarity index 74% rename from src/modules/fw_att_control/ecl_roll_controller.cpp rename to src/modules/fw_att_control/fw_roll_controller.cpp index d283389df0..b03dbffb2c 100644 --- a/src/modules/fw_att_control/ecl_roll_controller.cpp +++ b/src/modules/fw_att_control/fw_roll_controller.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2013-2020 Estimation and Control Library (ECL). All rights reserved. + * Copyright (c) 2013-2023 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 @@ -32,37 +32,32 @@ ****************************************************************************/ /** - * @file ecl_roll_controller.cpp - * Implementation of a simple orthogonal roll PID controller. - * - * Authors and acknowledgements in header. + * @file fw_roll_controller.cpp + * Implementation of a simple roll P controller. */ -#include "ecl_roll_controller.h" +#include "fw_roll_controller.h" #include #include #include -float ECL_RollController::control_attitude(const float dt, const ECL_ControlData &ctl_data) +float RollController::control_roll(float roll_setpoint, float euler_yaw_rate_setpoint, float roll, float pitch) { /* Do not calculate control signal with bad inputs */ - if (!(PX4_ISFINITE(ctl_data.roll_setpoint) && - PX4_ISFINITE(ctl_data.euler_yaw_rate_setpoint) && - PX4_ISFINITE(ctl_data.pitch) && - PX4_ISFINITE(ctl_data.roll))) { + if (!(PX4_ISFINITE(roll_setpoint) && + PX4_ISFINITE(euler_yaw_rate_setpoint) && + PX4_ISFINITE(pitch) && + PX4_ISFINITE(roll))) { return _body_rate_setpoint; } - /* Calculate the error */ - float roll_error = ctl_data.roll_setpoint - ctl_data.roll; - - /* Apply P controller: rate setpoint from current error and time constant */ + const float roll_error = roll_setpoint - roll; _euler_rate_setpoint = roll_error / _tc; /* Transform setpoint to body angular rates (jacobian) */ - const float roll_body_rate_setpoint_raw = _euler_rate_setpoint - sinf(ctl_data.pitch) * - ctl_data.euler_yaw_rate_setpoint; + const float roll_body_rate_setpoint_raw = _euler_rate_setpoint - sinf(pitch) * + euler_yaw_rate_setpoint; _body_rate_setpoint = math::constrain(roll_body_rate_setpoint_raw, -_max_rate, _max_rate); return _body_rate_setpoint; diff --git a/src/modules/fw_att_control/ecl_roll_controller.h b/src/modules/fw_att_control/fw_roll_controller.h similarity index 65% rename from src/modules/fw_att_control/ecl_roll_controller.h rename to src/modules/fw_att_control/fw_roll_controller.h index 63cc905c3c..a14277e3f0 100644 --- a/src/modules/fw_att_control/ecl_roll_controller.h +++ b/src/modules/fw_att_control/fw_roll_controller.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2020-2022 PX4 Development Team. All rights reserved. + * Copyright (c) 2020-2023 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,40 +32,41 @@ ****************************************************************************/ /** - * @file ecl_roll_controller.h - * Definition of a simple orthogonal roll PID controller. - * - * @author Lorenz Meier - * @author Thomas Gubler - * - * Acknowledgements: - * - * The control design is based on a design - * by Paul Riseborough and Andrew Tridgell, 2013, - * which in turn is based on initial work of - * Jonathan Challinger, 2012. + * @file fw_roll_controller.h + * Definition of a simple roll P controller. */ -#ifndef ECL_ROLL_CONTROLLER_H -#define ECL_ROLL_CONTROLLER_H +#ifndef FW_ROLL_CONTROLLER_H +#define FW_ROLL_CONTROLLER_H -#include "ecl_controller.h" - -class ECL_RollController : - public ECL_Controller +class RollController { public: - ECL_RollController() = default; - ~ECL_RollController() = default; + RollController() = default; + ~RollController() = default; /** * @brief Calculates both euler and body roll rate setpoints. * - * @param dt Time step [s] - * @param ctrl_data Various control inputs (attitude, body rates, attitdue stepoints, euler rate setpoints, current speeed) + * @param roll_setpoint roll setpoint [rad] + * @param euler_yaw_rate_setpoint euler yaw rate setpoint [rad/s] + * @param roll estimated roll [rad] + * @param pitch estimated pitch [rad] * @return Roll body rate setpoint [rad/s] */ - float control_attitude(const float dt, const ECL_ControlData &ctl_data) override; + float control_roll(float roll_setpoint, float euler_yaw_rate_setpoint, float roll, float pitch); + + void set_time_constant(float time_constant) { _tc = time_constant; } + 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 _tc; + float _max_rate; + float _euler_rate_setpoint; + float _body_rate_setpoint; }; -#endif // ECL_ROLL_CONTROLLER_H +#endif // FW_ROLL_CONTROLLER_H