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

Merged master into driver_framework

Browse Source
This commit is contained in:
Lorenz Meier
2015-11-20 09:14:37 +01:00
parent c9c5f59bce 71320ac0fe
commit 75e962ee7c
102 changed files with 2985 additions and 474 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -116,7 +116,7 @@ static int check_calibration(DevHandle &h, const char* param_template, int &devi
return !calibration_found;
}
static bool magnometerCheck(int mavlink_fd, unsigned instance, bool optional, int &device_id)
static bool magnometerCheck(int mavlink_fd, unsigned instance, bool optional, int &device_id, bool report_fail)
{
bool success = true;
@@ -127,8 +127,10 @@ static bool magnometerCheck(int mavlink_fd, unsigned instance, bool optional, in
if (!h.isValid()) {
if (!optional) {
mavlink_and_console_log_critical(mavlink_fd,
if (report_fail) {
mavlink_and_console_log_critical(mavlink_fd,
"PREFLIGHT FAIL: NO MAG SENSOR #%u", instance);
}
}
return false;
@@ -137,8 +139,10 @@ static bool magnometerCheck(int mavlink_fd, unsigned instance, bool optional, in
int ret = check_calibration(h, "CAL_MAG%u_ID", device_id);
if (ret) {
mavlink_and_console_log_critical(mavlink_fd,
if (report_fail) {
mavlink_and_console_log_critical(mavlink_fd,
"PREFLIGHT FAIL: MAG #%u UNCALIBRATED", instance);
}
success = false;
goto out;
}
@@ -146,8 +150,10 @@ static bool magnometerCheck(int mavlink_fd, unsigned instance, bool optional, in
ret = h.ioctl(MAGIOCSELFTEST, 0);
if (ret != OK) {
mavlink_and_console_log_critical(mavlink_fd,
if (report_fail) {
mavlink_and_console_log_critical(mavlink_fd,
"PREFLIGHT FAIL: MAG #%u SELFTEST FAILED", instance);
}
success = false;
goto out;
}
@@ -157,7 +163,7 @@ out:
return success;
}
static bool accelerometerCheck(int mavlink_fd, unsigned instance, bool optional, bool dynamic, int &device_id)
static bool accelerometerCheck(int mavlink_fd, unsigned instance, bool optional, bool dynamic, int &device_id, bool report_fail)
{
bool success = true;
@@ -168,8 +174,10 @@ static bool accelerometerCheck(int mavlink_fd, unsigned instance, bool optional,
if (!h.isValid()) {
if (!optional) {
mavlink_and_console_log_critical(mavlink_fd,
if (report_fail) {
mavlink_and_console_log_critical(mavlink_fd,
"PREFLIGHT FAIL: NO ACCEL SENSOR #%u", instance);
}
}
return false;
@@ -178,8 +186,10 @@ static bool accelerometerCheck(int mavlink_fd, unsigned instance, bool optional,
int ret = check_calibration(h, "CAL_ACC%u_ID", device_id);
if (ret) {
mavlink_and_console_log_critical(mavlink_fd,
if (report_fail) {
mavlink_and_console_log_critical(mavlink_fd,
"PREFLIGHT FAIL: ACCEL #%u UNCALIBRATED", instance);
}
success = false;
goto out;
}
@@ -187,8 +197,10 @@ static bool accelerometerCheck(int mavlink_fd, unsigned instance, bool optional,
ret = h.ioctl(ACCELIOCSELFTEST, 0);
if (ret != OK) {
mavlink_and_console_log_critical(mavlink_fd,
if (report_fail) {
mavlink_and_console_log_critical(mavlink_fd,
"PREFLIGHT FAIL: ACCEL #%u SELFTEST FAILED", instance);
}
success = false;
goto out;
}
@@ -204,13 +216,17 @@ static bool accelerometerCheck(int mavlink_fd, unsigned instance, bool optional,
float accel_magnitude = sqrtf(acc.x * acc.x + acc.y * acc.y + acc.z * acc.z);
if (accel_magnitude < 4.0f || accel_magnitude > 15.0f /* m/s^2 */) {
mavlink_and_console_log_critical(mavlink_fd, "PREFLIGHT FAIL: ACCEL RANGE, hold still on arming");
if (report_fail) {
mavlink_and_console_log_critical(mavlink_fd, "PREFLIGHT FAIL: ACCEL RANGE, hold still on arming");
}
/* this is frickin' fatal */
success = false;
goto out;
}
} else {
mavlink_log_critical(mavlink_fd, "PREFLIGHT FAIL: ACCEL READ");
if (report_fail) {
mavlink_and_console_log_critical(mavlink_fd, "PREFLIGHT FAIL: ACCEL READ");
}
/* this is frickin' fatal */
success = false;
goto out;
@@ -223,7 +239,7 @@ out:
return success;
}
static bool gyroCheck(int mavlink_fd, unsigned instance, bool optional, int &device_id)
static bool gyroCheck(int mavlink_fd, unsigned instance, bool optional, int &device_id, bool report_fail)
{
bool success = true;
@@ -234,8 +250,10 @@ static bool gyroCheck(int mavlink_fd, unsigned instance, bool optional, int &dev
if (!h.isValid()) {
if (!optional) {
mavlink_and_console_log_critical(mavlink_fd,
if (report_fail) {
mavlink_and_console_log_critical(mavlink_fd,
"PREFLIGHT FAIL: NO GYRO SENSOR #%u", instance);
}
}
return false;
@@ -244,8 +262,10 @@ static bool gyroCheck(int mavlink_fd, unsigned instance, bool optional, int &dev
int ret = check_calibration(h, "CAL_GYRO%u_ID", device_id);
if (ret) {
mavlink_and_console_log_critical(mavlink_fd,
if (report_fail) {
mavlink_and_console_log_critical(mavlink_fd,
"PREFLIGHT FAIL: GYRO #%u UNCALIBRATED", instance);
}
success = false;
goto out;
}
@@ -253,8 +273,10 @@ static bool gyroCheck(int mavlink_fd, unsigned instance, bool optional, int &dev
ret = h.ioctl(GYROIOCSELFTEST, 0);
if (ret != OK) {
mavlink_and_console_log_critical(mavlink_fd,
if (report_fail) {
mavlink_and_console_log_critical(mavlink_fd,
"PREFLIGHT FAIL: GYRO #%u SELFTEST FAILED", instance);
}
success = false;
goto out;
}
@@ -264,7 +286,7 @@ out:
return success;
}
static bool baroCheck(int mavlink_fd, unsigned instance, bool optional, int &device_id)
static bool baroCheck(int mavlink_fd, unsigned instance, bool optional, int &device_id, bool report_fail)
{
bool success = true;
@@ -275,8 +297,10 @@ static bool baroCheck(int mavlink_fd, unsigned instance, bool optional, int &dev
if (!h.isValid()) {
if (!optional) {
mavlink_and_console_log_critical(mavlink_fd,
if (report_fail) {
mavlink_and_console_log_critical(mavlink_fd,
"PREFLIGHT FAIL: NO BARO SENSOR #%u", instance);
}
}
return false;
@@ -300,7 +324,7 @@ static bool baroCheck(int mavlink_fd, unsigned instance, bool optional, int &dev
return success;
}
static bool airspeedCheck(int mavlink_fd, bool optional)
static bool airspeedCheck(int mavlink_fd, bool optional, bool report_fail)
{
bool success = true;
int ret;
@@ -310,13 +334,17 @@ static bool airspeedCheck(int mavlink_fd, bool optional)
if ((ret = orb_copy(ORB_ID(airspeed), fd, &airspeed)) ||
(hrt_elapsed_time(&airspeed.timestamp) > (500 * 1000))) {
mavlink_and_console_log_critical(mavlink_fd, "PREFLIGHT FAIL: AIRSPEED SENSOR MISSING");
if (report_fail) {
mavlink_and_console_log_critical(mavlink_fd, "PREFLIGHT FAIL: AIRSPEED SENSOR MISSING");
}
success = false;
goto out;
}
if (fabsf(airspeed.indicated_airspeed_m_s) > 6.0f) {
mavlink_and_console_log_critical(mavlink_fd, "AIRSPEED WARNING: WIND OR CALIBRATION ISSUE");
if (report_fail) {
mavlink_and_console_log_critical(mavlink_fd, "AIRSPEED WARNING: WIND OR CALIBRATION ISSUE");
}
// XXX do not make this fatal yet
}
@@ -325,7 +353,7 @@ out:
return success;
}
static bool gnssCheck(int mavlink_fd)
static bool gnssCheck(int mavlink_fd, bool report_fail)
{
bool success = true;
@@ -347,8 +375,10 @@ static bool gnssCheck(int mavlink_fd)
}
//Report failure to detect module
if(!success) {
mavlink_and_console_log_critical(mavlink_fd, "PREFLIGHT FAIL: GPS RECEIVER MISSING");
if (!success) {
if (report_fail) {
mavlink_and_console_log_critical(mavlink_fd, "PREFLIGHT FAIL: GPS RECEIVER MISSING");
}
}
px4_close(gpsSub);
@@ -356,7 +386,7 @@ static bool gnssCheck(int mavlink_fd)
}
bool preflightCheck(int mavlink_fd, bool checkMag, bool checkAcc, bool checkGyro,
bool checkBaro, bool checkAirspeed, bool checkRC, bool checkGNSS, bool checkDynamic)
bool checkBaro, bool checkAirspeed, bool checkRC, bool checkGNSS, bool checkDynamic, bool reportFailures)
{
bool failed = false;
@@ -371,7 +401,7 @@ bool preflightCheck(int mavlink_fd, bool checkMag, bool checkAcc, bool checkGyro
bool required = (i < max_mandatory_mag_count);
int device_id = -1;
if (!magnometerCheck(mavlink_fd, i, !required, device_id) && required) {
if (!magnometerCheck(mavlink_fd, i, !required, device_id, reportFailures) && required) {
failed = true;
}
@@ -382,7 +412,9 @@ bool preflightCheck(int mavlink_fd, bool checkMag, bool checkAcc, bool checkGyro
/* check if the primary device is present */
if (!prime_found && prime_id != 0) {
mavlink_log_critical(mavlink_fd, "warning: primary compass not operational");
if (reportFailures) {
mavlink_and_console_log_critical(mavlink_fd, "Warning: Primary compass not found");
}
failed = true;
}
}
@@ -398,7 +430,7 @@ bool preflightCheck(int mavlink_fd, bool checkMag, bool checkAcc, bool checkGyro
bool required = (i < max_mandatory_accel_count);
int device_id = -1;
if (!accelerometerCheck(mavlink_fd, i, !required, checkDynamic, device_id) && required) {
if (!accelerometerCheck(mavlink_fd, i, !required, checkDynamic, device_id, reportFailures) && required) {
failed = true;
}
@@ -409,7 +441,9 @@ bool preflightCheck(int mavlink_fd, bool checkMag, bool checkAcc, bool checkGyro
/* check if the primary device is present */
if (!prime_found && prime_id != 0) {
mavlink_log_critical(mavlink_fd, "warning: primary accelerometer not operational");
if (reportFailures) {
mavlink_and_console_log_critical(mavlink_fd, "Warning: Primary accelerometer not found");
}
failed = true;
}
}
@@ -425,7 +459,7 @@ bool preflightCheck(int mavlink_fd, bool checkMag, bool checkAcc, bool checkGyro
bool required = (i < max_mandatory_gyro_count);
int device_id = -1;
if (!gyroCheck(mavlink_fd, i, !required, device_id) && required) {
if (!gyroCheck(mavlink_fd, i, !required, device_id, reportFailures) && required) {
failed = true;
}
@@ -436,7 +470,9 @@ bool preflightCheck(int mavlink_fd, bool checkMag, bool checkAcc, bool checkGyro
/* check if the primary device is present */
if (!prime_found && prime_id != 0) {
mavlink_log_critical(mavlink_fd, "warning: primary gyro not operational");
if (reportFailures) {
mavlink_and_console_log_critical(mavlink_fd, "Warning: Primary gyro not found");
}
failed = true;
}
}
@@ -452,7 +488,7 @@ bool preflightCheck(int mavlink_fd, bool checkMag, bool checkAcc, bool checkGyro
bool required = (i < max_mandatory_baro_count);
int device_id = -1;
if (!baroCheck(mavlink_fd, i, !required, device_id) && required) {
if (!baroCheck(mavlink_fd, i, !required, device_id, reportFailures) && required) {
failed = true;
}
@@ -464,29 +500,33 @@ bool preflightCheck(int mavlink_fd, bool checkMag, bool checkAcc, bool checkGyro
// TODO there is no logic in place to calibrate the primary baro yet
// // check if the primary device is present
if (!prime_found && prime_id != 0) {
mavlink_log_critical(mavlink_fd, "warning: primary barometer not operational");
if (reportFailures) {
mavlink_and_console_log_critical(mavlink_fd, "warning: primary barometer not operational");
}
failed = true;
}
}
/* ---- AIRSPEED ---- */
if (checkAirspeed) {
if (!airspeedCheck(mavlink_fd, true)) {
if (!airspeedCheck(mavlink_fd, true, reportFailures)) {
failed = true;
}
}
/* ---- RC CALIBRATION ---- */
if (checkRC) {
if (rc_calibration_check(mavlink_fd) != OK) {
mavlink_log_critical(mavlink_fd, "RC calibration check failed");
if (rc_calibration_check(mavlink_fd, reportFailures) != OK) {
if (reportFailures) {
mavlink_and_console_log_critical(mavlink_fd, "RC calibration check failed");
}
failed = true;
}
}
/* ---- Global Navigation Satellite System receiver ---- */
if (checkGNSS) {
if(!gnssCheck(mavlink_fd)) {
if (!gnssCheck(mavlink_fd, reportFailures)) {
failed = true;
}
}