fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-07-12 22:00:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

Tiltrotor: move spin up tilt to control allocation (#21991)

Browse Source 
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 <silvan@auterion.com>
This commit is contained in:
Silvan Fuhrer
2023-08-24 13:57:24 +02:00
committed by GitHub
parent 86822171b9
commit 0eb276f273
5 changed files with 54 additions and 58 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.cpp
+31 -1
View File
@@ -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<flo
for (int i = 0; i < _tilts.count(); ++i) {
if (_tilts.config(i).tilt_direction == ActuatorEffectivenessTilts::TiltDirection::TowardsFront) {
actuator_sp(i + _first_tilt_idx) += control_collective_tilt;
// as long as throttle spoolup is not completed, leave the tilts in the disarmed position (in hover)
if (throttleSpoolupFinished() || _flight_phase != FlightPhase::HOVER_FLIGHT) {
actuator_sp(i + _first_tilt_idx) += control_collective_tilt;
} else {
actuator_sp(i + _first_tilt_idx) = NAN; // NaN sets tilts to disarmed position
}
}
}
@@ -213,3 +231,15 @@ void ActuatorEffectivenessTiltrotorVTOL::getUnallocatedControl(int matrix_index,
status.unallocated_torque[2] = 0.f;
}
}
bool ActuatorEffectivenessTiltrotorVTOL::throttleSpoolupFinished()
{
vehicle_status_s vehicle_status;
if (_vehicle_status_sub.update(&vehicle_status)) {
_armed = vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED;
_armed_time = vehicle_status.armed_time;
}
return _armed && hrt_elapsed_time(&_armed_time) > _param_spoolup_time * 1_s;
}
src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.hpp
+21 -1
View File
@@ -46,9 +46,11 @@
#include "ActuatorEffectivenessControlSurfaces.hpp"
#include "ActuatorEffectivenessTilts.hpp"
#include <px4_platform_common/module_params.h>
#include <uORB/topics/normalized_unsigned_setpoint.h>
#include <uORB/topics/tiltrotor_extra_controls.h>
#include <uORB/topics/vehicle_status.h>
#include <uORB/Subscription.hpp>
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};
};
src/modules/vtol_att_control/tiltrotor.cpp
+2 -39
View File
@@ -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()
src/modules/vtol_att_control/tiltrotor.h
-3
View File
@@ -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<px4::params::VT_TILT_MC>) _param_vt_tilt_mc,
(ParamFloat<px4::params::VT_TILT_TRANS>) _param_vt_tilt_trans,
(ParamFloat<px4::params::VT_TILT_FW>) _param_vt_tilt_fw,
(ParamFloat<px4::params::VT_TILT_SPINUP>) _param_vt_tilt_spinup,
(ParamFloat<px4::params::VT_TRANS_P2_DUR>) _param_vt_trans_p2_dur,
(ParamFloat<px4::params::VT_BT_TILT_DUR>) _param_vt_bt_tilt_dur
)
src/modules/vtol_att_control/tiltrotor_params.c
-14
View File
@@ -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
*