diff --git a/msg/airspeed.msg b/msg/airspeed.msg index b0ca817043..744f9d8830 100644 --- a/msg/airspeed.msg +++ b/msg/airspeed.msg @@ -3,4 +3,3 @@ float32 true_airspeed_m_s # true filtered airspeed in meters per second, -1 if float32 true_airspeed_unfiltered_m_s # true airspeed in meters per second, -1 if unknown float32 air_temperature_celsius # air temperature in degrees celsius, -1000 if unknown float32 confidence # confidence value from 0 to 1 for this sensor -float32 differential_pressure_filtered_pa # filtered differential pressure, can be negative diff --git a/src/modules/commander/PreflightCheck.cpp b/src/modules/commander/PreflightCheck.cpp index 97824ca3f9..7a28d45f4e 100644 --- a/src/modules/commander/PreflightCheck.cpp +++ b/src/modules/commander/PreflightCheck.cpp @@ -64,9 +64,10 @@ #include #include -#include +#include #include #include +#include #include "PreflightCheck.h" @@ -366,15 +367,27 @@ static bool baroCheck(orb_advert_t *mavlink_log_pub, unsigned instance, bool opt return success; } -static bool airspeedCheck(orb_advert_t *mavlink_log_pub, bool optional, bool report_fail, bool prearm, hrt_abstime time_since_boot) +static bool airspeedCheck(orb_advert_t *mavlink_log_pub, bool optional, bool report_fail, bool prearm) { bool success = true; int ret; - int fd = orb_subscribe(ORB_ID(airspeed)); + int fd_airspeed = orb_subscribe(ORB_ID(airspeed)); + int fd_diffpres = orb_subscribe(ORB_ID(differential_pressure)); + + struct differential_pressure_s differential_pressure; + + if ((ret = orb_copy(ORB_ID(differential_pressure), fd_diffpres, &differential_pressure)) || + (hrt_elapsed_time(&differential_pressure.timestamp) > (500 * 1000))) { + if (report_fail) { + mavlink_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: AIRSPEED SENSOR MISSING"); + } + success = false; + goto out; + } struct airspeed_s airspeed; - if ((ret = orb_copy(ORB_ID(airspeed), fd, &airspeed)) || + if ((ret = orb_copy(ORB_ID(airspeed), fd_airspeed, &airspeed)) || (hrt_elapsed_time(&airspeed.timestamp) > (500 * 1000))) { if (report_fail) { mavlink_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: AIRSPEED SENSOR MISSING"); @@ -398,27 +411,23 @@ static bool airspeedCheck(orb_advert_t *mavlink_log_pub, bool optional, bool rep } /** - * Check if differential pressure is off by more than 15Pa which equals to 5m/s when measuring no airspeed. + * Check if differential pressure is off by more than 15Pa which equals ~5m/s when measuring no airspeed. * Negative and positive offsets are considered. Do not check anymore while arming because pitot cover * might have been removed. */ - if (time_since_boot < 1e6) { - // the airspeed driver filter doesn't deliver the actual value yet - success = false; - goto out; - } - if (fabsf(airspeed.differential_pressure_filtered_pa) > 15.0f && !prearm) { + if (fabsf(differential_pressure.differential_pressure_filtered_pa) > 15.0f && !prearm) { if (report_fail) { - mavlink_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: AIRSPEED CALIBRATION BAD OR PITOT UNCOVERED"); + mavlink_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: CHECK AIRSPEED CAL OR PITOT"); } success = false; goto out; } out: - orb_unsubscribe(fd); + orb_unsubscribe(fd_airspeed); + orb_unsubscribe(fd_diffpres); return success; } @@ -561,6 +570,11 @@ bool preflightCheck(orb_advert_t *mavlink_log_pub, bool checkMag, bool checkAcc, bool checkDynamic, bool isVTOL, bool reportFailures, bool prearm, hrt_abstime time_since_boot) { + if (time_since_boot < 1e6) { + // the airspeed driver filter doesn't deliver the actual value yet + return true; + } + #ifdef __PX4_QURT // WARNING: Preflight checks are important and should be added back when // all the sensors are supported @@ -706,7 +720,7 @@ bool preflightCheck(orb_advert_t *mavlink_log_pub, bool checkMag, bool checkAcc, /* ---- AIRSPEED ---- */ if (checkAirspeed) { - if (!airspeedCheck(mavlink_log_pub, true, reportFailures, prearm, time_since_boot)) { + if (!airspeedCheck(mavlink_log_pub, true, reportFailures, prearm)) { failed = true; } } diff --git a/src/modules/commander/PreflightCheck.h b/src/modules/commander/PreflightCheck.h index de8d479813..8fda560fd7 100644 --- a/src/modules/commander/PreflightCheck.h +++ b/src/modules/commander/PreflightCheck.h @@ -72,16 +72,16 @@ bool preflightCheck(orb_advert_t *mavlink_log_pub, bool checkMag, bool checkAcc, bool checkGyro, bool checkBaro, bool checkAirspeed, bool checkRC, bool checkGNSS, bool checkDynamic, bool isVTOL, bool reportFailures, bool prearm, hrt_abstime time_since_boot); -const unsigned max_mandatory_gyro_count = 1; -const unsigned max_optional_gyro_count = 3; +static constexpr unsigned max_mandatory_gyro_count = 1; +static constexpr unsigned max_optional_gyro_count = 3; -const unsigned max_mandatory_accel_count = 1; -const unsigned max_optional_accel_count = 3; +static constexpr unsigned max_mandatory_accel_count = 1; +static constexpr unsigned max_optional_accel_count = 3; -const unsigned max_mandatory_mag_count = 1; -const unsigned max_optional_mag_count = 4; +static constexpr unsigned max_mandatory_mag_count = 1; +static constexpr unsigned max_optional_mag_count = 4; -const unsigned max_mandatory_baro_count = 1; -const unsigned max_optional_baro_count = 1; +static constexpr unsigned max_mandatory_baro_count = 1; +static constexpr unsigned max_optional_baro_count = 1; } diff --git a/src/modules/commander/airspeed_calibration.cpp b/src/modules/commander/airspeed_calibration.cpp index 01c5838c4b..8ed0e8e5e4 100644 --- a/src/modules/commander/airspeed_calibration.cpp +++ b/src/modules/commander/airspeed_calibration.cpp @@ -167,7 +167,7 @@ int do_airspeed_calibration(orb_advert_t *mavlink_log_pub) if (PX4_ISFINITE(diff_pres_offset)) { - int fd_scale = px4_open(AIRSPEED0_DEVICE_PATH, 0); + int fd_scale = px4_open(AIRSPEED0_DEVICE_PATH, 0); airscale.offset_pa = diff_pres_offset; if (fd_scale > 0) { if (PX4_OK != px4_ioctl(fd_scale, AIRSPEEDIOCSSCALE, (long unsigned int)&airscale)) { diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 8577652969..2ac72126bb 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -395,7 +395,6 @@ Sensors::diff_pres_poll(struct sensor_combined_s &raw) _voted_sensors_update.baro_pressure(), air_temperature_celsius)); _airspeed.air_temperature_celsius = air_temperature_celsius; - _airspeed.differential_pressure_filtered_pa = _diff_pres.differential_pressure_filtered_pa; int instance; orb_publish_auto(ORB_ID(airspeed), &_airspeed_pub, &_airspeed, &instance, ORB_PRIO_DEFAULT);