diff --git a/src/modules/vtol_att_control/standard.cpp b/src/modules/vtol_att_control/standard.cpp index 7c413d33b1..0291a9e164 100644 --- a/src/modules/vtol_att_control/standard.cpp +++ b/src/modules/vtol_att_control/standard.cpp @@ -179,7 +179,7 @@ void Standard::update_vtol_state() const bool airspeed_triggers_transition = PX4_ISFINITE(_airspeed_validated->calibrated_airspeed_m_s) && !_params->airspeed_disabled; - const bool minimum_trans_time_elapsed = time_since_trans_start > _params->front_trans_time_min; + const bool minimum_trans_time_elapsed = time_since_trans_start > getMinimumFrontTransitionTime(); bool transition_to_fw = false; @@ -261,14 +261,14 @@ void Standard::update_transition_state() PX4_ISFINITE(_airspeed_validated->calibrated_airspeed_m_s) && _airspeed_validated->calibrated_airspeed_m_s > 0.0f && _airspeed_validated->calibrated_airspeed_m_s >= _params->airspeed_blend && - time_since_trans_start > _params->front_trans_time_min) { + time_since_trans_start > getMinimumFrontTransitionTime()) { mc_weight = 1.0f - fabsf(_airspeed_validated->calibrated_airspeed_m_s - _params->airspeed_blend) / _airspeed_trans_blend_margin; // time based blending when no airspeed sensor is set } else if (_params->airspeed_disabled || !PX4_ISFINITE(_airspeed_validated->calibrated_airspeed_m_s)) { - mc_weight = 1.0f - time_since_trans_start / _params->front_trans_time_min; + mc_weight = 1.0f - time_since_trans_start / getMinimumFrontTransitionTime(); mc_weight = math::constrain(2.0f * mc_weight, 0.0f, 1.0f); } diff --git a/src/modules/vtol_att_control/tiltrotor.cpp b/src/modules/vtol_att_control/tiltrotor.cpp index fd4636c601..d4e2046e53 100644 --- a/src/modules/vtol_att_control/tiltrotor.cpp +++ b/src/modules/vtol_att_control/tiltrotor.cpp @@ -180,13 +180,13 @@ void Tiltrotor::update_vtol_state() bool transition_to_p2 = false; - if (time_since_trans_start > _params->front_trans_time_min) { + if (time_since_trans_start > getMinimumFrontTransitionTime()) { if (airspeed_triggers_transition) { transition_to_p2 = _airspeed_validated->calibrated_airspeed_m_s >= _params->transition_airspeed; } else { transition_to_p2 = _tilt_control >= _params_tiltrotor.tilt_transition && - time_since_trans_start > _params->front_trans_time_openloop;; + time_since_trans_start > getOpenLoopFrontTransitionTime(); } } @@ -359,9 +359,9 @@ void Tiltrotor::update_transition_state() // without airspeed do timed weight changes if ((_params->airspeed_disabled || !PX4_ISFINITE(_airspeed_validated->calibrated_airspeed_m_s)) && - time_since_trans_start > _params->front_trans_time_min) { - _mc_roll_weight = 1.0f - (time_since_trans_start - _params->front_trans_time_min) / - (_params->front_trans_time_openloop - _params->front_trans_time_min); + time_since_trans_start > getMinimumFrontTransitionTime()) { + _mc_roll_weight = 1.0f - (time_since_trans_start - getMinimumFrontTransitionTime()) / + (getOpenLoopFrontTransitionTime() - getMinimumFrontTransitionTime()); _mc_yaw_weight = _mc_roll_weight; } 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 99265c8d76..92b47b5e94 100644 --- a/src/modules/vtol_att_control/vtol_att_control_main.cpp +++ b/src/modules/vtol_att_control/vtol_att_control_main.cpp @@ -460,6 +460,12 @@ VtolAttitudeControl::Run() action_request_poll(); vehicle_cmd_poll(); + vehicle_air_data_s air_data; + + if (_vehicle_air_data_sub.update(&air_data)) { + _air_density = air_data.rho; + } + // check if mc and fw sp were updated bool mc_att_sp_updated = _mc_virtual_att_sp_sub.update(&_mc_virtual_att_sp); bool fw_att_sp_updated = _fw_virtual_att_sp_sub.update(&_fw_virtual_att_sp); 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 2bb060db5e..39e7a9322e 100644 --- a/src/modules/vtol_att_control/vtol_att_control_main.h +++ b/src/modules/vtol_att_control/vtol_att_control_main.h @@ -68,6 +68,7 @@ #include #include #include +#include #include #include #include @@ -125,6 +126,8 @@ public: bool get_immediate_transition() {return _immediate_transition;} void reset_immediate_transition() {_immediate_transition = false;} + float getAirDensity() const { return _air_density; } + struct actuator_controls_s *get_actuators_fw_in() {return &_actuators_fw_in;} struct actuator_controls_s *get_actuators_mc_in() {return &_actuators_mc_in;} struct actuator_controls_s *get_actuators_out0() {return &_actuators_out_0;} @@ -159,6 +162,7 @@ private: uORB::Subscription _action_request_sub{ORB_ID(action_request)}; uORB::Subscription _airspeed_validated_sub{ORB_ID(airspeed_validated)}; // airspeed subscription + uORB::Subscription _vehicle_air_data_sub{ORB_ID(vehicle_air_data)}; uORB::Subscription _fw_virtual_att_sp_sub{ORB_ID(fw_virtual_attitude_setpoint)}; uORB::Subscription _land_detected_sub{ORB_ID(vehicle_land_detected)}; uORB::Subscription _local_pos_sp_sub{ORB_ID(vehicle_local_position_setpoint)}; // setpoint subscription @@ -206,6 +210,8 @@ private: vehicle_local_position_setpoint_s _local_pos_sp{}; vtol_vehicle_status_s _vtol_vehicle_status{}; + float _air_density{CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C}; // [kg/m^3] + Params _params{}; // struct holding the parameters struct { diff --git a/src/modules/vtol_att_control/vtol_type.cpp b/src/modules/vtol_att_control/vtol_type.cpp index 3a8728ca82..bbf9bf1dba 100644 --- a/src/modules/vtol_att_control/vtol_type.cpp +++ b/src/modules/vtol_att_control/vtol_type.cpp @@ -648,3 +648,32 @@ float VtolType::pusher_assist() return forward_thrust; } + +float VtolType::getFrontTransitionTimeFactor() const +{ + // assumptions: transition_time = transition_true_airspeed / average_acceleration (thrust) + // transition_true_airspeed ~ sqrt(rho0 / rh0) + // average_acceleration ~ rho / rho0 + // transition_time ~ sqrt(rho0/rh0) * rho0 / rho + + // low value: hot day at 4000m AMSL with some margin + // high value: cold day at 0m AMSL with some margin + const float rho = math::constrain(_attc->getAirDensity(), 0.7f, 1.5f); + + if (PX4_ISFINITE(rho)) { + float rho0_over_rho = CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C / rho; + return sqrtf(rho0_over_rho) * rho0_over_rho; + } + + return 1.0f; +} + +float VtolType::getMinimumFrontTransitionTime() const +{ + return getFrontTransitionTimeFactor() * _params->front_trans_time_min; +} + +float VtolType::getOpenLoopFrontTransitionTime() const +{ + return getFrontTransitionTimeFactor() * _params->front_trans_time_openloop; +} diff --git a/src/modules/vtol_att_control/vtol_type.h b/src/modules/vtol_att_control/vtol_type.h index 7d7bc43aa9..1476a53d76 100644 --- a/src/modules/vtol_att_control/vtol_type.h +++ b/src/modules/vtol_att_control/vtol_type.h @@ -195,6 +195,16 @@ public: bool was_in_trans_mode() {return _flag_was_in_trans_mode;} + /** + * @return Minimum front transition time scaled for air density (if available) [s] + */ + float getMinimumFrontTransitionTime() const; + + /** + * @return Minimum open-loop front transition time scaled for air density (if available) [s] + */ + float getOpenLoopFrontTransitionTime() const; + virtual void parameters_update() = 0; VtolAttitudeControl *_attc; @@ -329,6 +339,11 @@ private: void stopBlendingThrottleAfterFrontTransition() { _throttle_blend_start_ts = 0; } + /** + * @return Transition time scale factor for density. + */ + float getFrontTransitionTimeFactor() const; + }; #endif