From c4330f5a475a7f242d5fe27a969f412a99e61ca4 Mon Sep 17 00:00:00 2001 From: ttechnick Date: Tue, 13 Jan 2026 15:31:13 +0100 Subject: [PATCH] feat(fw_att_control): tilt-tracking controller --- msg/CMakeLists.txt | 1 - msg/versioned/VehicleEulerRatesSetpoint.msg | 5 - src/lib/matrix/matrix/Quaternion.hpp | 42 +++++ src/modules/fw_att_control/CMakeLists.txt | 3 - .../FixedwingAttitudeControl.cpp | 149 ++++++------------ .../FixedwingAttitudeControl.hpp | 21 +-- .../fw_att_control/fw_pitch_controller.cpp | 65 -------- .../fw_att_control/fw_pitch_controller.h | 74 --------- .../fw_att_control/fw_roll_controller.cpp | 64 -------- .../fw_att_control/fw_roll_controller.h | 72 --------- .../fw_att_control/fw_yaw_controller.cpp | 98 ------------ .../fw_att_control/fw_yaw_controller.h | 79 ---------- .../FwLateralLongitudinalControl.cpp | 4 +- src/modules/logger/logged_topics.cpp | 1 - 14 files changed, 99 insertions(+), 579 deletions(-) delete mode 100644 msg/versioned/VehicleEulerRatesSetpoint.msg delete mode 100644 src/modules/fw_att_control/fw_pitch_controller.cpp delete mode 100644 src/modules/fw_att_control/fw_pitch_controller.h delete mode 100644 src/modules/fw_att_control/fw_roll_controller.cpp delete mode 100644 src/modules/fw_att_control/fw_roll_controller.h delete mode 100644 src/modules/fw_att_control/fw_yaw_controller.cpp delete mode 100644 src/modules/fw_att_control/fw_yaw_controller.h diff --git a/msg/CMakeLists.txt b/msg/CMakeLists.txt index a2ad4a4a1b..66a9f7e599 100644 --- a/msg/CMakeLists.txt +++ b/msg/CMakeLists.txt @@ -276,7 +276,6 @@ set(msg_files versioned/VehicleLocalPosition.msg versioned/VehicleOdometry.msg versioned/VehicleRatesSetpoint.msg - versioned/VehicleEulerRatesSetpoint.msg versioned/VehicleStatus.msg versioned/VtolVehicleStatus.msg versioned/Wind.msg diff --git a/msg/versioned/VehicleEulerRatesSetpoint.msg b/msg/versioned/VehicleEulerRatesSetpoint.msg deleted file mode 100644 index 802b025989..0000000000 --- a/msg/versioned/VehicleEulerRatesSetpoint.msg +++ /dev/null @@ -1,5 +0,0 @@ -uint64 timestamp # time since system start (microseconds) - -float32 roll_rate # body frame roll rate setpoint (rad/s) -float32 pitch_rate # body frame pitch rate setpoint (rad/s) -float32 yaw_rate # body frame yaw rate setpoint (rad/s) \ No newline at end of file diff --git a/src/lib/matrix/matrix/Quaternion.hpp b/src/lib/matrix/matrix/Quaternion.hpp index a2637606fd..4ac6243bc6 100644 --- a/src/lib/matrix/matrix/Quaternion.hpp +++ b/src/lib/matrix/matrix/Quaternion.hpp @@ -518,6 +518,48 @@ public: return Vector3(q(1), q(2), q(3)); } + /** + * Corresponding body x-axis to an attitude quaternion / + * first orthogonal unit basis vector + * + * == first column of the equivalent rotation matrix + * but calculated more efficiently than a full conversion + */ + Vector3 dcm_x() const + { + const Quaternion &q = *this; + Vector3 R_x; + const Type a = q(0); + const Type b = q(1); + const Type c = q(2); + const Type d = q(3); + R_x(0) = a * a + b * b - c * c - d * d; + R_x(1) = 2 * (b * c + a * d); + R_x(2) = 2 * (b * d - a * c); + return R_x; + } + + /** + * Corresponding body y-axis to an attitude quaternion / + * second orthogonal unit basis vector + * + * == second column of the equivalent rotation matrix + * but calculated more efficiently than a full conversion + */ + Vector3 dcm_y() const + { + const Quaternion &q = *this; + Vector3 R_y; + const Type a = q(0); + const Type b = q(1); + const Type c = q(2); + const Type d = q(3); + R_y(0) = 2 * (b * c - a * d); + R_y(1) = a * a - b * b + c * c - d * d; + R_y(2) = 2 * (c * d + a * b); + return R_y; + } + /** * Corresponding body z-axis to an attitude quaternion / * last orthogonal unit basis vector diff --git a/src/modules/fw_att_control/CMakeLists.txt b/src/modules/fw_att_control/CMakeLists.txt index 75dd515ddb..40588a3156 100644 --- a/src/modules/fw_att_control/CMakeLists.txt +++ b/src/modules/fw_att_control/CMakeLists.txt @@ -38,10 +38,7 @@ px4_add_module( FixedwingAttitudeControl.cpp FixedwingAttitudeControl.hpp - fw_pitch_controller.cpp - fw_roll_controller.cpp fw_wheel_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 08497b83d0..98567e107f 100644 --- a/src/modules/fw_att_control/FixedwingAttitudeControl.cpp +++ b/src/modules/fw_att_control/FixedwingAttitudeControl.cpp @@ -72,25 +72,10 @@ FixedwingAttitudeControl::init() void FixedwingAttitudeControl::parameters_update() { - _proportional_gain = matrix::Vector3f(1.0f / _param_fw_p_tc.get(), + _proportional_gain = matrix::Vector3f(1.0f / _param_fw_r_tc.get(), 1.0f / _param_fw_p_tc.get(), 0.0f); - _roll_ctrl.set_time_constant(_param_fw_r_tc.get()); - _roll_ctrl.set_max_rate(radians(_param_fw_r_rmax.get())); - - _pitch_ctrl.set_time_constant(_param_fw_p_tc.get()); - _pitch_ctrl.set_max_rate_pos(radians(_param_fw_p_rmax_pos.get())); - _pitch_ctrl.set_max_rate_neg(radians(_param_fw_p_rmax_neg.get())); - - _yaw_ctrl.set_max_rate(radians(_param_fw_y_rmax.get())); - - - _max_roll_rate = radians(_param_fw_r_rmax.get()); - _max_pitch_rate_pos = radians(_param_fw_p_rmax_pos.get()); - _max_pitch_rate_neg = radians(_param_fw_p_rmax_neg.get()); - _max_yaw_rate = radians(_param_fw_y_rmax.get()); - _wheel_ctrl.set_k_p(_param_fw_wr_p.get()); _wheel_ctrl.set_k_i(_param_fw_wr_i.get()); _wheel_ctrl.set_k_ff(_param_fw_wr_ff.get()); @@ -100,10 +85,8 @@ FixedwingAttitudeControl::parameters_update() } void -FixedwingAttitudeControl::vehicle_manual_poll(matrix::Quatf R) +FixedwingAttitudeControl::vehicle_manual_poll(const float yaw_body) { - const matrix::Eulerf euler_angles(R); - float yaw_body = euler_angles.psi(); if (_vcontrol_mode.flag_control_manual_enabled && _in_fw_or_transition_wo_tailsitter_transition) { // Always copy the new manual setpoint, even if it wasn't updated, to fill the actuators with valid values @@ -111,7 +94,7 @@ FixedwingAttitudeControl::vehicle_manual_poll(matrix::Quatf R) if (!_vcontrol_mode.flag_control_climb_rate_enabled && _vcontrol_mode.flag_control_attitude_enabled) { - // STABILIZED mode generate the attitude setpoint from manual user inputs + // STABILIZED mode: setpoint generation const float roll_body = _manual_control_setpoint.roll * radians(_param_fw_man_r_max.get()); @@ -120,11 +103,11 @@ FixedwingAttitudeControl::vehicle_manual_poll(matrix::Quatf R) pitch_body = constrain(pitch_body, -radians(_param_fw_man_p_max.get()), radians(_param_fw_man_p_max.get())); - _att_sp.thrust_body[0] = (_manual_control_setpoint.throttle + 1.f) * .5f; - const Quatf q(Eulerf(roll_body, pitch_body, yaw_body)); q.copyTo(_att_sp.q_d); + _att_sp.thrust_body[0] = (_manual_control_setpoint.throttle + 1.f) * .5f; + _att_sp.timestamp = hrt_absolute_time(); _attitude_sp_pub.publish(_att_sp); @@ -266,9 +249,10 @@ void FixedwingAttitudeControl::Run() /* fill in new attitude data */ _R = R_adapted; } + const matrix::Eulerf euler_angles(_R); - vehicle_manual_poll(_R); + vehicle_manual_poll(euler_angles.psi()); vehicle_attitude_setpoint_poll(); @@ -311,91 +295,61 @@ void FixedwingAttitudeControl::Run() if (q_sp.isAllFinite()) { - - // Current attitude const Quatf q_current(att.q); + const Quatf q_sp_full(q_sp); + const Vector3f ez_world(0.f, 0.f, 1.f); + const Vector3f ex_c = q_current.dcm_x(); + const Vector3f ez_c = q_current.dcm_z(); + const Vector3f ez_sp = q_sp_full.dcm_z(); - // Full desired attitude (setpoint) - const Quatf qd_full = q_sp; + // Tilt-only error quaternion + Quatf q_tilt_err(ez_c, ez_sp); - // --- Scheduling: 0 = normal FW, 1 = near-vertical/full-3D --- - const float pitch = Eulerf(q_current).theta(); // scheduling only - const float s = math::constrain((fabsf(pitch) - radians(60.f)) / radians(20.f), 0.f, 1.f); // 0..1 + // Reduced desired attitude + Quatf q_des_red = (q_tilt_err * q_current).normalized(); + Quatf q_err = (q_current.inversed() * q_des_red).canonical(); + Vector3f e = 2.f * q_err.imag(); - // --- Build reduced desired attitude qd_red: align "tilt" (body Z axis) but don't chase yaw --- - // Note: assumes Quatf::dcm_z() returns body Z axis expressed in world/NED (same convention as PX4 MC). - const Vector3f ez_current = q_current.dcm_z(); // current body Z in world - const Vector3f ez_desired = qd_full.dcm_z(); // desired body Z in world + Vector3f body_rates_setpoint; + body_rates_setpoint(0) = _proportional_gain(0) * e(0); + body_rates_setpoint(1) = _proportional_gain(1) * e(1) * 2.5f; // multiplied by 2.5 to maintain the gains + body_rates_setpoint(2) = 0.f; - // Quaternion rotating ez_current -> ez_desired (tilt correction only) - Quatf q_tilt_err(ez_current, ez_desired); + // Turn coordination + const float V = math::max(get_airspeed_constrained(), 0.1f); - // Convert that tilt error into an absolute reduced desired attitude. - // This matches the PX4 multicopter pattern (right multiplication). - Quatf qd_red; + Vector3f x_h = ex_c - ez_world * ex_c.dot(ez_world); // forward projected to horizontal + float r_tc_ff = 0.f; - if (fabsf(q_tilt_err(1)) > (1.f - 1e-5f) || fabsf(q_tilt_err(2)) > (1.f - 1e-5f)) { - // Corner case: vectors almost exactly opposite (180 deg). Tilt-only separation is ill-defined. - // Fall back to full desired attitude; it's safe/stable. - qd_red = qd_full; + const float xhn = x_h.norm(); - } else { - // Apply tilt correction to current attitude to get a reduced desired attitude - qd_red = q_tilt_err * q_current; + if (xhn > 1e-3f) { + + x_h /= xhn; + Vector3f y_h = ez_world.cross(x_h); + const float yhn = y_h.norm(); + + if (yhn > 1e-6f) { + + y_h /= yhn; + const float cos_tilt = ez_c.dot(ez_world); + const float sin_bank = ez_c.dot(y_h); + + float tan_bank = 0.f; + + if (fabsf(cos_tilt) > 0.1f) { + tan_bank = sin_bank / cos_tilt; + } + + r_tc_ff = (9.81f / V) * tan_bank * 0.6f; + } } - // --- Compute rate setpoint A: tilt-only attitude tracking (normal FW) --- - - // qe_tilt is rotation from current to reduced desired - const Quatf qe_tilt = (q_current.inversed() * qd_red).canonical(); - - // Use bounded quaternion vector part as error (stable & well-tuned pattern) - const Vector3f e_tilt = 2.f * qe_tilt.imag(); - - Vector3f rates_tilt = e_tilt.emult(_proportional_gain); - - // In normal FW flight, yaw is NOT attitude-tracked; use existing yaw/turn coordination - const float yaw_rate_tc = _yaw_ctrl.get_body_rate_setpoint(); - rates_tilt(2) = yaw_rate_tc; - - // --- Compute rate setpoint B: full 3D attitude tracking (near-vertical) --- - const Quatf qe_full = (q_current.inversed() * qd_full).canonical(); - const Vector3f e_full = 2.f * qe_full.imag(); - Vector3f rates_full = e_full.emult(_proportional_gain); - - // --- Blend A and B --- - Vector3f body_rates_setpoint = (1.f - s) * rates_tilt + s * rates_full; - - - // limit rates (kept identical) - body_rates_setpoint(0) = constrain(body_rates_setpoint(0), -_max_roll_rate, _max_roll_rate); - body_rates_setpoint(1) = constrain(body_rates_setpoint(1), -_max_pitch_rate_neg, _max_pitch_rate_pos); - body_rates_setpoint(2) = constrain(body_rates_setpoint(2), -_max_yaw_rate, _max_yaw_rate); - - - /////////////////////////////////// - const Eulerf euler_sp(q_sp); - const float roll_sp = euler_sp.phi(); - const float pitch_sp = euler_sp.theta(); - - _roll_ctrl.control_roll(roll_sp, _yaw_ctrl.get_euler_rate_setpoint(), euler_angles.phi(), - euler_angles.theta()); - _pitch_ctrl.control_pitch(pitch_sp, _yaw_ctrl.get_euler_rate_setpoint(), euler_angles.phi(), - euler_angles.theta()); - _yaw_ctrl.control_yaw(roll_sp, _pitch_ctrl.get_euler_rate_setpoint(), euler_angles.phi(), - euler_angles.theta(), get_airspeed_constrained()); - - /* Update input data for rate controllers */ - Vector3f _euler_body_rates_sp = Vector3f(_roll_ctrl.get_body_rate_setpoint(), _pitch_ctrl.get_body_rate_setpoint(), - _yaw_ctrl.get_body_rate_setpoint()); - - _euler_rates_sp.timestamp = hrt_absolute_time(); - _euler_rates_sp.roll_rate = _euler_body_rates_sp(0); - _euler_rates_sp.pitch_rate = _euler_body_rates_sp(1); - _euler_rates_sp.yaw_rate = _euler_body_rates_sp(2); - /////////////////////////////////// - + body_rates_setpoint(2) = -r_tc_ff; + body_rates_setpoint(0) = math::constrain(body_rates_setpoint(0), -radians(_param_fw_r_rmax.get()), radians(_param_fw_r_rmax.get())); + body_rates_setpoint(1) = math::constrain(body_rates_setpoint(1), -radians(_param_fw_p_rmax_neg.get()), radians(_param_fw_p_rmax_pos.get())); + body_rates_setpoint(2) = math::constrain(body_rates_setpoint(2), -radians(_param_fw_y_rmax.get()), radians(_param_fw_y_rmax.get())); autotune_attitude_control_status_s pid_autotune; matrix::Vector3f bodyrate_autotune_ff; @@ -434,7 +388,6 @@ void FixedwingAttitudeControl::Run() _rates_sp.timestamp = hrt_absolute_time(); _rate_sp_pub.publish(_rates_sp); - _euler_rates_sp_pub.publish(_euler_rates_sp); } } } diff --git a/src/modules/fw_att_control/FixedwingAttitudeControl.hpp b/src/modules/fw_att_control/FixedwingAttitudeControl.hpp index 2b459e9c53..447281aca6 100644 --- a/src/modules/fw_att_control/FixedwingAttitudeControl.hpp +++ b/src/modules/fw_att_control/FixedwingAttitudeControl.hpp @@ -34,10 +34,8 @@ #pragma once #include -#include "fw_pitch_controller.h" -#include "fw_roll_controller.h" + #include "fw_wheel_controller.h" -#include "fw_yaw_controller.h" #include #include #include @@ -65,7 +63,6 @@ #include #include #include -#include #include using matrix::Eulerf; @@ -116,14 +113,12 @@ private: uORB::Publication _attitude_sp_pub; uORB::Publication _rate_sp_pub{ORB_ID(vehicle_rates_setpoint)}; - uORB::Publication _euler_rates_sp_pub{ORB_ID(vehicle_euler_rates_setpoint)}; uORB::Publication _landing_gear_wheel_pub{ORB_ID(landing_gear_wheel)}; manual_control_setpoint_s _manual_control_setpoint{}; vehicle_attitude_setpoint_s _att_sp{}; vehicle_control_mode_s _vcontrol_mode{}; vehicle_rates_setpoint_s _rates_sp{}; - vehicle_euler_rates_setpoint_s _euler_rates_sp{}; vehicle_status_s _vehicle_status{}; landing_gear_wheel_s _landing_gear_wheel{}; @@ -164,24 +159,14 @@ private: (ParamFloat) _param_fw_y_rmax, (ParamFloat) _param_man_yr_max - ) matrix::Vector3f _proportional_gain; - RollController _roll_ctrl; - PitchController _pitch_ctrl; - YawController _yaw_ctrl; - - float _max_roll_rate; - float _max_pitch_rate_pos; - float _max_pitch_rate_neg; - float _max_yaw_rate; - WheelController _wheel_ctrl; void parameters_update(); - void vehicle_manual_poll(matrix::Quatf R); + void vehicle_manual_poll(const float yaw_body); void vehicle_attitude_setpoint_poll(); void vehicle_land_detected_poll(); float get_airspeed_constrained(); -}; \ No newline at end of file +}; diff --git a/src/modules/fw_att_control/fw_pitch_controller.cpp b/src/modules/fw_att_control/fw_pitch_controller.cpp deleted file mode 100644 index 90ad9dce43..0000000000 --- a/src/modules/fw_att_control/fw_pitch_controller.cpp +++ /dev/null @@ -1,65 +0,0 @@ -/**************************************************************************** - * - * 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 - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name ECL nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file fw_pitch_controller.cpp - * Implementation of a simple pitch P controller. - */ - -#include "fw_pitch_controller.h" -#include -#include -#include - -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(pitch_setpoint) && - PX4_ISFINITE(euler_yaw_rate_setpoint) && - PX4_ISFINITE(pitch) && - PX4_ISFINITE(roll))) { - - return _body_rate_setpoint; - } - - 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(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/fw_pitch_controller.h b/src/modules/fw_att_control/fw_pitch_controller.h deleted file mode 100644 index d8a40ab636..0000000000 --- a/src/modules/fw_att_control/fw_pitch_controller.h +++ /dev/null @@ -1,74 +0,0 @@ -/**************************************************************************** - * - * 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 - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file fw_pitch_controller.h - * Definition of a simple pitch P controller. - */ - -#ifndef FW_PITCH_CONTROLLER_H -#define FW_PITCH_CONTROLLER_H - -class PitchController -{ -public: - PitchController() = default; - ~PitchController() = default; - - /** - * @brief Calculates both euler and body pitch rate setpoints. - * - * @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_pitch(float pitch_setpoint, float euler_yaw_rate_setpoint, float roll, float pitch); - - 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; } - - float get_euler_rate_setpoint() { return _euler_rate_setpoint; } - float get_body_rate_setpoint() { return _body_rate_setpoint; } - -private: - float _tc{}; - float _max_rate_pos{}; - float _max_rate_neg{}; - float _euler_rate_setpoint{}; - float _body_rate_setpoint{}; -}; - -#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 deleted file mode 100644 index 659adfa9c2..0000000000 --- a/src/modules/fw_att_control/fw_roll_controller.cpp +++ /dev/null @@ -1,64 +0,0 @@ -/**************************************************************************** - * - * 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 - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name ECL nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file fw_roll_controller.cpp - * Implementation of a simple roll P controller. - */ - -#include "fw_roll_controller.h" -#include -#include -#include - -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(roll_setpoint) && - PX4_ISFINITE(euler_yaw_rate_setpoint) && - PX4_ISFINITE(pitch) && - PX4_ISFINITE(roll))) { - - return _body_rate_setpoint; - } - - 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(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/fw_roll_controller.h b/src/modules/fw_att_control/fw_roll_controller.h deleted file mode 100644 index 5e47f0ffd0..0000000000 --- a/src/modules/fw_att_control/fw_roll_controller.h +++ /dev/null @@ -1,72 +0,0 @@ -/**************************************************************************** - * - * 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 - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file fw_roll_controller.h - * Definition of a simple roll P controller. - */ - -#ifndef FW_ROLL_CONTROLLER_H -#define FW_ROLL_CONTROLLER_H - -class RollController -{ -public: - RollController() = default; - ~RollController() = default; - - /** - * @brief Calculates both euler and body roll rate setpoints. - * - * @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_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 // FW_ROLL_CONTROLLER_H diff --git a/src/modules/fw_att_control/fw_yaw_controller.cpp b/src/modules/fw_att_control/fw_yaw_controller.cpp deleted file mode 100644 index 4f27e8a610..0000000000 --- a/src/modules/fw_att_control/fw_yaw_controller.cpp +++ /dev/null @@ -1,98 +0,0 @@ -/**************************************************************************** - * - * 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 - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name ECL nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file fw_yaw_controller.cpp - * Implementation of a simple coordinated turn yaw controller. - */ - -#include "fw_yaw_controller.h" -#include -#include -#include - -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(roll_setpoint) && - PX4_ISFINITE(roll) && - PX4_ISFINITE(pitch) && - PX4_ISFINITE(euler_pitch_rate_setpoint) && - PX4_ISFINITE(airspeed))) { - - return _body_rate_setpoint; - } - - float constrained_roll; - bool inverted = false; - - /* roll is used as feedforward term and inverted flight needs to be considered */ - if (fabsf(roll) < math::radians(90.f)) { - /* not inverted, but numerically still potentially close to infinity */ - 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 (roll > 0.f) { - /* right hemisphere */ - constrained_roll = math::constrain(roll, math::radians(100.f), math::radians(180.f)); - - } else { - /* left hemisphere */ - constrained_roll = math::constrain(roll, math::radians(-180.f), math::radians(-100.f)); - } - } - - 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(pitch) * CONSTANTS_ONE_G / airspeed; - - /* Transform setpoint to body angular rates (jacobian) */ - 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)) { - _body_rate_setpoint = 0.f; - } - - return _body_rate_setpoint; -} diff --git a/src/modules/fw_att_control/fw_yaw_controller.h b/src/modules/fw_att_control/fw_yaw_controller.h deleted file mode 100644 index e6f9ff9795..0000000000 --- a/src/modules/fw_att_control/fw_yaw_controller.h +++ /dev/null @@ -1,79 +0,0 @@ -/**************************************************************************** - * - * 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 - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file fw_yaw_controller.h - * Definition of a simple coordinated turn controller. - * - * 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. - */ - -#ifndef FW_YAW_CONTROLLER_H -#define FW_YAW_CONTROLLER_H - -class YawController -{ -public: - YawController() = default; - ~YawController() = default; - - /** - * @brief Calculates both euler and body yaw rate setpoints for coordinated turn based on current attitude and airspeed - * - * @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_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 // FW_YAW_CONTROLLER_H diff --git a/src/modules/fw_lateral_longitudinal_control/FwLateralLongitudinalControl.cpp b/src/modules/fw_lateral_longitudinal_control/FwLateralLongitudinalControl.cpp index c9f3753ec1..e6cbedfea2 100644 --- a/src/modules/fw_lateral_longitudinal_control/FwLateralLongitudinalControl.cpp +++ b/src/modules/fw_lateral_longitudinal_control/FwLateralLongitudinalControl.cpp @@ -298,7 +298,9 @@ void FwLateralLongitudinalControl::Run() // additional is_finite checks that should not be necessary, but are kept for safety float roll_body = PX4_ISFINITE(roll_sp) ? roll_sp : 0.0f; float pitch_body = PX4_ISFINITE(pitch_sp) ? pitch_sp : 0.0f; - const float yaw_body = _yaw; // yaw is not controlled in fixed wing, need to set it though for quaternion generation + + float yaw_body = _yaw; + const float thrust_body_x = PX4_ISFINITE(throttle_sp) ? throttle_sp : 0.0f; if (_control_mode_sub.get().flag_control_manual_enabled) { diff --git a/src/modules/logger/logged_topics.cpp b/src/modules/logger/logged_topics.cpp index 41e2cd163a..0810dfc9a1 100644 --- a/src/modules/logger/logged_topics.cpp +++ b/src/modules/logger/logged_topics.cpp @@ -148,7 +148,6 @@ void LoggedTopics::add_default_topics() add_topic("vehicle_local_position_setpoint", 100); add_topic("vehicle_magnetometer", 200); add_topic("vehicle_rates_setpoint", 20); - add_topic("vehicle_euler_rates_setpoint", 20); add_topic("vehicle_roi", 1000); add_topic("vehicle_status"); add_topic("vtx");