fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-16 03:13:33 +08:00
Code Issues Packages Projects Releases Wiki Activity

Commander: Fix preflight check reporting and simplify logic

Browse Source
This commit is contained in:
Lorenz Meier
2015-11-19 18:28:16 +01:00
parent f2b988dcaa
commit 7a5391a723
3 changed files with 33 additions and 14 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/commander/PreflightCheck.cpp
+22 -8
View File
@@ -124,8 +124,10 @@ static bool magnometerCheck(int mavlink_fd, unsigned instance, bool optional, in
if (fd < 0) {
if (!optional) {
if(report_fail) 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;
@@ -134,8 +136,10 @@ static bool magnometerCheck(int mavlink_fd, unsigned instance, bool optional, in
int ret = check_calibration(fd, "CAL_MAG%u_ID", device_id);
if (ret) {
if(report_fail) 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;
}
@@ -143,8 +147,10 @@ static bool magnometerCheck(int mavlink_fd, unsigned instance, bool optional, in
ret = px4_ioctl(fd, MAGIOCSELFTEST, 0);
if (ret != OK) {
if(report_fail) 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;
}
@@ -164,8 +170,10 @@ static bool accelerometerCheck(int mavlink_fd, unsigned instance, bool optional,
if (fd < 0) {
if (!optional) {
if(report_fail) 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;
@@ -174,8 +182,10 @@ static bool accelerometerCheck(int mavlink_fd, unsigned instance, bool optional,
int ret = check_calibration(fd, "CAL_ACC%u_ID", device_id);
if (ret) {
if(report_fail) 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;
}
@@ -183,8 +193,10 @@ static bool accelerometerCheck(int mavlink_fd, unsigned instance, bool optional,
ret = px4_ioctl(fd, ACCELIOCSELFTEST, 0);
if (ret != OK) {
if(report_fail) 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;
}
@@ -200,7 +212,9 @@ 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 */) {
if(report_fail) 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;
@@ -506,7 +520,7 @@ bool preflightCheck(int mavlink_fd, bool checkMag, bool checkAcc, bool checkGyro
/* ---- Global Navigation Satellite System receiver ---- */
if (checkGNSS) {
if(!gnssCheck(mavlink_fd, reportFailures)) {
if (!gnssCheck(mavlink_fd, reportFailures)) {
failed = true;
}
}
src/modules/commander/commander.cpp
+5 -3
View File
@@ -1034,7 +1034,7 @@ int commander_thread_main(int argc, char *argv[])
// XXX for now just set sensors as initialized
status.condition_system_sensors_initialized = true;
status.condition_system_prearm_error_reported = true;
status.condition_system_prearm_error_reported = false;
status.condition_system_hotplug_timeout = false;
status.counter++;
@@ -2197,10 +2197,12 @@ int commander_thread_main(int argc, char *argv[])
mavlink_and_console_log_info(mavlink_fd, "data link #%i regained", i);
} else if (telemetry_last_dl_loss[i] == 0) {
/* new link */
status.condition_system_prearm_error_reported = false;
status_changed = true;
}
/* got link again or new */
status.condition_system_prearm_error_reported = false;
status_changed = true;
telemetry_lost[i] = false;
have_link = true;
src/modules/commander/state_machine_helper.cpp
+6 -3
View File
@@ -271,6 +271,7 @@ arming_state_transition(struct vehicle_status_s *status, ///< current vehicle s
/* reset feedback state */
if (status->arming_state != vehicle_status_s::ARMING_STATE_STANDBY_ERROR &&
status->arming_state != vehicle_status_s::ARMING_STATE_INIT &&
valid_transition) {
status->condition_system_prearm_error_reported = false;
}
@@ -745,8 +746,8 @@ int preflight_check(struct vehicle_status_s *status, const int mavlink_fd, bool
{
/*
*/
bool reportFailures = force_report || (!status->data_link_lost &&
!status->condition_system_prearm_error_reported && status->condition_system_hotplug_timeout);
bool reportFailures = force_report || (!status->condition_system_prearm_error_reported &&
status->condition_system_hotplug_timeout);
bool checkAirspeed = false;
/* Perform airspeed check only if circuit breaker is not
@@ -755,7 +756,9 @@ int preflight_check(struct vehicle_status_s *status, const int mavlink_fd, bool
checkAirspeed = true;
}
bool preflight_ok = Commander::preflightCheck(mavlink_fd, true, true, true, true, checkAirspeed, !(status->rc_input_mode == vehicle_status_s::RC_IN_MODE_OFF), !status->circuit_breaker_engaged_gpsfailure_check, true, reportFailures);
bool preflight_ok = Commander::preflightCheck(mavlink_fd, true, true, true, true,
checkAirspeed, !(status->rc_input_mode == vehicle_status_s::RC_IN_MODE_OFF),
!status->circuit_breaker_engaged_gpsfailure_check, true, reportFailures);
if (!status->cb_usb && status->usb_connected && prearm) {
preflight_ok = false;