From afc9d6dfabfb2db04f2bf63373192d5394a5e90e Mon Sep 17 00:00:00 2001 From: bresch Date: Wed, 28 Feb 2024 10:17:35 +0100 Subject: [PATCH] FwTrim class --- ROMFS/px4fmu_common/init.d/rc.fw_apps | 1 - ROMFS/px4fmu_common/init.d/rc.vtol_apps | 1 - boards/ark/fmu-v6x/default.px4board | 1 - boards/px4/fmu-v5/default.px4board | 1 - boards/px4/fmu-v5x/default.px4board | 1 - boards/px4/fmu-v6c/default.px4board | 1 - boards/px4/fmu-v6u/default.px4board | 1 - boards/px4/fmu-v6x/default.px4board | 1 - boards/px4/sitl/default.px4board | 1 - src/modules/fw_rate_control/CMakeLists.txt | 4 +- .../fw_rate_control/FixedwingRateControl.cpp | 51 +++------ .../fw_rate_control/FixedwingRateControl.hpp | 19 +--- .../{fw_auto_trim => fw_trim}/CMakeLists.txt | 7 +- .../{fw_auto_trim => fw_trim}/FwAutoTrim.cpp | 6 +- .../{fw_auto_trim => fw_trim}/FwAutoTrim.hpp | 2 +- .../fw_rate_control/fw_trim/FwTrim.cpp | 106 ++++++++++++++++++ .../fw_rate_control/fw_trim/FwTrim.hpp | 86 ++++++++++++++ 17 files changed, 225 insertions(+), 65 deletions(-) rename src/modules/fw_rate_control/{fw_auto_trim => fw_trim}/CMakeLists.txt (92%) rename src/modules/fw_rate_control/{fw_auto_trim => fw_trim}/FwAutoTrim.cpp (96%) rename src/modules/fw_rate_control/{fw_auto_trim => fw_trim}/FwAutoTrim.hpp (98%) create mode 100644 src/modules/fw_rate_control/fw_trim/FwTrim.cpp create mode 100644 src/modules/fw_rate_control/fw_trim/FwTrim.hpp diff --git a/ROMFS/px4fmu_common/init.d/rc.fw_apps b/ROMFS/px4fmu_common/init.d/rc.fw_apps index 5bddcb60d1..f3c559298e 100644 --- a/ROMFS/px4fmu_common/init.d/rc.fw_apps +++ b/ROMFS/px4fmu_common/init.d/rc.fw_apps @@ -27,7 +27,6 @@ airspeed_selector start # Start attitude control auto-tuner # fw_autotune_attitude_control start -fw_auto_trim start # # Start Land Detector. diff --git a/ROMFS/px4fmu_common/init.d/rc.vtol_apps b/ROMFS/px4fmu_common/init.d/rc.vtol_apps index fa7185dde7..fdc16af83d 100644 --- a/ROMFS/px4fmu_common/init.d/rc.vtol_apps +++ b/ROMFS/px4fmu_common/init.d/rc.vtol_apps @@ -39,7 +39,6 @@ fw_rate_control start vtol fw_att_control start vtol fw_pos_control start vtol fw_autotune_attitude_control start vtol -fw_auto_trim start vtol # Start Land Detector # Multicopter for now until we have something for VTOL diff --git a/boards/ark/fmu-v6x/default.px4board b/boards/ark/fmu-v6x/default.px4board index 411e9ca6e2..4018e493a2 100644 --- a/boards/ark/fmu-v6x/default.px4board +++ b/boards/ark/fmu-v6x/default.px4board @@ -45,7 +45,6 @@ CONFIG_MODULES_EVENTS=y CONFIG_MODULES_FLIGHT_MODE_MANAGER=y CONFIG_MODULES_FW_ATT_CONTROL=y CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y -CONFIG_MODULES_FW_AUTO_TRIM=y CONFIG_MODULES_FW_POS_CONTROL=y CONFIG_MODULES_FW_RATE_CONTROL=y CONFIG_MODULES_GIMBAL=y diff --git a/boards/px4/fmu-v5/default.px4board b/boards/px4/fmu-v5/default.px4board index c5cfb8508f..bfb8b6af46 100644 --- a/boards/px4/fmu-v5/default.px4board +++ b/boards/px4/fmu-v5/default.px4board @@ -54,7 +54,6 @@ CONFIG_MODULES_EVENTS=y CONFIG_MODULES_FLIGHT_MODE_MANAGER=y CONFIG_MODULES_FW_ATT_CONTROL=y CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y -CONFIG_MODULES_FW_AUTO_TRIM=y CONFIG_MODULES_FW_POS_CONTROL=y CONFIG_MODULES_FW_RATE_CONTROL=y CONFIG_MODULES_GIMBAL=y diff --git a/boards/px4/fmu-v5x/default.px4board b/boards/px4/fmu-v5x/default.px4board index 22a53f514c..e2b5833dfe 100644 --- a/boards/px4/fmu-v5x/default.px4board +++ b/boards/px4/fmu-v5x/default.px4board @@ -60,7 +60,6 @@ CONFIG_MODULES_EVENTS=y CONFIG_MODULES_FLIGHT_MODE_MANAGER=y CONFIG_MODULES_FW_ATT_CONTROL=y CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y -CONFIG_MODULES_FW_AUTO_TRIM=y CONFIG_MODULES_FW_POS_CONTROL=y CONFIG_MODULES_FW_RATE_CONTROL=y CONFIG_MODULES_GIMBAL=y diff --git a/boards/px4/fmu-v6c/default.px4board b/boards/px4/fmu-v6c/default.px4board index 8593e98ea9..148e6326fb 100644 --- a/boards/px4/fmu-v6c/default.px4board +++ b/boards/px4/fmu-v6c/default.px4board @@ -43,7 +43,6 @@ CONFIG_MODULES_EVENTS=y CONFIG_MODULES_FLIGHT_MODE_MANAGER=y CONFIG_MODULES_FW_ATT_CONTROL=y CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y -CONFIG_MODULES_FW_AUTO_TRIM=y CONFIG_MODULES_FW_POS_CONTROL=y CONFIG_MODULES_FW_RATE_CONTROL=y CONFIG_MODULES_GIMBAL=y diff --git a/boards/px4/fmu-v6u/default.px4board b/boards/px4/fmu-v6u/default.px4board index b7c7eb0d00..50b4f6ba3d 100644 --- a/boards/px4/fmu-v6u/default.px4board +++ b/boards/px4/fmu-v6u/default.px4board @@ -46,7 +46,6 @@ CONFIG_MODULES_EVENTS=y CONFIG_MODULES_FLIGHT_MODE_MANAGER=y CONFIG_MODULES_FW_ATT_CONTROL=y CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y -CONFIG_MODULES_FW_AUTO_TRIM=y CONFIG_MODULES_FW_POS_CONTROL=y CONFIG_MODULES_FW_RATE_CONTROL=y CONFIG_MODULES_GIMBAL=y diff --git a/boards/px4/fmu-v6x/default.px4board b/boards/px4/fmu-v6x/default.px4board index 91fe55f508..4e21f0fd08 100644 --- a/boards/px4/fmu-v6x/default.px4board +++ b/boards/px4/fmu-v6x/default.px4board @@ -55,7 +55,6 @@ CONFIG_MODULES_EVENTS=y CONFIG_MODULES_FLIGHT_MODE_MANAGER=y CONFIG_MODULES_FW_ATT_CONTROL=y CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y -CONFIG_MODULES_FW_AUTO_TRIM=y CONFIG_MODULES_FW_POS_CONTROL=y CONFIG_MODULES_FW_RATE_CONTROL=y CONFIG_MODULES_GIMBAL=y diff --git a/boards/px4/sitl/default.px4board b/boards/px4/sitl/default.px4board index 1e727c8a93..7dd7ce481a 100644 --- a/boards/px4/sitl/default.px4board +++ b/boards/px4/sitl/default.px4board @@ -19,7 +19,6 @@ CONFIG_EKF2_AUX_GLOBAL_POSITION=y CONFIG_MODULES_EVENTS=y CONFIG_MODULES_FLIGHT_MODE_MANAGER=y CONFIG_MODULES_FW_ATT_CONTROL=y -CONFIG_MODULES_FW_AUTO_TRIM=y CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y CONFIG_MODULES_FW_POS_CONTROL=y CONFIG_FIGURE_OF_EIGHT=y diff --git a/src/modules/fw_rate_control/CMakeLists.txt b/src/modules/fw_rate_control/CMakeLists.txt index 9845e3ee4e..5400fc33a6 100644 --- a/src/modules/fw_rate_control/CMakeLists.txt +++ b/src/modules/fw_rate_control/CMakeLists.txt @@ -30,7 +30,7 @@ # POSSIBILITY OF SUCH DAMAGE. # ############################################################################ -add_subdirectory(fw_auto_trim) +add_subdirectory(fw_trim) px4_add_module( MODULE modules__fw_rate_control @@ -39,7 +39,7 @@ px4_add_module( FixedwingRateControl.cpp FixedwingRateControl.hpp DEPENDS - FwAutoTrim + FwTrim px4_work_queue RateControl SlewRate diff --git a/src/modules/fw_rate_control/FixedwingRateControl.cpp b/src/modules/fw_rate_control/FixedwingRateControl.cpp index b8e3b18126..788f9d41b9 100644 --- a/src/modules/fw_rate_control/FixedwingRateControl.cpp +++ b/src/modules/fw_rate_control/FixedwingRateControl.cpp @@ -37,7 +37,6 @@ using namespace time_literals; using namespace matrix; using math::constrain; -using math::interpolate; using math::radians; FixedwingRateControl::FixedwingRateControl(bool vtol) : @@ -54,7 +53,7 @@ FixedwingRateControl::FixedwingRateControl(bool vtol) : parameters_update(); _rate_ctrl_status_pub.advertise(); - _auto_trim_slew.setSlewRate(Vector3f(0.01f, 0.01f, 0.01f)); + _trim_slew.setSlewRate(Vector3f(0.01f, 0.01f, 0.01f)); } FixedwingRateControl::~FixedwingRateControl() @@ -128,13 +127,14 @@ FixedwingRateControl::vehicle_manual_poll() } else { // Manual/direct control, filled in FW-frame. Note that setpoints will get transformed to body frame prior publishing. + const Vector3f trim = _trim_slew.getState(); _vehicle_torque_setpoint.xyz[0] = math::constrain(_manual_control_setpoint.roll * _param_fw_man_r_sc.get() + - _param_trim_roll.get(), -1.f, 1.f); + trim(0), -1.f, 1.f); _vehicle_torque_setpoint.xyz[1] = math::constrain(-_manual_control_setpoint.pitch * _param_fw_man_p_sc.get() + - _param_trim_pitch.get(), -1.f, 1.f); + trim(1), -1.f, 1.f); _vehicle_torque_setpoint.xyz[2] = math::constrain(_manual_control_setpoint.yaw * _param_fw_man_y_sc.get() + - _param_trim_yaw.get(), -1.f, 1.f); + trim(2), -1.f, 1.f); _vehicle_thrust_setpoint.xyz[0] = math::constrain((_manual_control_setpoint.throttle + 1.f) * .5f, 0.f, 1.f); } @@ -322,30 +322,8 @@ void FixedwingRateControl::Run() } } - /* bi-linear interpolation over airspeed for actuator trim scheduling */ - Vector3f trim(_param_trim_roll.get(), _param_trim_pitch.get(), _param_trim_yaw.get()); - - if (airspeed < _param_fw_airspd_trim.get()) { - trim(0) += interpolate(airspeed, _param_fw_airspd_min.get(), _param_fw_airspd_trim.get(), - _param_fw_dtrim_r_vmin.get(), - 0.0f); - trim(1) += interpolate(airspeed, _param_fw_airspd_min.get(), _param_fw_airspd_trim.get(), - _param_fw_dtrim_p_vmin.get(), - 0.0f); - trim(2) += interpolate(airspeed, _param_fw_airspd_min.get(), _param_fw_airspd_trim.get(), - _param_fw_dtrim_y_vmin.get(), - 0.0f); - - } else { - trim(0) += interpolate(airspeed, _param_fw_airspd_trim.get(), _param_fw_airspd_max.get(), 0.0f, - _param_fw_dtrim_r_vmax.get()); - trim(1) += interpolate(airspeed, _param_fw_airspd_trim.get(), _param_fw_airspd_max.get(), 0.0f, - _param_fw_dtrim_p_vmax.get()); - trim(2) += interpolate(airspeed, _param_fw_airspd_trim.get(), _param_fw_airspd_max.get(), 0.0f, - _param_fw_dtrim_y_vmax.get()); - } - - trim += _auto_trim_slew.update(_auto_trim.getTrim(), dt); + _trim.setAirspeed(airspeed); + _trim_slew.update(_trim.getTrim(), dt); if (_vcontrol_mode.flag_control_rates_enabled) { _rates_sp_sub.update(&_rates_sp); @@ -373,12 +351,11 @@ void FixedwingRateControl::Run() } if (control_u.isAllFinite()) { - matrix::constrain(control_u + trim, -1.f, 1.f).copyTo(_vehicle_torque_setpoint.xyz); - _auto_trim.update(control_u + _auto_trim_slew.getState(), dt); + matrix::constrain(control_u + _trim_slew.getState(), -1.f, 1.f).copyTo(_vehicle_torque_setpoint.xyz); } else { _rate_control.resetIntegral(); - trim.copyTo(_vehicle_torque_setpoint.xyz); + _trim_slew.getState().copyTo(_vehicle_torque_setpoint.xyz); } /* throttle passed through if it is finite */ @@ -409,7 +386,7 @@ void FixedwingRateControl::Run() } else { // full manual _rate_control.resetIntegral(); - _auto_trim.reset(); + _trim.reset(); } // Add feed-forward from roll control output to yaw control output @@ -437,6 +414,8 @@ void FixedwingRateControl::Run() _vehicle_torque_setpoint.timestamp_sample = angular_velocity.timestamp_sample; _vehicle_torque_setpoint_pub.publish(_vehicle_torque_setpoint); } + + _trim.updateAutoTrim(Vector3f(_vehicle_torque_setpoint.xyz), dt); } updateActuatorControlsStatus(dt); @@ -574,6 +553,12 @@ fw_rate_control is the fixed-wing rate controller. return 0; } +int FixedwingRateControl::print_status() +{ + _trim.print_status(); + return 0; +} + extern "C" __EXPORT int fw_rate_control_main(int argc, char *argv[]) { return FixedwingRateControl::main(argc, argv); diff --git a/src/modules/fw_rate_control/FixedwingRateControl.hpp b/src/modules/fw_rate_control/FixedwingRateControl.hpp index 2c478c3db8..b02e542790 100644 --- a/src/modules/fw_rate_control/FixedwingRateControl.hpp +++ b/src/modules/fw_rate_control/FixedwingRateControl.hpp @@ -69,7 +69,7 @@ #include #include -#include "fw_auto_trim/FwAutoTrim.hpp" +#include "fw_trim/FwTrim.hpp" using matrix::Eulerf; using matrix::Quatf; @@ -94,6 +94,8 @@ public: /** @see ModuleBase */ static int print_usage(const char *reason = nullptr); + int print_status() override; + bool init(); private: @@ -172,13 +174,6 @@ private: (ParamBool) _param_fw_bat_scale_en, - (ParamFloat) _param_fw_dtrim_p_vmax, - (ParamFloat) _param_fw_dtrim_p_vmin, - (ParamFloat) _param_fw_dtrim_r_vmax, - (ParamFloat) _param_fw_dtrim_r_vmin, - (ParamFloat) _param_fw_dtrim_y_vmax, - (ParamFloat) _param_fw_dtrim_y_vmin, - (ParamFloat) _param_fw_man_p_sc, (ParamFloat) _param_fw_man_r_sc, (ParamFloat) _param_fw_man_y_sc, @@ -202,17 +197,13 @@ private: (ParamFloat) _param_fw_yr_p, (ParamFloat) _param_fw_yr_d, - (ParamFloat) _param_trim_pitch, - (ParamFloat) _param_trim_roll, - (ParamFloat) _param_trim_yaw, - (ParamInt) _param_fw_spoilers_man ) RateControl _rate_control; ///< class for rate control calculations - FwAutoTrim _auto_trim{this}; - SlewRate _auto_trim_slew{}; ///< prevents large trim changes + FwTrim _trim{this}; + SlewRate _trim_slew{}; ///< prevents large trim changes void updateActuatorControlsStatus(float dt); diff --git a/src/modules/fw_rate_control/fw_auto_trim/CMakeLists.txt b/src/modules/fw_rate_control/fw_trim/CMakeLists.txt similarity index 92% rename from src/modules/fw_rate_control/fw_auto_trim/CMakeLists.txt rename to src/modules/fw_rate_control/fw_trim/CMakeLists.txt index 5a1fb9584a..718e7e3545 100644 --- a/src/modules/fw_rate_control/fw_auto_trim/CMakeLists.txt +++ b/src/modules/fw_rate_control/fw_trim/CMakeLists.txt @@ -30,9 +30,10 @@ # POSSIBILITY OF SUCH DAMAGE. # ############################################################################ -px4_add_library(FwAutoTrim +px4_add_library(FwTrim + FwTrim.cpp FwAutoTrim.cpp ) -# target_link_libraries(FwAutoTrim) -target_include_directories(FwAutoTrim PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) +# target_link_libraries(FwTrim) +target_include_directories(FwTrim PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) diff --git a/src/modules/fw_rate_control/fw_auto_trim/FwAutoTrim.cpp b/src/modules/fw_rate_control/fw_trim/FwAutoTrim.cpp similarity index 96% rename from src/modules/fw_rate_control/fw_auto_trim/FwAutoTrim.cpp rename to src/modules/fw_rate_control/fw_trim/FwAutoTrim.cpp index ac8a577a31..1225e88ea6 100644 --- a/src/modules/fw_rate_control/fw_auto_trim/FwAutoTrim.cpp +++ b/src/modules/fw_rate_control/fw_trim/FwAutoTrim.cpp @@ -195,9 +195,9 @@ void FwAutoTrim::publishStatus(const hrt_abstime now) int FwAutoTrim::print_status() { perf_print_counter(_cycle_perf); - printf("Status: %d\n", static_cast(_state)); - printf("Trim validated\n"); - _trim_validated.print(); + printf("[AutoTrim] State: %d\n", static_cast(_state)); + printf("[AutoTrim] Trim validated = (%.3f, %.3f, %.3f)\n", (double)_trim_validated(0), (double)_trim_validated(1), + (double)_trim_validated(2)); return 0; } diff --git a/src/modules/fw_rate_control/fw_auto_trim/FwAutoTrim.hpp b/src/modules/fw_rate_control/fw_trim/FwAutoTrim.hpp similarity index 98% rename from src/modules/fw_rate_control/fw_auto_trim/FwAutoTrim.hpp rename to src/modules/fw_rate_control/fw_trim/FwAutoTrim.hpp index 3d0368785f..aca667d9b1 100644 --- a/src/modules/fw_rate_control/fw_auto_trim/FwAutoTrim.hpp +++ b/src/modules/fw_rate_control/fw_trim/FwAutoTrim.hpp @@ -100,7 +100,7 @@ private: matrix::Vector3f _trim_validated{}; - perf_counter_t _cycle_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle time")}; + perf_counter_t _cycle_perf{perf_alloc(PC_ELAPSED, "[AutoTrim] cycle time")}; DEFINE_PARAMETERS( (ParamFloat) _param_fw_airspd_min, diff --git a/src/modules/fw_rate_control/fw_trim/FwTrim.cpp b/src/modules/fw_rate_control/fw_trim/FwTrim.cpp new file mode 100644 index 0000000000..b7d0fb865c --- /dev/null +++ b/src/modules/fw_rate_control/fw_trim/FwTrim.cpp @@ -0,0 +1,106 @@ +/**************************************************************************** + * + * Copyright (c) 2024 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. + * + ****************************************************************************/ + +#include "FwTrim.hpp" + +#include + +using namespace time_literals; +using matrix::Vector3f; +using math::interpolate; + +FwTrim::FwTrim(ModuleParams *parent) : + ModuleParams(parent) +{ + updateParams(); + _airspeed = _param_fw_airspd_trim.get(); +} + +void FwTrim::updateParams() +{ + ModuleParams::updateParams(); + updateParameterizedTrim(); +} + +void FwTrim::reset() +{ + _auto_trim.reset(); +} + +void FwTrim::updateAutoTrim(const Vector3f &torque_sp, const float dt) +{ + _auto_trim.update(torque_sp - _parameterized_trim, dt); +} + +void FwTrim::setAirspeed(const float airspeed) +{ + _airspeed = airspeed; + updateParameterizedTrim(); +} + +void FwTrim::updateParameterizedTrim() +{ + /* bi-linear interpolation over airspeed for actuator trim scheduling */ + Vector3f trim(_param_trim_roll.get(), _param_trim_pitch.get(), _param_trim_yaw.get()); + + if (_airspeed < _param_fw_airspd_trim.get()) { + trim(0) += interpolate(_airspeed, _param_fw_airspd_min.get(), _param_fw_airspd_trim.get(), + _param_fw_dtrim_r_vmin.get(), + 0.0f); + trim(1) += interpolate(_airspeed, _param_fw_airspd_min.get(), _param_fw_airspd_trim.get(), + _param_fw_dtrim_p_vmin.get(), + 0.0f); + trim(2) += interpolate(_airspeed, _param_fw_airspd_min.get(), _param_fw_airspd_trim.get(), + _param_fw_dtrim_y_vmin.get(), + 0.0f); + + } else { + trim(0) += interpolate(_airspeed, _param_fw_airspd_trim.get(), _param_fw_airspd_max.get(), 0.0f, + _param_fw_dtrim_r_vmax.get()); + trim(1) += interpolate(_airspeed, _param_fw_airspd_trim.get(), _param_fw_airspd_max.get(), 0.0f, + _param_fw_dtrim_p_vmax.get()); + trim(2) += interpolate(_airspeed, _param_fw_airspd_trim.get(), _param_fw_airspd_max.get(), 0.0f, + _param_fw_dtrim_y_vmax.get()); + } + + _parameterized_trim = trim; +} + +int FwTrim::print_status() +{ + Vector3f trim = getTrim(); + printf("Trim = (%.3f, %.3f, %.3f)\n", (double)trim(0), (double)trim(1), (double)trim(2)); + + _auto_trim.print_status(); + return 0; +} diff --git a/src/modules/fw_rate_control/fw_trim/FwTrim.hpp b/src/modules/fw_rate_control/fw_trim/FwTrim.hpp new file mode 100644 index 0000000000..b804c07d86 --- /dev/null +++ b/src/modules/fw_rate_control/fw_trim/FwTrim.hpp @@ -0,0 +1,86 @@ +/**************************************************************************** + * + * Copyright (c) 2024 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. + * + ****************************************************************************/ + +#pragma once + +#include +#include +#include +#include +#include +#include + +#include "FwAutoTrim.hpp" + +using namespace time_literals; + +class FwTrim : public ModuleParams +{ +public: + FwTrim(ModuleParams *parent); + ~FwTrim() = default; + + void reset(); + void setAirspeed(float airspeed); + void updateAutoTrim(const matrix::Vector3f &torque_sp, float dt); + matrix::Vector3f getTrim() const { return _parameterized_trim + _auto_trim.getTrim(); } + int print_status(); + +protected: + void updateParams() override; + +private: + void updateParameterizedTrim(); + + FwAutoTrim _auto_trim{this}; + matrix::Vector3f _parameterized_trim{}; + + float _airspeed{0.f}; + + DEFINE_PARAMETERS( + (ParamFloat) _param_trim_pitch, + (ParamFloat) _param_trim_roll, + (ParamFloat) _param_trim_yaw, + + (ParamFloat) _param_fw_dtrim_p_vmax, + (ParamFloat) _param_fw_dtrim_p_vmin, + (ParamFloat) _param_fw_dtrim_r_vmax, + (ParamFloat) _param_fw_dtrim_r_vmin, + (ParamFloat) _param_fw_dtrim_y_vmax, + (ParamFloat) _param_fw_dtrim_y_vmin, + + (ParamFloat) _param_fw_airspd_trim, + (ParamFloat) _param_fw_airspd_min, + (ParamFloat) _param_fw_airspd_max + ) +};