From d4206195c6c91fc348d4df9bbf46d2906d9a0474 Mon Sep 17 00:00:00 2001 From: Silvan Fuhrer Date: Wed, 17 May 2023 16:44:56 +0200 Subject: [PATCH] FW att controller: pitch controller: separate 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_pitch_controller.cpp} | 34 +++++----- ...tch_controller.h => fw_pitch_controller.h} | 63 ++++++++----------- .../fw_att_control/fw_roll_controller.cpp | 2 +- 6 files changed, 46 insertions(+), 62 deletions(-) rename src/modules/fw_att_control/{ecl_pitch_controller.cpp => fw_pitch_controller.cpp} (67%) rename src/modules/fw_att_control/{ecl_pitch_controller.h => fw_pitch_controller.h} (60%) diff --git a/src/modules/fw_att_control/CMakeLists.txt b/src/modules/fw_att_control/CMakeLists.txt index cdd5ba5870..a1f7a7b09a 100644 --- a/src/modules/fw_att_control/CMakeLists.txt +++ b/src/modules/fw_att_control/CMakeLists.txt @@ -39,7 +39,7 @@ px4_add_module( FixedwingAttitudeControl.hpp ecl_controller.cpp - ecl_pitch_controller.cpp + fw_pitch_controller.cpp fw_roll_controller.cpp ecl_wheel_controller.cpp ecl_yaw_controller.cpp diff --git a/src/modules/fw_att_control/FixedwingAttitudeControl.cpp b/src/modules/fw_att_control/FixedwingAttitudeControl.cpp index eb442c670c..086c38236b 100644 --- a/src/modules/fw_att_control/FixedwingAttitudeControl.cpp +++ b/src/modules/fw_att_control/FixedwingAttitudeControl.cpp @@ -343,7 +343,8 @@ void FixedwingAttitudeControl::Run() if (PX4_ISFINITE(_att_sp.roll_body) && PX4_ISFINITE(_att_sp.pitch_body)) { _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); + _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); if (wheel_control) { diff --git a/src/modules/fw_att_control/FixedwingAttitudeControl.hpp b/src/modules/fw_att_control/FixedwingAttitudeControl.hpp index 14d1452c72..e70cf27738 100644 --- a/src/modules/fw_att_control/FixedwingAttitudeControl.hpp +++ b/src/modules/fw_att_control/FixedwingAttitudeControl.hpp @@ -34,7 +34,7 @@ #pragma once #include -#include "ecl_pitch_controller.h" +#include "fw_pitch_controller.h" #include "fw_roll_controller.h" #include "ecl_wheel_controller.h" #include "ecl_yaw_controller.h" @@ -160,7 +160,7 @@ private: ) RollController _roll_ctrl; - ECL_PitchController _pitch_ctrl; + PitchController _pitch_ctrl; ECL_YawController _yaw_ctrl; ECL_WheelController _wheel_ctrl; diff --git a/src/modules/fw_att_control/ecl_pitch_controller.cpp b/src/modules/fw_att_control/fw_pitch_controller.cpp similarity index 67% rename from src/modules/fw_att_control/ecl_pitch_controller.cpp rename to src/modules/fw_att_control/fw_pitch_controller.cpp index 2518a4a5ff..90ad9dce43 100644 --- a/src/modules/fw_att_control/ecl_pitch_controller.cpp +++ b/src/modules/fw_att_control/fw_pitch_controller.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2013-2020 Estimation and Control Library (ECL). 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,38 +32,34 @@ ****************************************************************************/ /** - * @file ecl_pitch_controller.cpp - * Implementation of a simple orthogonal pitch PID controller. - * - * Authors and acknowledgements in header. + * @file fw_pitch_controller.cpp + * Implementation of a simple pitch P controller. */ -#include "ecl_pitch_controller.h" +#include "fw_pitch_controller.h" #include #include #include -float ECL_PitchController::control_attitude(const float dt, const ECL_ControlData &ctl_data) +float PitchController::control_pitch(float pitch_setpoint, float euler_yaw_rate_setpoint, float roll, float pitch) { /* Do not calculate control signal with bad inputs */ - if (!(PX4_ISFINITE(ctl_data.pitch_setpoint) && - PX4_ISFINITE(ctl_data.roll) && - PX4_ISFINITE(ctl_data.pitch) && - PX4_ISFINITE(ctl_data.euler_yaw_rate_setpoint))) { + if (!(PX4_ISFINITE(pitch_setpoint) && + PX4_ISFINITE(euler_yaw_rate_setpoint) && + PX4_ISFINITE(pitch) && + PX4_ISFINITE(roll))) { return _body_rate_setpoint; } - /* Calculate the error */ - float pitch_error = ctl_data.pitch_setpoint - ctl_data.pitch; - - /* Apply P controller: rate setpoint from current error and time constant */ - _euler_rate_setpoint = pitch_error / _tc; + const float pitch_error = pitch_setpoint - pitch; + _euler_rate_setpoint = pitch_error / _tc; /* Transform setpoint to body angular rates (jacobian) */ - const float pitch_body_rate_setpoint_raw = cosf(ctl_data.roll) * _euler_rate_setpoint + - cosf(ctl_data.pitch) * sinf(ctl_data.roll) * ctl_data.euler_yaw_rate_setpoint; - _body_rate_setpoint = math::constrain(pitch_body_rate_setpoint_raw, -_max_rate_neg, _max_rate); + const float pitch_body_rate_setpoint_raw = cosf(roll) * _euler_rate_setpoint + + cosf(pitch) * sinf(roll) * euler_yaw_rate_setpoint; + + _body_rate_setpoint = math::constrain(pitch_body_rate_setpoint_raw, -_max_rate_neg, _max_rate_pos); return _body_rate_setpoint; } diff --git a/src/modules/fw_att_control/ecl_pitch_controller.h b/src/modules/fw_att_control/fw_pitch_controller.h similarity index 60% rename from src/modules/fw_att_control/ecl_pitch_controller.h rename to src/modules/fw_att_control/fw_pitch_controller.h index 97cc9ecb8b..a71168f190 100644 --- a/src/modules/fw_att_control/ecl_pitch_controller.h +++ b/src/modules/fw_att_control/fw_pitch_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,56 +32,43 @@ ****************************************************************************/ /** - * @file ecl_pitch_controller.h - * Definition of a simple orthogonal pitch 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_pitch_controller.h + * Definition of a simple pitch P controller. */ -#ifndef ECL_PITCH_CONTROLLER_H -#define ECL_PITCH_CONTROLLER_H +#ifndef FW_PITCH_CONTROLLER_H +#define FW_PITCH_CONTROLLER_H -#include - -#include "ecl_controller.h" - -class ECL_PitchController : - public ECL_Controller +class PitchController { public: - ECL_PitchController() = default; - ~ECL_PitchController() = default; + PitchController() = default; + ~PitchController() = default; /** * @brief Calculates both euler and body pitch rate setpoints. * - * @param dt Time step [s] - * @param ctrl_data Various control inputs (attitude, body rates, attitdue stepoints, euler rate setpoints, current speeed) + * @param pitch_setpoint pitch setpoint [rad] + * @param euler_yaw_rate_setpoint euler yaw rate setpoint [rad/s] + * @param roll estimated roll [rad] + * @param pitch estimated pitch [rad] * @return Pitch body rate setpoint [rad/s] */ - float control_attitude(const float dt, const ECL_ControlData &ctl_data) override; + float control_pitch(float pitch_setpoint, float euler_yaw_rate_setpoint, float roll, float pitch); - /* Additional Setters */ - void set_max_rate_pos(float max_rate_pos) - { - _max_rate = max_rate_pos; - } + void set_time_constant(float time_constant) { _tc = time_constant; } + void set_max_rate_pos(float max_rate_pos) { _max_rate_pos = max_rate_pos; } + void set_max_rate_neg(float max_rate_neg) { _max_rate_neg = max_rate_neg; } - void set_max_rate_neg(float max_rate_neg) - { - _max_rate_neg = max_rate_neg; - } + float get_euler_rate_setpoint() { return _euler_rate_setpoint; } + float get_body_rate_setpoint() { return _body_rate_setpoint; } -protected: - float _max_rate_neg{0.0f}; +private: + float _tc; + float _max_rate_pos; + float _max_rate_neg; + float _euler_rate_setpoint; + float _body_rate_setpoint; }; -#endif // ECL_PITCH_CONTROLLER_H +#endif // FW_PITCH_CONTROLLER_H diff --git a/src/modules/fw_att_control/fw_roll_controller.cpp b/src/modules/fw_att_control/fw_roll_controller.cpp index b03dbffb2c..659adfa9c2 100644 --- a/src/modules/fw_att_control/fw_roll_controller.cpp +++ b/src/modules/fw_att_control/fw_roll_controller.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2013-2023 Estimation and Control Library (ECL). 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