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

sensors remove airspeed negative check

Browse Source
This commit is contained in:
Daniel Agar
2018-07-16 01:09:19 -04:00
parent 79f172eb92
commit 9a4de09325
4 changed files with 26 additions and 19 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/fw_att_control/FixedwingAttitudeControl.cpp
+1
View File
@@ -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 */ /* 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) 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); && (hrt_elapsed_time(&_airspeed_sub.get().timestamp) < 1e6);
if (!_parameters.airspeed_disabled && airspeed_valid) { if (!_parameters.airspeed_disabled && airspeed_valid) {
src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp
+14 -9
View File
@@ -372,16 +372,21 @@ FixedwingPositionControl::manual_control_setpoint_poll()
void void
FixedwingPositionControl::airspeed_poll() FixedwingPositionControl::airspeed_poll()
{ {
if (!_parameters.airspeed_disabled && _sub_airspeed.updated()) { if (!_parameters.airspeed_disabled && _sub_airspeed.update()) {
_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 (_sub_airspeed.get().indicated_airspeed_m_s > 0.0f const airspeed_s &airspeed = _sub_airspeed.get();
&& _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); _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 { } else {
_eas2tas = 1.0f; _eas2tas = 1.0f;
src/modules/sensors/sensors.cpp
+9 -10
View File
@@ -342,18 +342,17 @@ Sensors::diff_pres_poll(const vehicle_air_data_s &raw)
} }
/* don't risk to feed negative airspeed into the system */ /* don't risk to feed negative airspeed into the system */
airspeed.indicated_airspeed_m_s = math::max(0.0f, airspeed.indicated_airspeed_m_s = calc_indicated_airspeed_corrected((enum AIRSPEED_COMPENSATION_MODEL)
calc_indicated_airspeed_corrected((enum AIRSPEED_COMPENSATION_MODEL)_parameters.air_cmodel, _parameters.air_cmodel,
smodel, _parameters.air_tube_length, _parameters.air_tube_diameter_mm, smodel, _parameters.air_tube_length, _parameters.air_tube_diameter_mm,
diff_pres.differential_pressure_filtered_pa, raw.baro_pressure_pa, diff_pres.differential_pressure_filtered_pa, raw.baro_pressure_pa,
air_temperature_celsius)); air_temperature_celsius);
airspeed.true_airspeed_m_s = math::max(0.0f, airspeed.true_airspeed_m_s = calc_true_airspeed_from_indicated(airspeed.indicated_airspeed_m_s, raw.baro_pressure_pa,
calc_true_airspeed_from_indicated(airspeed.indicated_airspeed_m_s, raw.baro_pressure_pa, air_temperature_celsius)); air_temperature_celsius);
airspeed.true_airspeed_unfiltered_m_s = math::max(0.0f, airspeed.true_airspeed_unfiltered_m_s = calc_true_airspeed(diff_pres.differential_pressure_raw_pa +
calc_true_airspeed(diff_pres.differential_pressure_raw_pa + raw.baro_pressure_pa, raw.baro_pressure_pa, raw.baro_pressure_pa, raw.baro_pressure_pa, air_temperature_celsius);
air_temperature_celsius));
airspeed.air_temperature_celsius = air_temperature_celsius; airspeed.air_temperature_celsius = air_temperature_celsius;
src/modules/vtol_att_control/standard.cpp
+2
View File
@@ -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 // 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 && if (_airspeed_trans_blend_margin > 0.0f &&
_airspeed->indicated_airspeed_m_s > 0.0f &&
_airspeed->indicated_airspeed_m_s >= _params->airspeed_blend && _airspeed->indicated_airspeed_m_s >= _params->airspeed_blend &&
time_since_trans_start > _params->front_trans_time_min) { time_since_trans_start > _params->front_trans_time_min) {
mc_weight = 1.0f - fabsf(_airspeed->indicated_airspeed_m_s - _params->airspeed_blend) / mc_weight = 1.0f - fabsf(_airspeed->indicated_airspeed_m_s - _params->airspeed_blend) /
_airspeed_trans_blend_margin; _airspeed_trans_blend_margin;
// time based blending when no airspeed sensor is set // time based blending when no airspeed sensor is set