|
|
|
@ -385,20 +385,6 @@ static bool airspeedCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &statu
@@ -385,20 +385,6 @@ static bool airspeedCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &statu
|
|
|
|
|
int fd_airspeed = orb_subscribe(ORB_ID(airspeed)); |
|
|
|
|
airspeed_s airspeed = {}; |
|
|
|
|
|
|
|
|
|
int fd_diffpres = orb_subscribe(ORB_ID(differential_pressure)); |
|
|
|
|
differential_pressure_s differential_pressure = {}; |
|
|
|
|
|
|
|
|
|
if ((orb_copy(ORB_ID(differential_pressure), fd_diffpres, &differential_pressure) != PX4_OK) || |
|
|
|
|
(hrt_elapsed_time(&differential_pressure.timestamp) > 1_s)) { |
|
|
|
|
if (report_fail && !optional) { |
|
|
|
|
mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Airspeed Sensor missing"); |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
present = false; |
|
|
|
|
success = false; |
|
|
|
|
goto out; |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
if ((orb_copy(ORB_ID(airspeed), fd_airspeed, &airspeed) != PX4_OK) || |
|
|
|
|
(hrt_elapsed_time(&airspeed.timestamp) > 1_s)) { |
|
|
|
|
if (report_fail && !optional) { |
|
|
|
@ -427,11 +413,11 @@ static bool airspeedCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &statu
@@ -427,11 +413,11 @@ static bool airspeedCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &statu
|
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
/**
|
|
|
|
|
* Check if differential pressure is off by more than 15Pa which equals ~5m/s when measuring no airspeed. |
|
|
|
|
* Check if airspeed is higher than 4m/s (accepted max) while the vehicle is landed / not flying |
|
|
|
|
* Negative and positive offsets are considered. Do not check anymore while arming because pitot cover |
|
|
|
|
* might have been removed. |
|
|
|
|
*/ |
|
|
|
|
if (fabsf(differential_pressure.differential_pressure_filtered_pa) > 15.0f && !prearm) { |
|
|
|
|
if (fabsf(airspeed.indicated_airspeed_m_s) > 4.0f && !prearm) { |
|
|
|
|
if (report_fail) { |
|
|
|
|
mavlink_log_critical(mavlink_log_pub, "Preflight Fail: check Airspeed Cal or Pitot"); |
|
|
|
|
} |
|
|
|
@ -445,7 +431,6 @@ out:
@@ -445,7 +431,6 @@ out:
|
|
|
|
|
set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_DIFFPRESSURE, present, !optional, success, status); |
|
|
|
|
|
|
|
|
|
orb_unsubscribe(fd_airspeed); |
|
|
|
|
orb_unsubscribe(fd_diffpres); |
|
|
|
|
|
|
|
|
|
return success; |
|
|
|
|
} |
|
|
|
|