dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

FwTrim class

This commit is contained in:
bresch 2024-02-28 10:17:35 +01:00 committed by Silvan Fuhrer
parent 5f992423c1
commit afc9d6dfab
17 changed files with 225 additions and 65 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
ROMFS/px4fmu_common/init.d/rc.fw_apps
View File

@ -27,7 +27,6 @@ airspeed_selector start
# Start attitude control auto-tuner
#
fw_autotune_attitude_control start
fw_auto_trim start
#
# Start Land Detector.

1
ROMFS/px4fmu_common/init.d/rc.vtol_apps
View File

@ -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

1
boards/ark/fmu-v6x/default.px4board
View File

@ -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

1
boards/px4/fmu-v5/default.px4board
View File

@ -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

1
boards/px4/fmu-v5x/default.px4board
View File

@ -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

1
boards/px4/fmu-v6c/default.px4board
View File

@ -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

1
boards/px4/fmu-v6u/default.px4board
View File

@ -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

1
boards/px4/fmu-v6x/default.px4board
View File

@ -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

1
boards/px4/sitl/default.px4board
View File

@ -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

4
src/modules/fw_rate_control/CMakeLists.txt
View File

@ -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

51
src/modules/fw_rate_control/FixedwingRateControl.cpp
View File

@ -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);

19
src/modules/fw_rate_control/FixedwingRateControl.hpp
View File

@ -69,7 +69,7 @@
#include <uORB/topics/vehicle_thrust_setpoint.h>
#include <uORB/topics/vehicle_torque_setpoint.h>
#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<px4::params::FW_BAT_SCALE_EN>) _param_fw_bat_scale_en,
(ParamFloat<px4::params::FW_DTRIM_P_VMAX>) _param_fw_dtrim_p_vmax,
(ParamFloat<px4::params::FW_DTRIM_P_VMIN>) _param_fw_dtrim_p_vmin,
(ParamFloat<px4::params::FW_DTRIM_R_VMAX>) _param_fw_dtrim_r_vmax,
(ParamFloat<px4::params::FW_DTRIM_R_VMIN>) _param_fw_dtrim_r_vmin,
(ParamFloat<px4::params::FW_DTRIM_Y_VMAX>) _param_fw_dtrim_y_vmax,
(ParamFloat<px4::params::FW_DTRIM_Y_VMIN>) _param_fw_dtrim_y_vmin,
(ParamFloat<px4::params::FW_MAN_P_SC>) _param_fw_man_p_sc,
(ParamFloat<px4::params::FW_MAN_R_SC>) _param_fw_man_r_sc,
(ParamFloat<px4::params::FW_MAN_Y_SC>) _param_fw_man_y_sc,
@ -202,17 +197,13 @@ private:
(ParamFloat<px4::params::FW_YR_P>) _param_fw_yr_p,
(ParamFloat<px4::params::FW_YR_D>) _param_fw_yr_d,
(ParamFloat<px4::params::TRIM_PITCH>) _param_trim_pitch,
(ParamFloat<px4::params::TRIM_ROLL>) _param_trim_roll,
(ParamFloat<px4::params::TRIM_YAW>) _param_trim_yaw,
(ParamInt<px4::params::FW_SPOILERS_MAN>) _param_fw_spoilers_man
)
RateControl _rate_control; ///< class for rate control calculations
FwAutoTrim _auto_trim{this};
SlewRate<matrix::Vector3f> _auto_trim_slew{}; ///< prevents large trim changes
FwTrim _trim{this};
SlewRate<matrix::Vector3f> _trim_slew{}; ///< prevents large trim changes
void updateActuatorControlsStatus(float dt);

7
src/modules/fw_rate_control/fw_auto_trim/CMakeLists.txt → src/modules/fw_rate_control/fw_trim/CMakeLists.txt
View File

@ -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})

6
src/modules/fw_rate_control/fw_auto_trim/FwAutoTrim.cpp → src/modules/fw_rate_control/fw_trim/FwAutoTrim.cpp
View File

@ -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<int>(_state));
printf("Trim validated\n");
_trim_validated.print();
printf("[AutoTrim] State: %d\n", static_cast<int>(_state));
printf("[AutoTrim] Trim validated = (%.3f, %.3f, %.3f)\n", (double)_trim_validated(0), (double)_trim_validated(1),
(double)_trim_validated(2));
return 0;
}

2
src/modules/fw_rate_control/fw_auto_trim/FwAutoTrim.hpp → src/modules/fw_rate_control/fw_trim/FwAutoTrim.hpp
View File

@ -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<px4::params::FW_AIRSPD_MIN>) _param_fw_airspd_min,

106
src/modules/fw_rate_control/fw_trim/FwTrim.cpp Normal file
View File

@ -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 <mathlib/mathlib.h>
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;
}

86
src/modules/fw_rate_control/fw_trim/FwTrim.hpp Normal file
View File

@ -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 <drivers/drv_hrt.h>
#include <px4_platform_common/defines.h>
#include <px4_platform_common/module.h>
#include <px4_platform_common/module_params.h>
#include <px4_platform_common/posix.h>
#include <uORB/topics/parameter_update.h>
#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<px4::params::TRIM_PITCH>) _param_trim_pitch,
(ParamFloat<px4::params::TRIM_ROLL>) _param_trim_roll,
(ParamFloat<px4::params::TRIM_YAW>) _param_trim_yaw,
(ParamFloat<px4::params::FW_DTRIM_P_VMAX>) _param_fw_dtrim_p_vmax,
(ParamFloat<px4::params::FW_DTRIM_P_VMIN>) _param_fw_dtrim_p_vmin,
(ParamFloat<px4::params::FW_DTRIM_R_VMAX>) _param_fw_dtrim_r_vmax,
(ParamFloat<px4::params::FW_DTRIM_R_VMIN>) _param_fw_dtrim_r_vmin,
(ParamFloat<px4::params::FW_DTRIM_Y_VMAX>) _param_fw_dtrim_y_vmax,
(ParamFloat<px4::params::FW_DTRIM_Y_VMIN>) _param_fw_dtrim_y_vmin,
(ParamFloat<px4::params::FW_AIRSPD_TRIM>) _param_fw_airspd_trim,
(ParamFloat<px4::params::FW_AIRSPD_MIN>) _param_fw_airspd_min,
(ParamFloat<px4::params::FW_AIRSPD_MAX>) _param_fw_airspd_max
)
};