From 6d12b04bb0576a9241de6dc75df3b44c707c68ca Mon Sep 17 00:00:00 2001 From: Silvan Date: Fri, 4 Apr 2025 16:57:40 +0200 Subject: [PATCH] VTOL: do central handling of airspeed measurement - 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 --- src/modules/vtol_att_control/standard.cpp | 15 +++++++-------- src/modules/vtol_att_control/tailsitter.cpp | 5 ++--- src/modules/vtol_att_control/tiltrotor.cpp | 12 ++++++------ .../vtol_att_control/vtol_att_control_main.cpp | 18 +++++++++++++++++- .../vtol_att_control/vtol_att_control_main.h | 9 ++++++--- src/modules/vtol_att_control/vtol_type.cpp | 10 ++++------ src/modules/vtol_att_control/vtol_type.h | 1 - 7 files changed, 42 insertions(+), 28 deletions(-) diff --git a/src/modules/vtol_att_control/standard.cpp b/src/modules/vtol_att_control/standard.cpp index 3681c2b17c..5ab114ba16 100644 --- a/src/modules/vtol_att_control/standard.cpp +++ b/src/modules/vtol_att_control/standard.cpp @@ -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 diff --git a/src/modules/vtol_att_control/tailsitter.cpp b/src/modules/vtol_att_control/tailsitter.cpp index eaacbe68d6..8b159d2481 100644 --- a/src/modules/vtol_att_control/tailsitter.cpp +++ b/src/modules/vtol_att_control/tailsitter.cpp @@ -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; diff --git a/src/modules/vtol_att_control/tiltrotor.cpp b/src/modules/vtol_att_control/tiltrotor.cpp index 7fb52b4688..1cc1e19088 100644 --- a/src/modules/vtol_att_control/tiltrotor.cpp +++ b/src/modules/vtol_att_control/tiltrotor.cpp @@ -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()); diff --git a/src/modules/vtol_att_control/vtol_att_control_main.cpp b/src/modules/vtol_att_control/vtol_att_control_main.cpp index de0ce8f78c..353d236ce6 100644 --- a/src/modules/vtol_att_control/vtol_att_control_main.cpp +++ b/src/modules/vtol_att_control/vtol_att_control_main.cpp @@ -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(); diff --git a/src/modules/vtol_att_control/vtol_att_control_main.h b/src/modules/vtol_att_control/vtol_att_control_main.h index ad08d23b54..6040a2e71d 100644 --- a/src/modules/vtol_att_control/vtol_att_control_main.h +++ b/src/modules/vtol_att_control/vtol_att_control_main.h @@ -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) _param_vt_type, - (ParamFloat) _param_vt_spoiler_mc_ld + (ParamFloat) _param_vt_spoiler_mc_ld, + (ParamBool) _param_fw_use_airspd ) }; diff --git a/src/modules/vtol_att_control/vtol_type.cpp b/src/modules/vtol_att_control/vtol_type.cpp index 7eb77e89d5..def70ece07 100644 --- a/src/modules/vtol_att_control/vtol_type.cpp +++ b/src/modules/vtol_att_control/vtol_type.cpp @@ -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()) { diff --git a/src/modules/vtol_att_control/vtol_type.h b/src/modules/vtol_att_control/vtol_type.h index 592779ecfd..69d1bed6f4 100644 --- a/src/modules/vtol_att_control/vtol_type.h +++ b/src/modules/vtol_att_control/vtol_type.h @@ -338,7 +338,6 @@ protected: (ParamFloat) _param_vt_arsp_trans, (ParamFloat) _param_vt_f_trans_thr, (ParamFloat) _param_vt_arsp_blend, - (ParamBool) _param_fw_use_airspd, (ParamFloat) _param_vt_trans_timeout, (ParamFloat) _param_mpc_xy_cruise, (ParamInt) _param_vt_fw_difthr_en,