forked from Archive/PX4-Autopilot
FwTrim class
This commit is contained in:
parent
5f992423c1
commit
afc9d6dfab
|
@ -27,7 +27,6 @@ airspeed_selector start
|
|||
# Start attitude control auto-tuner
|
||||
#
|
||||
fw_autotune_attitude_control start
|
||||
fw_auto_trim start
|
||||
|
||||
#
|
||||
# Start Land Detector.
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -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);
|
||||
|
|
|
@ -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);
|
||||
|
||||
|
|
|
@ -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})
|
|
@ -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;
|
||||
}
|
|
@ -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,
|
|
@ -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;
|
||||
}
|
|
@ -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
|
||||
)
|
||||
};
|
Loading…
Reference in New Issue