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

VTOL: do central handling of airspeed measurement

Browse Source 
- only use data from airspeed_validated topic if source is SENSOR
    - add 1s timeout (set to NAN if older)
    - use FW_USE_AIRSPD consitently (treat the same as CAS=NAN)

Signed-off-by: Silvan <silvan@auterion.com>
This commit is contained in:
Silvan 2025-04-04 16:57:40 +02:00 committed by Silvan Fuhrer
parent f7bb5d13f7
commit 6d12b04bb0
7 changed files with 42 additions and 28 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

15
src/modules/vtol_att_control/standard.cpp
View File

@ -104,8 +104,8 @@ void Standard::update_vtol_state()
const Vector3f vel = R_to_body * Vector3f(_local_pos->vx, _local_pos->vy, _local_pos->vz);
exit_backtransition_speed_condition = vel(0) < _param_mpc_xy_cruise.get();
} else if (PX4_ISFINITE(_airspeed_validated->calibrated_airspeed_m_s)) {
exit_backtransition_speed_condition = _airspeed_validated->calibrated_airspeed_m_s < _param_mpc_xy_cruise.get();
} else if (PX4_ISFINITE(_attc->get_calibrated_airspeed())) {
exit_backtransition_speed_condition = _attc->get_calibrated_airspeed() < _param_mpc_xy_cruise.get();
}
const bool exit_backtransition_time_condition = _time_since_trans_start > _param_vt_b_trans_dur.get();
@ -220,19 +220,18 @@ void Standard::update_transition_state()
// do blending of mc and fw controls if a blending airspeed has been provided and the minimum transition time has passed
if (_airspeed_trans_blend_margin > 0.0f &&
PX4_ISFINITE(_airspeed_validated->calibrated_airspeed_m_s) &&
_airspeed_validated->calibrated_airspeed_m_s > 0.0f &&
_airspeed_validated->calibrated_airspeed_m_s >= getBlendAirspeed() &&
PX4_ISFINITE(_attc->get_calibrated_airspeed()) &&
_attc->get_calibrated_airspeed() > 0.0f &&
_attc->get_calibrated_airspeed() >= getBlendAirspeed() &&
_time_since_trans_start > getMinimumFrontTransitionTime()) {
mc_weight = 1.0f - fabsf(_airspeed_validated->calibrated_airspeed_m_s - getBlendAirspeed()) /
mc_weight = 1.0f - fabsf(_attc->get_calibrated_airspeed() - getBlendAirspeed()) /
_airspeed_trans_blend_margin;
// time based blending when no airspeed sensor is set
} else if (!_param_fw_use_airspd.get() || !PX4_ISFINITE(_airspeed_validated->calibrated_airspeed_m_s)) {
} else if (!PX4_ISFINITE(_attc->get_calibrated_airspeed())) {
mc_weight = 1.0f - _time_since_trans_start / getMinimumFrontTransitionTime();
mc_weight = math::constrain(2.0f * mc_weight, 0.0f, 1.0f);
}
// ramp up FW_PSP_OFF

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

@ -336,15 +336,14 @@ void Tailsitter::fill_actuator_outputs()
bool Tailsitter::isFrontTransitionCompletedBase()
{
const bool airspeed_triggers_transition = PX4_ISFINITE(_airspeed_validated->calibrated_airspeed_m_s)
&& _param_fw_use_airspd.get();
const bool airspeed_triggers_transition = PX4_ISFINITE(_attc->get_calibrated_airspeed());
bool transition_to_fw = false;
const float pitch = Eulerf(Quatf(_v_att->q)).theta();
if (pitch <= PITCH_THRESHOLD_AUTO_TRANSITION_TO_FW) {
if (airspeed_triggers_transition) {
transition_to_fw = _airspeed_validated->calibrated_airspeed_m_s >= _param_vt_arsp_trans.get() ;
transition_to_fw = _attc->get_calibrated_airspeed() >= _param_vt_arsp_trans.get() ;
} else {
transition_to_fw = true;

12
src/modules/vtol_att_control/tiltrotor.cpp
View File

@ -104,8 +104,8 @@ void Tiltrotor::update_vtol_state()
const Vector3f vel = R_to_body * Vector3f(_local_pos->vx, _local_pos->vy, _local_pos->vz);
exit_backtransition_speed_condition = vel(0) < _param_mpc_xy_cruise.get() ;
} else if (PX4_ISFINITE(_airspeed_validated->calibrated_airspeed_m_s)) {
exit_backtransition_speed_condition = _airspeed_validated->calibrated_airspeed_m_s < _param_mpc_xy_cruise.get() ;
} else if (PX4_ISFINITE(_attc->get_calibrated_airspeed())) {
exit_backtransition_speed_condition = _attc->get_calibrated_airspeed() < _param_mpc_xy_cruise.get() ;
}
const bool exit_backtransition_time_condition = _time_since_trans_start > _param_vt_b_trans_dur.get() ;
@ -251,14 +251,14 @@ void Tiltrotor::update_transition_state()
_mc_roll_weight = 1.0f;
_mc_yaw_weight = 1.0f;
if (_param_fw_use_airspd.get() && PX4_ISFINITE(_airspeed_validated->calibrated_airspeed_m_s) &&
_airspeed_validated->calibrated_airspeed_m_s >= getBlendAirspeed()) {
_mc_roll_weight = 1.0f - (_airspeed_validated->calibrated_airspeed_m_s - getBlendAirspeed()) /
if (PX4_ISFINITE(_attc->get_calibrated_airspeed()) &&
_attc->get_calibrated_airspeed() >= getBlendAirspeed()) {
_mc_roll_weight = 1.0f - (_attc->get_calibrated_airspeed() - getBlendAirspeed()) /
(getTransitionAirspeed() - getBlendAirspeed());
}
// without airspeed do timed weight changes
if ((!_param_fw_use_airspd.get() || !PX4_ISFINITE(_airspeed_validated->calibrated_airspeed_m_s)) &&
if ((!PX4_ISFINITE(_attc->get_calibrated_airspeed())) &&
_time_since_trans_start > getMinimumFrontTransitionTime()) {
_mc_roll_weight = 1.0f - (_time_since_trans_start - getMinimumFrontTransitionTime()) /
(getOpenLoopFrontTransitionTime() - getMinimumFrontTransitionTime());

18
src/modules/vtol_att_control/vtol_att_control_main.cpp
View File

@ -350,7 +350,6 @@ VtolAttitudeControl::Run()
_local_pos_sub.update(&_local_pos);
_local_pos_sp_sub.update(&_local_pos_sp);
_pos_sp_triplet_sub.update(&_pos_sp_triplet);
_airspeed_validated_sub.update(&_airspeed_validated);
_tecs_status_sub.update(&_tecs_status);
_land_detected_sub.update(&_land_detected);
@ -365,6 +364,23 @@ VtolAttitudeControl::Run()
}
}
if (_airspeed_validated_sub.updated()) {
airspeed_validated_s airspeed_validated;
if (_airspeed_validated_sub.copy(&airspeed_validated)) {
const bool airspeed_from_sensor = airspeed_validated.airspeed_source == airspeed_validated_s::SENSOR_1
|| airspeed_validated.airspeed_source == airspeed_validated_s::SENSOR_2
|| airspeed_validated.airspeed_source == airspeed_validated_s::SENSOR_3;
const bool use_airspeed = _param_fw_use_airspd.get() && airspeed_from_sensor;
_calibrated_airspeed = use_airspeed ? airspeed_validated.calibrated_airspeed_m_s : NAN;
_time_last_airspeed_update = airspeed_validated.timestamp;
} else if (hrt_elapsed_time(&_time_last_airspeed_update) > 1_s) {
_calibrated_airspeed = NAN;
}
}
vehicle_status_poll();
action_request_poll();
vehicle_cmd_poll();

9
src/modules/vtol_att_control/vtol_att_control_main.h
View File

@ -128,7 +128,6 @@ public:
struct vehicle_thrust_setpoint_s *get_vehicle_thrust_setpoint_virtual_mc() {return &_vehicle_thrust_setpoint_virtual_mc;}
struct vehicle_thrust_setpoint_s *get_vehicle_thrust_setpoint_virtual_fw() {return &_vehicle_thrust_setpoint_virtual_fw;}
struct airspeed_validated_s *get_airspeed() {return &_airspeed_validated;}
struct position_setpoint_triplet_s *get_pos_sp_triplet() {return &_pos_sp_triplet;}
struct tecs_status_s *get_tecs_status() {return &_tecs_status;}
struct vehicle_attitude_s *get_att() {return &_vehicle_attitude;}
@ -144,7 +143,9 @@ public:
struct vehicle_thrust_setpoint_s *get_thrust_setpoint_0() {return &_thrust_setpoint_0;}
struct vehicle_thrust_setpoint_s *get_thrust_setpoint_1() {return &_thrust_setpoint_1;}
struct vtol_vehicle_status_s *get_vtol_vehicle_status() {return &_vtol_vehicle_status;}
float get_home_position_z() { return _home_position_z; }
float get_calibrated_airspeed() { return _calibrated_airspeed; }
private:
void Run() override;
@ -196,7 +197,6 @@ private:
vehicle_thrust_setpoint_s _thrust_setpoint_0{};
vehicle_thrust_setpoint_s _thrust_setpoint_1{};
airspeed_validated_s _airspeed_validated{};
position_setpoint_triplet_s _pos_sp_triplet{};
tecs_status_s _tecs_status{};
vehicle_attitude_s _vehicle_attitude{};
@ -208,6 +208,8 @@ private:
vtol_vehicle_status_s _vtol_vehicle_status{};
vtol_vehicle_status_s _prev_published_vtol_vehicle_status{};
float _home_position_z{NAN};
float _calibrated_airspeed{NAN};
hrt_abstime _time_last_airspeed_update{0};
float _air_density{atmosphere::kAirDensitySeaLevelStandardAtmos}; // [kg/m^3]
@ -242,6 +244,7 @@ private:
DEFINE_PARAMETERS(
(ParamInt<px4::params::VT_TYPE>) _param_vt_type,
(ParamFloat<px4::params::VT_SPOILER_MC_LD>) _param_vt_spoiler_mc_ld
(ParamFloat<px4::params::VT_SPOILER_MC_LD>) _param_vt_spoiler_mc_ld,
(ParamBool<px4::params::FW_USE_AIRSPD>) _param_fw_use_airspd
)
};

10
src/modules/vtol_att_control/vtol_type.cpp
View File

@ -78,7 +78,6 @@ VtolType::VtolType(VtolAttitudeControl *att_controller) :
_thrust_setpoint_1 = _attc->get_thrust_setpoint_1();
_local_pos = _attc->get_local_pos();
_local_pos_sp = _attc->get_local_pos_sp();
_airspeed_validated = _attc->get_airspeed();
_tecs_status = _attc->get_tecs_status();
_land_detected = _attc->get_land_detected();
}
@ -175,8 +174,7 @@ bool VtolType::isFrontTransitionCompleted()
bool VtolType::isFrontTransitionCompletedBase()
{
// continue the transition to fw mode while monitoring airspeed for a final switch to fw mode
const bool airspeed_triggers_transition = PX4_ISFINITE(_airspeed_validated->calibrated_airspeed_m_s)
&& _param_fw_use_airspd.get();
const bool airspeed_triggers_transition = PX4_ISFINITE(_attc->get_calibrated_airspeed());
const bool minimum_trans_time_elapsed = _time_since_trans_start > getMinimumFrontTransitionTime();
const bool openloop_trans_time_elapsed = _time_since_trans_start > getOpenLoopFrontTransitionTime();
@ -184,7 +182,7 @@ bool VtolType::isFrontTransitionCompletedBase()
if (airspeed_triggers_transition) {
transition_to_fw = minimum_trans_time_elapsed
&& _airspeed_validated->calibrated_airspeed_m_s >= getTransitionAirspeed();
&& _attc->get_calibrated_airspeed() >= getTransitionAirspeed();
} else {
transition_to_fw = openloop_trans_time_elapsed;
@ -331,8 +329,8 @@ bool VtolType::isFrontTransitionTimeout()
// check front transition timeout
if (_common_vtol_mode == mode::TRANSITION_TO_FW) {
// when we use airspeed, we can timeout earlier if airspeed is not increasing fast enough
if (_param_fw_use_airspd.get() && _time_since_trans_start > getOpenLoopFrontTransitionTime()
&& _airspeed_validated->calibrated_airspeed_m_s < getBlendAirspeed()) {
if (PX4_ISFINITE(_attc->get_calibrated_airspeed()) && _time_since_trans_start > getOpenLoopFrontTransitionTime()
&& _attc->get_calibrated_airspeed() < getBlendAirspeed()) {
return true;
} else if (_time_since_trans_start > getFrontTransitionTimeout()) {

1
src/modules/vtol_att_control/vtol_type.h
View File

@ -338,7 +338,6 @@ protected:
(ParamFloat<px4::params::VT_ARSP_TRANS>) _param_vt_arsp_trans,
(ParamFloat<px4::params::VT_F_TRANS_THR>) _param_vt_f_trans_thr,
(ParamFloat<px4::params::VT_ARSP_BLEND>) _param_vt_arsp_blend,
(ParamBool<px4::params::FW_USE_AIRSPD>) _param_fw_use_airspd,
(ParamFloat<px4::params::VT_TRANS_TIMEOUT>) _param_vt_trans_timeout,
(ParamFloat<px4::params::MPC_XY_CRUISE>) _param_mpc_xy_cruise,
(ParamInt<px4::params::VT_FW_DIFTHR_EN>) _param_vt_fw_difthr_en,