From 0eb276f2737284733f6375296f09ae0b4e1c2c42 Mon Sep 17 00:00:00 2001 From: Silvan Fuhrer Date: Thu, 24 Aug 2023 13:57:24 +0200 Subject: [PATCH] Tiltrotor: move spin up tilt to control allocation (#21991) EffectivenessTiltrotor: link time when to tilt motors to MC position to COM_SPOOLUP_TIME - remove VT_TILT_SPINUP and special spin up tilt handling form the VTOL module - now handle the spoolup in the allocation, directly linked to COM_SPOOLUP_TIME - leave tilts at disarmed value during spoolup Signed-off-by: Silvan Fuhrer --- .../ActuatorEffectivenessTiltrotorVTOL.cpp | 32 ++++++++++++++- .../ActuatorEffectivenessTiltrotorVTOL.hpp | 22 +++++++++- src/modules/vtol_att_control/tiltrotor.cpp | 41 +------------------ src/modules/vtol_att_control/tiltrotor.h | 3 -- .../vtol_att_control/tiltrotor_params.c | 14 ------- 5 files changed, 54 insertions(+), 58 deletions(-) diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.cpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.cpp index 592c157f5a..970c2dbf67 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.cpp +++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.cpp @@ -48,8 +48,19 @@ ActuatorEffectivenessTiltrotorVTOL::ActuatorEffectivenessTiltrotorVTOL(ModulePar _mc_rotors(this, ActuatorEffectivenessRotors::AxisConfiguration::Configurable, true), _control_surfaces(this), _tilts(this) { + _param_handles.com_spoolup_time = param_find("COM_SPOOLUP_TIME"); + + updateParams(); setFlightPhase(FlightPhase::HOVER_FLIGHT); } + +void ActuatorEffectivenessTiltrotorVTOL::updateParams() +{ + ModuleParams::updateParams(); + + param_get(_param_handles.com_spoolup_time, &_param_spoolup_time); +} + bool ActuatorEffectivenessTiltrotorVTOL::getEffectivenessMatrix(Configuration &configuration, EffectivenessUpdateReason external_update) @@ -139,7 +150,14 @@ void ActuatorEffectivenessTiltrotorVTOL::updateSetpoint(const matrix::Vector _param_spoolup_time * 1_s; +} diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.hpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.hpp index f8dd985290..310d937064 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.hpp +++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.hpp @@ -46,9 +46,11 @@ #include "ActuatorEffectivenessControlSurfaces.hpp" #include "ActuatorEffectivenessTilts.hpp" +#include + #include #include - +#include #include class ActuatorEffectivenessTiltrotorVTOL : public ModuleParams, public ActuatorEffectiveness @@ -111,4 +113,22 @@ protected: YawTiltSaturationFlags _yaw_tilt_saturation_flags{}; uORB::Subscription _tiltrotor_extra_controls_sub{ORB_ID(tiltrotor_extra_controls)}; + +private: + + void updateParams() override; + + struct ParamHandles { + param_t com_spoolup_time; + }; + + ParamHandles _param_handles{}; + + float _param_spoolup_time{1.f}; + + // Tilt handling during motor spoolup: leave the tilts in their disarmed position unitil 1s after arming + bool throttleSpoolupFinished(); + uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)}; + bool _armed{false}; + uint64_t _armed_time{0}; }; diff --git a/src/modules/vtol_att_control/tiltrotor.cpp b/src/modules/vtol_att_control/tiltrotor.cpp index 2215b96de2..c06d7e888e 100644 --- a/src/modules/vtol_att_control/tiltrotor.cpp +++ b/src/modules/vtol_att_control/tiltrotor.cpp @@ -43,7 +43,6 @@ #include "vtol_att_control_main.h" using namespace matrix; -using namespace time_literals; #define FRONTTRANS_THR_MIN 0.25f #define BACKTRANS_THROTTLE_DOWNRAMP_DUR_S 0.5f @@ -182,44 +181,8 @@ void Tiltrotor::update_mc_state() { VtolType::update_mc_state(); - /*Motor spin up: define the first second after arming as motor spin up time, during which - * the tilt is set to the value of VT_TILT_SPINUP. This allows the user to set a spin up - * tilt angle in case the propellers don't spin up smoothly in full upright (MC mode) position. - */ - - const int spin_up_duration_p1 = 1000_ms; // duration of 1st phase of spinup (at fixed tilt) - const int spin_up_duration_p2 = 700_ms; // duration of 2nd phase of spinup (transition from spinup tilt to mc tilt) - - // reset this timestamp while disarmed - if (!_v_control_mode->flag_armed) { - _last_timestamp_disarmed = hrt_absolute_time(); - _tilt_motors_for_startup = _param_vt_tilt_spinup.get() > 0.01f; // spinup phase only required if spinup tilt > 0 - - } else if (_tilt_motors_for_startup) { - // leave motors tilted forward after arming to allow them to spin up easier - if (hrt_absolute_time() - _last_timestamp_disarmed > (spin_up_duration_p1 + spin_up_duration_p2)) { - _tilt_motors_for_startup = false; - } - } - - if (_tilt_motors_for_startup) { - if (hrt_absolute_time() - _last_timestamp_disarmed < spin_up_duration_p1) { - _tilt_control = _param_vt_tilt_spinup.get(); - - } else { - // duration phase 2: begin to adapt tilt to multicopter tilt - float delta_tilt = (_param_vt_tilt_mc.get() - _param_vt_tilt_spinup.get()); - _tilt_control = _param_vt_tilt_spinup.get() + delta_tilt / spin_up_duration_p2 * (hrt_absolute_time() - - (_last_timestamp_disarmed + spin_up_duration_p1)); - } - - _mc_yaw_weight = 0.0f; //disable yaw control during spinup - - } else { - // normal operation - _tilt_control = VtolType::pusher_assist() + _param_vt_tilt_mc.get(); - _mc_yaw_weight = 1.0f; - } + _tilt_control = VtolType::pusher_assist() + _param_vt_tilt_mc.get(); + _mc_yaw_weight = 1.0f; } void Tiltrotor::update_fw_state() diff --git a/src/modules/vtol_att_control/tiltrotor.h b/src/modules/vtol_att_control/tiltrotor.h index b5baa864da..cac172c2ae 100644 --- a/src/modules/vtol_att_control/tiltrotor.h +++ b/src/modules/vtol_att_control/tiltrotor.h @@ -91,14 +91,11 @@ private: void blendThrottleDuringBacktransition(const float scale, const float target_throttle); bool isFrontTransitionCompletedBase() override; - hrt_abstime _last_timestamp_disarmed{0}; /**< used for calculating time since arming */ - bool _tilt_motors_for_startup{false}; DEFINE_PARAMETERS_CUSTOM_PARENT(VtolType, (ParamFloat) _param_vt_tilt_mc, (ParamFloat) _param_vt_tilt_trans, (ParamFloat) _param_vt_tilt_fw, - (ParamFloat) _param_vt_tilt_spinup, (ParamFloat) _param_vt_trans_p2_dur, (ParamFloat) _param_vt_bt_tilt_dur ) diff --git a/src/modules/vtol_att_control/tiltrotor_params.c b/src/modules/vtol_att_control/tiltrotor_params.c index 9d8d76ea89..92fa558454 100644 --- a/src/modules/vtol_att_control/tiltrotor_params.c +++ b/src/modules/vtol_att_control/tiltrotor_params.c @@ -71,20 +71,6 @@ PARAM_DEFINE_FLOAT(VT_TILT_TRANS, 0.4f); */ PARAM_DEFINE_FLOAT(VT_TILT_FW, 1.0f); -/** - * Tilt when disarmed and in the first second after arming - * - * This specific tilt during spin-up is necessary for some systems whose motors otherwise don't - * spin-up freely. - * - * @min 0.0 - * @max 1.0 - * @increment 0.01 - * @decimal 2 - * @group VTOL Attitude Control - */ -PARAM_DEFINE_FLOAT(VT_TILT_SPINUP, 0.0f); - /** * Duration of front transition phase 2 *