From ab4ceee34176e84bcc5e1104cd0b80574d9d2b3f Mon Sep 17 00:00:00 2001 From: bresch Date: Mon, 22 Jan 2024 17:24:21 +0100 Subject: [PATCH] Add new auto-trim feature for fixed-wings Estimates the torque trim values while flying --- msg/AutoTrimStatus.msg | 18 ++ msg/CMakeLists.txt | 1 + src/lib/slew_rate/SlewRate.hpp | 17 +- src/modules/fw_rate_control/CMakeLists.txt | 2 + .../fw_rate_control/FixedwingRateControl.cpp | 119 +++++----- .../fw_rate_control/FixedwingRateControl.hpp | 18 +- .../fw_rate_control/fw_rate_control_params.c | 15 ++ .../fw_rate_control/fw_trim/CMakeLists.txt | 39 ++++ .../fw_rate_control/fw_trim/FwAutoTrim.cpp | 203 ++++++++++++++++++ .../fw_rate_control/fw_trim/FwAutoTrim.hpp | 111 ++++++++++ .../fw_rate_control/fw_trim/FwTrim.cpp | 150 +++++++++++++ .../fw_rate_control/fw_trim/FwTrim.hpp | 95 ++++++++ src/modules/logger/logged_topics.cpp | 1 + 13 files changed, 713 insertions(+), 76 deletions(-) create mode 100644 msg/AutoTrimStatus.msg create mode 100644 src/modules/fw_rate_control/fw_trim/CMakeLists.txt create mode 100644 src/modules/fw_rate_control/fw_trim/FwAutoTrim.cpp create mode 100644 src/modules/fw_rate_control/fw_trim/FwAutoTrim.hpp 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/msg/AutoTrimStatus.msg b/msg/AutoTrimStatus.msg new file mode 100644 index 0000000000..efcf4adaa9 --- /dev/null +++ b/msg/AutoTrimStatus.msg @@ -0,0 +1,18 @@ +# Auto trimming state and current estimate +uint64 timestamp # Time since system start (microseconds) +uint64 timestamp_sample + +float32[3] trim_estimate # Roll/pitch/yaw trim value obtained from torque setpoints averaged over several seconds +float32[3] trim_estimate_var +float32[3] trim_test # Same as trim_estimate but done on a shorter period. Used to validate the trim estimate. +float32[3] trim_test_var +float32[3] trim_validated # Final roll/pitch/yaw trim estimate, verified by trim_test + +uint8 STATE_IDLE = 0 +uint8 STATE_SAMPLING = 1 +uint8 STATE_SAMPLING_TEST = 2 +uint8 STATE_VERIFICATION = 3 +uint8 STATE_COMPLETE = 4 +uint8 STATE_FAIL = 5 + +uint8 state diff --git a/msg/CMakeLists.txt b/msg/CMakeLists.txt index 596ca30fcd..f663100e74 100644 --- a/msg/CMakeLists.txt +++ b/msg/CMakeLists.txt @@ -52,6 +52,7 @@ set(msg_files ArmingCheckReply.msg ArmingCheckRequest.msg AutotuneAttitudeControlStatus.msg + AutoTrimStatus.msg BatteryStatus.msg Buffer128.msg ButtonEvent.msg diff --git a/src/lib/slew_rate/SlewRate.hpp b/src/lib/slew_rate/SlewRate.hpp index 363f2d2cc7..3ea5809835 100644 --- a/src/lib/slew_rate/SlewRate.hpp +++ b/src/lib/slew_rate/SlewRate.hpp @@ -56,13 +56,13 @@ public: * Set maximum rate of change for the value * @param slew_rate maximum rate of change */ - void setSlewRate(const Type slew_rate) { _slew_rate = slew_rate; } + void setSlewRate(const Type &slew_rate) { _slew_rate = slew_rate; } /** * Set value ignoring slew rate for initialization purpose * @param value new applied value */ - void setForcedValue(const Type value) { _value = value; } + void setForcedValue(const Type &value) { _value = value; } /** * Get value from last update of the slew rate @@ -76,7 +76,7 @@ public: * @param deltatime time in seconds since last update * @return actual value that complies with the slew rate */ - Type update(const Type new_value, const float deltatime) + Type update(const Type &new_value, const float deltatime) { // Limit the rate of change of the value const Type dvalue_desired = new_value - _value; @@ -90,3 +90,14 @@ protected: Type _slew_rate{}; ///< maximum rate of change for the value Type _value{}; ///< state to keep last value of the slew rate }; + +template<> +inline matrix::Vector3f SlewRate::update(const matrix::Vector3f &new_value, const float deltatime) +{ + // Limit the rate of change of the value + const matrix::Vector3f dvalue_desired = new_value - _value; + const matrix::Vector3f dvalue_max = _slew_rate * deltatime; + const matrix::Vector3f dvalue = matrix::constrain(dvalue_desired, -dvalue_max, dvalue_max); + _value += dvalue; + return _value; +} diff --git a/src/modules/fw_rate_control/CMakeLists.txt b/src/modules/fw_rate_control/CMakeLists.txt index 595c8314af..5400fc33a6 100644 --- a/src/modules/fw_rate_control/CMakeLists.txt +++ b/src/modules/fw_rate_control/CMakeLists.txt @@ -30,6 +30,7 @@ # POSSIBILITY OF SUCH DAMAGE. # ############################################################################ +add_subdirectory(fw_trim) px4_add_module( MODULE modules__fw_rate_control @@ -38,6 +39,7 @@ px4_add_module( FixedwingRateControl.cpp FixedwingRateControl.hpp DEPENDS + 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 1f2cff00d6..3db49ab890 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,6 +53,7 @@ FixedwingRateControl::FixedwingRateControl(bool vtol) : parameters_update(); _rate_ctrl_status_pub.advertise(); + _trim_slew.setSlewRate(Vector3f(0.01f, 0.01f, 0.01f)); } FixedwingRateControl::~FixedwingRateControl() @@ -96,6 +96,12 @@ FixedwingRateControl::parameters_update() return PX4_OK; } +void +FixedwingRateControl::save_params() +{ + _trim.saveParams(); +} + void FixedwingRateControl::vehicle_manual_poll() { @@ -127,13 +133,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); } @@ -256,7 +263,14 @@ void FixedwingRateControl::Run() const bool is_fixed_wing = _vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING; _in_fw_or_transition_wo_tailsitter_transition = is_fixed_wing || is_in_transition_except_tailsitter; - _vehicle_control_mode_sub.update(&_vcontrol_mode); + { + const bool armed_prev = _vcontrol_mode.flag_armed; + _vehicle_control_mode_sub.update(&_vcontrol_mode); + + if (!_vcontrol_mode.flag_armed && armed_prev) { + save_params(); + } + } vehicle_land_detected_poll(); @@ -321,78 +335,56 @@ 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()); + _trim.setAirspeed(airspeed); + _trim_slew.update(_trim.getTrim(), dt); - 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); + _rates_sp_sub.update(&_rates_sp); - } 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()); + Vector3f body_rates_setpoint = Vector3f(_rates_sp.roll, _rates_sp.pitch, _rates_sp.yaw); + + // Tailsitter: rotate setpoint from hover to fixed-wing frame (controller is in fixed-wing frame, interface in hover) + if (_vehicle_status.is_vtol_tailsitter) { + body_rates_setpoint = Vector3f(-_rates_sp.yaw, _rates_sp.pitch, _rates_sp.roll); } - if (_vcontrol_mode.flag_control_rates_enabled) { - _rates_sp_sub.update(&_rates_sp); + // Run attitude RATE controllers which need the desired attitudes from above, add trim. + const Vector3f angular_acceleration_setpoint = _rate_control.update(rates, body_rates_setpoint, angular_accel, dt, + _landed); - Vector3f body_rates_setpoint = Vector3f(_rates_sp.roll, _rates_sp.pitch, _rates_sp.yaw); + const Vector3f gain_ff(_param_fw_rr_ff.get(), _param_fw_pr_ff.get(), _param_fw_yr_ff.get()); + const Vector3f feedforward = gain_ff.emult(body_rates_setpoint) * _airspeed_scaling; - // Tailsitter: rotate setpoint from hover to fixed-wing frame (controller is in fixed-wing frame, interface in hover) - if (_vehicle_status.is_vtol_tailsitter) { - body_rates_setpoint = Vector3f(-_rates_sp.yaw, _rates_sp.pitch, _rates_sp.roll); - } + Vector3f control_u = angular_acceleration_setpoint * _airspeed_scaling * _airspeed_scaling + feedforward; - // Run attitude RATE controllers which need the desired attitudes from above, add trim. - const Vector3f angular_acceleration_setpoint = _rate_control.update(rates, body_rates_setpoint, angular_accel, dt, - _landed); + // Special case yaw in Acro: if the parameter FW_ACRO_YAW_CTL is not set then don't control yaw + if (!_vcontrol_mode.flag_control_attitude_enabled && !_param_fw_acro_yaw_en.get()) { + control_u(2) = _manual_control_setpoint.yaw * _param_fw_man_y_sc.get(); + _rate_control.resetIntegral(2); + } - const Vector3f gain_ff(_param_fw_rr_ff.get(), _param_fw_pr_ff.get(), _param_fw_yr_ff.get()); - const Vector3f feedforward = gain_ff.emult(body_rates_setpoint) * _airspeed_scaling; + if (control_u.isAllFinite()) { + matrix::constrain(control_u + _trim_slew.getState(), -1.f, 1.f).copyTo(_vehicle_torque_setpoint.xyz); - Vector3f control_u = angular_acceleration_setpoint * _airspeed_scaling * _airspeed_scaling + feedforward; + } else { + _rate_control.resetIntegral(); + _trim_slew.getState().copyTo(_vehicle_torque_setpoint.xyz); + } - // Special case yaw in Acro: if the parameter FW_ACRO_YAW_CTL is not set then don't control yaw - if (!_vcontrol_mode.flag_control_attitude_enabled && !_param_fw_acro_yaw_en.get()) { - control_u(2) = _manual_control_setpoint.yaw * _param_fw_man_y_sc.get(); - _rate_control.resetIntegral(2); - } + /* throttle passed through if it is finite */ + _vehicle_thrust_setpoint.xyz[0] = PX4_ISFINITE(_rates_sp.thrust_body[0]) ? _rates_sp.thrust_body[0] : 0.0f; - if (control_u.isAllFinite()) { - matrix::constrain(control_u + trim, -1.f, 1.f).copyTo(_vehicle_torque_setpoint.xyz); + /* scale effort by battery status */ + if (_param_fw_bat_scale_en.get() && _vehicle_thrust_setpoint.xyz[0] > 0.1f) { - } else { - _rate_control.resetIntegral(); - trim.copyTo(_vehicle_torque_setpoint.xyz); - } + if (_battery_status_sub.updated()) { + battery_status_s battery_status{}; - /* throttle passed through if it is finite */ - _vehicle_thrust_setpoint.xyz[0] = PX4_ISFINITE(_rates_sp.thrust_body[0]) ? _rates_sp.thrust_body[0] : 0.0f; - - /* scale effort by battery status */ - if (_param_fw_bat_scale_en.get() && _vehicle_thrust_setpoint.xyz[0] > 0.1f) { - - if (_battery_status_sub.updated()) { - battery_status_s battery_status{}; - - if (_battery_status_sub.copy(&battery_status) && battery_status.connected && battery_status.scale > 0.f) { - _battery_scale = battery_status.scale; - } + if (_battery_status_sub.copy(&battery_status) && battery_status.connected && battery_status.scale > 0.f) { + _battery_scale = battery_status.scale; } - - _vehicle_thrust_setpoint.xyz[0] *= _battery_scale; } + + _vehicle_thrust_setpoint.xyz[0] *= _battery_scale; } // publish rate controller status @@ -405,6 +397,7 @@ void FixedwingRateControl::Run() } else { // full manual _rate_control.resetIntegral(); + _trim.reset(); } // Add feed-forward from roll control output to yaw control output @@ -432,6 +425,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); diff --git a/src/modules/fw_rate_control/FixedwingRateControl.hpp b/src/modules/fw_rate_control/FixedwingRateControl.hpp index ec3b7e1c9e..94302e3e32 100644 --- a/src/modules/fw_rate_control/FixedwingRateControl.hpp +++ b/src/modules/fw_rate_control/FixedwingRateControl.hpp @@ -69,6 +69,8 @@ #include #include +#include "fw_trim/FwTrim.hpp" + using matrix::Eulerf; using matrix::Quatf; @@ -170,13 +172,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, @@ -200,15 +195,14 @@ 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 + FwTrim _trim{this}; + SlewRate _trim_slew{}; ///< prevents large trim changes + void updateActuatorControlsStatus(float dt); /** @@ -216,6 +210,8 @@ private: */ int parameters_update(); + void save_params(); + void vehicle_manual_poll(); void vehicle_land_detected_poll(); diff --git a/src/modules/fw_rate_control/fw_rate_control_params.c b/src/modules/fw_rate_control/fw_rate_control_params.c index 8493f11c72..7cde54dce5 100644 --- a/src/modules/fw_rate_control/fw_rate_control_params.c +++ b/src/modules/fw_rate_control/fw_rate_control_params.c @@ -295,6 +295,21 @@ PARAM_DEFINE_INT32(FW_BAT_SCALE_EN, 0); */ PARAM_DEFINE_INT32(FW_ARSP_SCALE_EN, 1); +/** + * Auto-Trim mode + * + * In calibration mode, the estimated trim is used to set the TRIM_ROLL/PITCH/YAW + * parameters after landing. The parameter is then changed to continuous mode. + * In continuous mode, part of the auto-trim estimated + * during flight is used to update the existing trim. + * + * @group FW Rate Control + * @value 0 Disabled + * @value 1 Calibration + * @value 2 Continuous + */ +PARAM_DEFINE_INT32(FW_ATRIM_MODE, 1); + /** * Roll trim increment at minimum airspeed * diff --git a/src/modules/fw_rate_control/fw_trim/CMakeLists.txt b/src/modules/fw_rate_control/fw_trim/CMakeLists.txt new file mode 100644 index 0000000000..718e7e3545 --- /dev/null +++ b/src/modules/fw_rate_control/fw_trim/CMakeLists.txt @@ -0,0 +1,39 @@ +############################################################################ +# +# 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. +# +############################################################################ +px4_add_library(FwTrim + FwTrim.cpp + FwAutoTrim.cpp +) + +# target_link_libraries(FwTrim) +target_include_directories(FwTrim PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) diff --git a/src/modules/fw_rate_control/fw_trim/FwAutoTrim.cpp b/src/modules/fw_rate_control/fw_trim/FwAutoTrim.cpp new file mode 100644 index 0000000000..7b5b4a4d7c --- /dev/null +++ b/src/modules/fw_rate_control/fw_trim/FwAutoTrim.cpp @@ -0,0 +1,203 @@ +/**************************************************************************** + * + * 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 "FwAutoTrim.hpp" + +#include + +using namespace time_literals; +using matrix::Vector3f; + +FwAutoTrim::FwAutoTrim(ModuleParams *parent) : + ModuleParams(parent) +{ + _auto_trim_status_pub.advertise(); + updateParams(); +} + +FwAutoTrim::~FwAutoTrim() +{ + perf_free(_cycle_perf); +} + +void FwAutoTrim::updateParams() +{ + ModuleParams::updateParams(); +} + +void FwAutoTrim::reset() +{ + _state = state::idle; + _trim_estimate.reset(); + _trim_test.reset(); + _trim_validated.zero(); +} + +void FwAutoTrim::update(const Vector3f &torque_sp, const float dt) +{ + if (_vehicle_land_detected_sub.updated()) { + vehicle_land_detected_s vehicle_land_detected; + + if (_vehicle_land_detected_sub.copy(&vehicle_land_detected)) { + _landed = vehicle_land_detected.landed; + } + } + + perf_begin(_cycle_perf); + + if (_vehicle_status_sub.updated()) { + vehicle_status_s vehicle_status; + + if (_vehicle_status_sub.copy(&vehicle_status)) { + _armed = (vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED); + _fixed_wing = (vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING); + _tailsitter = vehicle_status.is_vtol_tailsitter; + } + } + + if (_airspeed_validated_sub.updated()) { + airspeed_validated_s airspeed_validated; + + if (_airspeed_validated_sub.copy(&airspeed_validated)) { + if (PX4_ISFINITE(airspeed_validated.calibrated_airspeed_m_s)) { + _calibrated_airspeed_m_s = airspeed_validated.calibrated_airspeed_m_s; + + } else { + _calibrated_airspeed_m_s = airspeed_validated.calibrated_ground_minus_wind_m_s; + } + } + } + + if (_vehicle_attitude_sub.updated()) { + vehicle_attitude_s vehicle_attitude; + + if (_vehicle_attitude_sub.copy(&vehicle_attitude)) { + const Vector3f earth_z_in_body_frame = matrix::Quatf(vehicle_attitude.q).rotateVectorInverse(Vector3f(0.f, 0.f, 1.f)); + _cos_tilt = _tailsitter ? earth_z_in_body_frame(0) : earth_z_in_body_frame(2); + } + } + + const hrt_abstime now = hrt_absolute_time(); + + const bool run_auto_trim = _fixed_wing + && _armed + && !_landed + && (dt > 0.001f) && (dt < 0.1f) + && torque_sp.isAllFinite() + && ((_calibrated_airspeed_m_s >= _param_fw_airspd_min.get() && _calibrated_airspeed_m_s <= _param_fw_airspd_max.get()) + || (!PX4_ISFINITE(_calibrated_airspeed_m_s) && (_param_sys_has_num_aspd.get() == 0))) + && _cos_tilt > cosf(math::radians(_kTiltMaxDeg)); + + if (run_auto_trim) { + switch (_state) { + default: + + // fallthrough + case state::idle: + _trim_estimate.reset(); + _trim_test.reset(); + _state = state::sampling; + _state_start_time = now; + break; + + case state::sampling: + // Average the torque setpoint over a long period of time + _trim_estimate.update(torque_sp); + + if ((now - _state_start_time) > 5_s) { + _state = state::sampling_test; + _state_start_time = now; + } + + break; + + case state::sampling_test: + // Average a smaller amount of data to validate the trim estimate + _trim_test.update(torque_sp); + + if ((now - _state_start_time) > 2_s) { + _state = state::verification; + _state_start_time = now; + } + + break; + + case state::verification: { + // Perform a statistical test between the estimated and test data + const float gate = 5.f; + const Vector3f innovation = _trim_test.mean() - _trim_estimate.mean(); + const Vector3f innovation_variance = _trim_test.variance() + _trim_estimate.variance(); + const float test_ratio = Vector3f(innovation.edivide(innovation_variance * gate * gate)) * innovation; + + if ((test_ratio <= 1.f) && !innovation.longerThan(0.05f)) { + _trim_validated = _trim_estimate.mean(); + _state = state::complete; + _state_start_time = now; + + } else { + _state = state::fail; + } + + } break; + + case state::complete: + //TODO: depending on param, stay or restart + _state = state::idle; + break; + } + + } else { + _state = state::fail; + } + + publishStatus(now); + + perf_end(_cycle_perf); +} + +void FwAutoTrim::publishStatus(const hrt_abstime now) +{ + auto_trim_status_s status_msg{}; + + _trim_validated.copyTo(status_msg.trim_validated); + _trim_estimate.mean().copyTo(status_msg.trim_estimate); + _trim_estimate.variance().copyTo(status_msg.trim_estimate_var); + _trim_test.mean().copyTo(status_msg.trim_test); + _trim_test.variance().copyTo(status_msg.trim_test_var); + status_msg.state = static_cast(_state); + + status_msg.timestamp = now; + status_msg.timestamp_sample = now; + + _auto_trim_status_pub.publish(status_msg); +} diff --git a/src/modules/fw_rate_control/fw_trim/FwAutoTrim.hpp b/src/modules/fw_rate_control/fw_trim/FwAutoTrim.hpp new file mode 100644 index 0000000000..c310ef667b --- /dev/null +++ b/src/modules/fw_rate_control/fw_trim/FwAutoTrim.hpp @@ -0,0 +1,111 @@ +/**************************************************************************** + * + * 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 +#include +#include +#include +#include +#include +#include +#include +#include +#include + +using namespace time_literals; + +class FwAutoTrim : public ModuleParams +{ +public: + FwAutoTrim(ModuleParams *parent); + ~FwAutoTrim(); + + void reset(); + void update(const matrix::Vector3f &torque_sp, float dt); + + const matrix::Vector3f &getTrim() const { return _trim_validated; } + +protected: + void updateParams() override; + +private: + void publishStatus(const hrt_abstime now); + + uORB::Publication _auto_trim_status_pub{ORB_ID(auto_trim_status)}; + + uORB::Subscription _vehicle_land_detected_sub{ORB_ID(vehicle_land_detected)}; + uORB::Subscription _airspeed_validated_sub{ORB_ID(airspeed_validated)}; + uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)}; + uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)}; + + enum class state { + idle = auto_trim_status_s::STATE_IDLE, + sampling = auto_trim_status_s::STATE_SAMPLING, + sampling_test = auto_trim_status_s::STATE_SAMPLING_TEST, + verification = auto_trim_status_s::STATE_VERIFICATION, + complete = auto_trim_status_s::STATE_COMPLETE, + fail = auto_trim_status_s::STATE_FAIL, + } _state{state::idle}; + + hrt_abstime _state_start_time{0}; + + bool _armed{false}; + bool _landed{false}; + bool _fixed_wing{false}; + bool _tailsitter{false}; + float _calibrated_airspeed_m_s{0.f}; + float _cos_tilt{0.f}; + + math::WelfordMeanVector _trim_estimate{}; + math::WelfordMeanVector _trim_test{}; + + matrix::Vector3f _trim_validated{}; + + perf_counter_t _cycle_perf{perf_alloc(PC_ELAPSED, "[AutoTrim] cycle time")}; + + DEFINE_PARAMETERS( + (ParamInt) _param_sys_has_num_aspd, + (ParamFloat) _param_fw_airspd_min, + (ParamFloat) _param_fw_airspd_max + ) + + static constexpr float _kTiltMaxDeg = 10.f; +}; 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..07eb04aadb --- /dev/null +++ b/src/modules/fw_rate_control/fw_trim/FwTrim.cpp @@ -0,0 +1,150 @@ +/**************************************************************************** + * + * 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::saveParams() +{ + const Vector3f autotrim = _auto_trim.getTrim(); + bool updated = false; + const Vector3f trim_prev(_param_trim_roll.get(), _param_trim_pitch.get(), _param_trim_yaw.get()); + + if (_param_fw_atrim_mode.get() == static_cast(AutoTrimMode::kCalibration)) { + // Replace the current trim with the one identified during auto-trim + updated |= _param_trim_roll.commit_no_notification(_param_trim_roll.get() + autotrim(0)); + updated |= _param_trim_pitch.commit_no_notification(_param_trim_pitch.get() + autotrim(1)); + updated |= _param_trim_yaw.commit_no_notification(_param_trim_yaw.get() + autotrim(2)); + + if (updated) { + _param_fw_atrim_mode.set(static_cast(AutoTrimMode::kContinuous)); + _param_fw_atrim_mode.commit(); + } + + } else if (_param_fw_atrim_mode.get() == static_cast(AutoTrimMode::kContinuous)) { + // In continuous trim mode, limit the amount of trim that can be applied back to the parameter + const Vector3f constrained_autotrim = matrix::constrain(autotrim, -0.05f, 0.05f); + updated |= _param_trim_roll.commit_no_notification(_param_trim_roll.get() + constrained_autotrim(0)); + updated |= _param_trim_pitch.commit_no_notification(_param_trim_pitch.get() + constrained_autotrim(1)); + updated |= _param_trim_yaw.commit_no_notification(_param_trim_yaw.get() + constrained_autotrim(2)); + + if (updated) { + _param_trim_yaw.commit(); + } + + } else { + // nothing to do + } + + if (updated) { + const Vector3f trim_new(_param_trim_roll.get(), _param_trim_pitch.get(), _param_trim_yaw.get()); + PX4_INFO("Trim offset committed: [%.3f %.3f %.3f]->[%.3f %.3f %.3f]", + (double)trim_prev(0), (double)trim_prev(1), (double)trim_prev(2), + (double)trim_new(0), (double)trim_new(1), (double)trim_new(2)); + } + + _auto_trim.reset(); +} + +void FwTrim::reset() +{ + _auto_trim.reset(); +} + +void FwTrim::updateAutoTrim(const Vector3f &torque_sp, const float dt) +{ + _auto_trim.update(torque_sp - _parameterized_trim, dt); +} + +Vector3f FwTrim::getTrim() const +{ + Vector3f trim = _parameterized_trim; + + if (_param_fw_atrim_mode.get() > 0) { + trim += _auto_trim.getTrim(); + } + + return trim; +} + +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; +} 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..915af78161 --- /dev/null +++ b/src/modules/fw_rate_control/fw_trim/FwTrim.hpp @@ -0,0 +1,95 @@ +/**************************************************************************** + * + * 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 + +#include "FwAutoTrim.hpp" + +using namespace time_literals; + +class FwTrim : public ModuleParams +{ +public: + FwTrim(ModuleParams *parent); + ~FwTrim() = default; + + void saveParams(); + void reset(); + void setAirspeed(float airspeed); + void updateAutoTrim(const matrix::Vector3f &torque_sp, float dt); + matrix::Vector3f getTrim() const; + +protected: + void updateParams() override; + +private: + enum class AutoTrimMode { + kDisabled = 0, + kCalibration, + kContinuous + }; + + void updateParameterizedTrim(); + + FwAutoTrim _auto_trim{this}; + matrix::Vector3f _parameterized_trim{}; + + float _airspeed{0.f}; + + DEFINE_PARAMETERS( + (ParamInt) _param_fw_atrim_mode, + + (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 + ) +}; diff --git a/src/modules/logger/logged_topics.cpp b/src/modules/logger/logged_topics.cpp index d468563597..6463f4b0ec 100644 --- a/src/modules/logger/logged_topics.cpp +++ b/src/modules/logger/logged_topics.cpp @@ -51,6 +51,7 @@ void LoggedTopics::add_default_topics() add_topic("airspeed", 1000); add_optional_topic("airspeed_validated", 200); add_optional_topic("autotune_attitude_control_status", 100); + add_optional_topic("auto_trim_status", 200); add_optional_topic("camera_capture"); add_optional_topic("camera_trigger"); add_optional_topic("can_interface_status", 10);