diff --git a/src/modules/fw_att_control/FixedwingAttitudeControl.cpp b/src/modules/fw_att_control/FixedwingAttitudeControl.cpp index 352ea501ce..2511eef767 100644 --- a/src/modules/fw_att_control/FixedwingAttitudeControl.cpp +++ b/src/modules/fw_att_control/FixedwingAttitudeControl.cpp @@ -565,6 +565,7 @@ void FixedwingAttitudeControl::run() /* if airspeed is non-finite or not valid or if we are asked not to control it, we assume the normal average speed */ const bool airspeed_valid = PX4_ISFINITE(_airspeed_sub.get().indicated_airspeed_m_s) + && (_airspeed_sub.get().indicated_airspeed_m_s > 0.0f) && (hrt_elapsed_time(&_airspeed_sub.get().timestamp) < 1e6); if (!_parameters.airspeed_disabled && airspeed_valid) { diff --git a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp index 4aeace4805..2389fafdcf 100644 --- a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp +++ b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp @@ -372,16 +372,21 @@ FixedwingPositionControl::manual_control_setpoint_poll() void FixedwingPositionControl::airspeed_poll() { - if (!_parameters.airspeed_disabled && _sub_airspeed.updated()) { - _sub_airspeed.update(); - _airspeed_valid = PX4_ISFINITE(_sub_airspeed.get().indicated_airspeed_m_s) - && PX4_ISFINITE(_sub_airspeed.get().true_airspeed_m_s); - _airspeed_last_received = hrt_absolute_time(); - _airspeed = _sub_airspeed.get().indicated_airspeed_m_s; + if (!_parameters.airspeed_disabled && _sub_airspeed.update()) { - if (_sub_airspeed.get().indicated_airspeed_m_s > 0.0f - && _sub_airspeed.get().true_airspeed_m_s > _sub_airspeed.get().indicated_airspeed_m_s) { - _eas2tas = max(_sub_airspeed.get().true_airspeed_m_s / _sub_airspeed.get().indicated_airspeed_m_s, 1.0f); + const airspeed_s &airspeed = _sub_airspeed.get(); + + _airspeed_valid = PX4_ISFINITE(airspeed.indicated_airspeed_m_s) + && PX4_ISFINITE(airspeed.true_airspeed_m_s) + && (airspeed.indicated_airspeed_m_s > 0.0f); + + _airspeed_last_received = hrt_absolute_time(); + _airspeed = airspeed.indicated_airspeed_m_s; + + if (_airspeed_valid + && airspeed.true_airspeed_m_s > airspeed.indicated_airspeed_m_s) { + + _eas2tas = max(airspeed.true_airspeed_m_s / airspeed.indicated_airspeed_m_s, 1.0f); } else { _eas2tas = 1.0f; diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index e561acc01d..aa42e34b93 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -342,18 +342,17 @@ Sensors::diff_pres_poll(const vehicle_air_data_s &raw) } /* don't risk to feed negative airspeed into the system */ - airspeed.indicated_airspeed_m_s = math::max(0.0f, - calc_indicated_airspeed_corrected((enum AIRSPEED_COMPENSATION_MODEL)_parameters.air_cmodel, - smodel, _parameters.air_tube_length, _parameters.air_tube_diameter_mm, - diff_pres.differential_pressure_filtered_pa, raw.baro_pressure_pa, - air_temperature_celsius)); + airspeed.indicated_airspeed_m_s = calc_indicated_airspeed_corrected((enum AIRSPEED_COMPENSATION_MODEL) + _parameters.air_cmodel, + smodel, _parameters.air_tube_length, _parameters.air_tube_diameter_mm, + diff_pres.differential_pressure_filtered_pa, raw.baro_pressure_pa, + air_temperature_celsius); - airspeed.true_airspeed_m_s = math::max(0.0f, - calc_true_airspeed_from_indicated(airspeed.indicated_airspeed_m_s, raw.baro_pressure_pa, air_temperature_celsius)); + airspeed.true_airspeed_m_s = calc_true_airspeed_from_indicated(airspeed.indicated_airspeed_m_s, raw.baro_pressure_pa, + air_temperature_celsius); - airspeed.true_airspeed_unfiltered_m_s = math::max(0.0f, - calc_true_airspeed(diff_pres.differential_pressure_raw_pa + raw.baro_pressure_pa, raw.baro_pressure_pa, - air_temperature_celsius)); + airspeed.true_airspeed_unfiltered_m_s = calc_true_airspeed(diff_pres.differential_pressure_raw_pa + + raw.baro_pressure_pa, raw.baro_pressure_pa, air_temperature_celsius); airspeed.air_temperature_celsius = air_temperature_celsius; diff --git a/src/modules/vtol_att_control/standard.cpp b/src/modules/vtol_att_control/standard.cpp index 8ad2093b13..506f810f40 100644 --- a/src/modules/vtol_att_control/standard.cpp +++ b/src/modules/vtol_att_control/standard.cpp @@ -245,8 +245,10 @@ 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 && + _airspeed->indicated_airspeed_m_s > 0.0f && _airspeed->indicated_airspeed_m_s >= _params->airspeed_blend && time_since_trans_start > _params->front_trans_time_min) { + mc_weight = 1.0f - fabsf(_airspeed->indicated_airspeed_m_s - _params->airspeed_blend) / _airspeed_trans_blend_margin; // time based blending when no airspeed sensor is set