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

Tailsitter: make pitch angle thresholds for transition in Stabilized depending on max manual pitch 

Transitions in Stabilized mode are done manually, the pilot controls the pitch angle
and if it's above the threshold the transition is declared finished (plus airspeed
check for front transition). Thus we can't have fixed thresholds but need to link
them to the actual max pitch angle in Stabilized mode.

Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
This commit is contained in:
Silvan Fuhrer 2023-05-11 17:50:46 +02:00
parent 1ddacec36b
commit 599a66c8a5
2 changed files with 26 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

23
src/modules/vtol_att_control/tailsitter.cpp
View File

@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2015-2022 PX4 Development Team. All rights reserved.
* Copyright (c) 2015-2023 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
@ -42,9 +42,6 @@
#include "tailsitter.h"
#include "vtol_att_control_main.h"
#define PITCH_TRANSITION_FRONT_P1 -1.1f // pitch angle to switch to TRANSITION_P2
#define PITCH_TRANSITION_BACK -0.25f // pitch angle to switch to MC
using namespace matrix;
Tailsitter::Tailsitter(VtolAttitudeControl *attc) :
@ -91,8 +88,15 @@ void Tailsitter::update_vtol_state()
case vtol_mode::TRANSITION_BACK:
const float pitch = Eulerf(Quatf(_v_att->q)).theta();
float pitch_threshold_mc = PITCH_THRESHOLD_AUTO_TRANSITION_TO_MC;
// if doing transition in Stabilized mode set threshold to max angle plus 5° margin
if (!_v_control_mode->flag_control_altitude_enabled) {
pitch_threshold_mc = math::radians(-_param_mpc_tilt_max.get() - 5.f);
}
// check if we have reached pitch angle to switch to MC mode
if (pitch >= PITCH_TRANSITION_BACK || _time_since_trans_start > _param_vt_b_trans_dur.get()) {
if (pitch >= pitch_threshold_mc || _time_since_trans_start > _param_vt_b_trans_dur.get()) {
_vtol_mode = vtol_mode::MC_MODE;
}
@ -325,7 +329,14 @@ bool Tailsitter::isFrontTransitionCompletedBase()
bool transition_to_fw = false;
const float pitch = Eulerf(Quatf(_v_att->q)).theta();
if (pitch <= PITCH_TRANSITION_FRONT_P1) {
float pitch_threshold_fw = PITCH_THRESHOLD_AUTO_TRANSITION_TO_FW;
// if doing transition in Stabilized mode set threshold to max angle minus 5° margin
if (!_v_control_mode->flag_control_altitude_enabled) {
pitch_threshold_fw = math::radians(-_param_mpc_tilt_max.get() + 5.f);
}
if (pitch <= pitch_threshold_fw) {
if (airspeed_triggers_transition) {
transition_to_fw = _airspeed_validated->calibrated_airspeed_m_s >= _param_vt_arsp_trans.get() ;

11
src/modules/vtol_att_control/tailsitter.h
View File

@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2015 PX4 Development Team. All rights reserved.
* Copyright (c) 2015-2023 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
@ -48,6 +48,12 @@
#include <drivers/drv_hrt.h>
#include <matrix/matrix/math.hpp>
// [rad] Pitch threshold required for completing transition to fixed-wing in automatic transitions
static constexpr float PITCH_THRESHOLD_AUTO_TRANSITION_TO_FW = -1.05f; // -60°
// [rad] Pitch threshold required for completing transition to hover in automatic transitions
static constexpr float PITCH_THRESHOLD_AUTO_TRANSITION_TO_MC = -0.26f; // -15°
class Tailsitter : public VtolType
{
@ -82,7 +88,8 @@ private:
bool isFrontTransitionCompletedBase() override;
DEFINE_PARAMETERS_CUSTOM_PARENT(VtolType,
(ParamFloat<px4::params::FW_PSP_OFF>) _param_fw_psp_off
(ParamFloat<px4::params::FW_PSP_OFF>) _param_fw_psp_off,
(ParamFloat<px4::params::MPC_MAN_TILT_MAX>) _param_mpc_tilt_max
)