yemai/bizhang_-obav
1
0
Fork 0
mirror of https://gitee.com/xiaohuolufeihua/bizhang_-obav.git synced 2026-05-22 09:28:58 +00:00
Code Issues Packages Projects Releases Wiki Activity

Airspeed: preflight check for bad offset, fixed calls to preflight checks (vtol & airspeed)

Browse Source
This commit is contained in:
Andreas Antener
2016-11-14 00:15:20 +01:00
committed by Lorenz Meier
parent f092b31f54
commit f772fc2d02
8 changed files with 72 additions and 43 deletions
Download Patch File Download Diff File Expand all files Collapse all files

26
src/modules/commander/PreflightCheck.cpp
View File

@@ -363,7 +363,7 @@ 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)
static bool airspeedCheck(orb_advert_t *mavlink_log_pub, bool optional, bool report_fail, bool prearm, hrt_abstime time_since_boot)
{
bool success = true;
int ret;
@@ -388,11 +388,24 @@ static bool airspeedCheck(orb_advert_t *mavlink_log_pub, bool optional, bool rep
goto out;
}
if (fabsf(airspeed.indicated_airspeed_m_s) > 6.0f) {
/**
* Check if differential pressure is off by more than 15Pa which equals to 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 (report_fail) {
mavlink_log_critical(mavlink_log_pub, "AIRSPEED WARNING: WIND OR CALIBRATION ISSUE");
mavlink_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: AIRSPEED CALIBRATION ISSUE");
}
// XXX do not make this fatal yet
success = false;
goto out;
}
out:
@@ -511,7 +524,8 @@ out:
}
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 checkBaro, bool checkAirspeed, bool checkRC, bool checkGNSS,
bool checkDynamic, bool isVTOL, bool reportFailures, bool prearm, hrt_abstime time_since_boot)
{
#ifdef __PX4_QURT
@@ -651,7 +665,7 @@ bool preflightCheck(orb_advert_t *mavlink_log_pub, bool checkMag, bool checkAcc,
/* ---- AIRSPEED ---- */
if (checkAirspeed) {
if (!airspeedCheck(mavlink_log_pub, true, reportFailures)) {
if (!airspeedCheck(mavlink_log_pub, true, reportFailures, prearm, time_since_boot)) {
failed = true;
}
}