Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Commander : Effectively track failure reporting and handle hotplug sensors #3171

Merged
merged 1 commit into from
Nov 14, 2015
Merged

Commander : Effectively track failure reporting and handle hotplug sensors #3171

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
3 changes: 3 additions & 0 deletions msg/vehicle_status.msg
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -120,6 +120,8 @@ bool in_transition_mode
bool condition_battery_voltage_valid
bool condition_system_in_air_restore # true if we can restore in mid air
bool condition_system_sensors_initialized
bool condition_system_prearm_error_reported # true if errors have already been reported
bool condition_system_hotplug_timeout # true if the hotplug sensor search is over
bool condition_system_returned_to_home
bool condition_auto_mission_available
bool condition_global_position_valid # set to true by the commander app if the quality of the position estimate is good enough to use it for navigation
Expand All @@ -139,6 +141,7 @@ bool rc_signal_lost_cmd # true if RC lost mode is commanded
bool rc_input_blocked # set if RC input should be ignored temporarily
uint8 rc_input_mode # set to 1 to disable the RC input, 2 to enable manual control to RC in mapping.

bool data_link_found_new # new datalink to GCS found
bool data_link_lost # datalink to GCS lost
bool data_link_lost_cmd # datalink to GCS lost mode commanded
uint8 data_link_lost_counter # counts unique data link lost events
Expand Down
68 changes: 34 additions & 34 deletions src/modules/commander/PreflightCheck.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -114,7 +114,7 @@ int check_calibration(int fd, const char* param_template, int &devid)
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;

Expand All @@ -124,7 +124,7 @@ static bool magnometerCheck(int mavlink_fd, unsigned instance, bool optional, in

if (fd < 0) {
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);
}

Expand All @@ -134,7 +134,7 @@ static bool magnometerCheck(int mavlink_fd, unsigned instance, bool optional, in
int ret = check_calibration(fd, "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;
Expand All @@ -143,7 +143,7 @@ static bool magnometerCheck(int mavlink_fd, unsigned instance, bool optional, in
ret = px4_ioctl(fd, 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;
Expand All @@ -154,7 +154,7 @@ static bool magnometerCheck(int mavlink_fd, unsigned instance, bool optional, in
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;

Expand All @@ -164,7 +164,7 @@ static bool accelerometerCheck(int mavlink_fd, unsigned instance, bool optional,

if (fd < 0) {
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);
}

Expand All @@ -174,7 +174,7 @@ static bool accelerometerCheck(int mavlink_fd, unsigned instance, bool optional,
int ret = check_calibration(fd, "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;
Expand All @@ -183,7 +183,7 @@ static bool accelerometerCheck(int mavlink_fd, unsigned instance, bool optional,
ret = px4_ioctl(fd, 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;
Expand All @@ -200,13 +200,13 @@ 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_log_critical(mavlink_fd, "PREFLIGHT FAIL: ACCEL READ");
/* this is frickin' fatal */
success = false;
goto out;
Expand All @@ -219,7 +219,7 @@ static bool accelerometerCheck(int mavlink_fd, unsigned instance, bool optional,
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;

Expand All @@ -229,7 +229,7 @@ static bool gyroCheck(int mavlink_fd, unsigned instance, bool optional, int &dev

if (fd < 0) {
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);
}

Expand All @@ -239,7 +239,7 @@ static bool gyroCheck(int mavlink_fd, unsigned instance, bool optional, int &dev
int ret = check_calibration(fd, "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;
Expand All @@ -248,7 +248,7 @@ static bool gyroCheck(int mavlink_fd, unsigned instance, bool optional, int &dev
ret = px4_ioctl(fd, 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;
Expand All @@ -259,7 +259,7 @@ static bool gyroCheck(int mavlink_fd, unsigned instance, bool optional, int &dev
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;

Expand All @@ -269,7 +269,7 @@ static bool baroCheck(int mavlink_fd, unsigned instance, bool optional, int &dev

if (fd < 0) {
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);
}

Expand All @@ -294,7 +294,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;
Expand All @@ -304,13 +304,13 @@ 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
}

Expand All @@ -319,7 +319,7 @@ static bool airspeedCheck(int mavlink_fd, bool optional)
return success;
}

static bool gnssCheck(int mavlink_fd)
static bool gnssCheck(int mavlink_fd, bool report_fail)
{
bool success = true;

Expand All @@ -342,15 +342,15 @@ 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(report_fail) mavlink_and_console_log_critical(mavlink_fd, "PREFLIGHT FAIL: GPS RECEIVER MISSING");
}

px4_close(gpsSub);
return success;
}

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;

Expand All @@ -365,7 +365,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;
}

Expand All @@ -376,7 +376,7 @@ 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_log_critical(mavlink_fd, "Warning: Primary compass not found");
failed = true;
}
}
Expand All @@ -392,7 +392,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;
}

Expand All @@ -403,7 +403,7 @@ 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_log_critical(mavlink_fd, "Warning: Primary accelerometer not found");
failed = true;
}
}
Expand All @@ -419,7 +419,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;
}

Expand All @@ -430,7 +430,7 @@ 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_log_critical(mavlink_fd, "Warning: Primary gyro not found");
failed = true;
}
}
Expand All @@ -446,7 +446,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;
}

Expand All @@ -458,29 +458,29 @@ 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_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_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;
}
}
Expand Down
2 changes: 1 addition & 1 deletion src/modules/commander/PreflightCheck.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -67,7 +67,7 @@ namespace Commander
* true if the GNSS receiver should be checked
**/
bool preflightCheck(int mavlink_fd, bool checkMag, bool checkAcc,
bool checkGyro, bool checkBaro, bool checkAirspeed, bool checkRC, bool checkGNSS, bool checkDynamic = false);
bool checkGyro, bool checkBaro, bool checkAirspeed, bool checkRC, bool checkGNSS, bool checkDynamic, bool reportFailures = false);

const unsigned max_mandatory_gyro_count = 1;
const unsigned max_optional_gyro_count = 3;
Expand Down
Loading