dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

Merge pull request #3171 from mhkabir/commander_prearm 

Commander : Effectively track failure reporting and handle hotplug sensors
This commit is contained in:
Lorenz Meier 2015-11-14 10:29:28 +01:00
parent 4f4f1d901e 5fcfdb759c
commit 32bbbb0cdc
8 changed files with 153 additions and 83 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
msg/vehicle_status.msg
View File

@ -120,6 +120,8 @@ bool in_transition_mode
bool condition_battery_voltage_valid bool condition_battery_voltage_valid
bool condition_system_in_air_restore # true if we can restore in mid air bool condition_system_in_air_restore # true if we can restore in mid air
bool condition_system_sensors_initialized 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_system_returned_to_home
bool condition_auto_mission_available 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 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
@ -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 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. 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 # datalink to GCS lost
bool data_link_lost_cmd # datalink to GCS lost mode commanded bool data_link_lost_cmd # datalink to GCS lost mode commanded
uint8 data_link_lost_counter # counts unique data link lost events uint8 data_link_lost_counter # counts unique data link lost events

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

@ -114,7 +114,7 @@ int check_calibration(int fd, const char* param_template, int &devid)
return !calibration_found; 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; bool success = true;
@ -124,7 +124,7 @@ static bool magnometerCheck(int mavlink_fd, unsigned instance, bool optional, in
if (fd < 0) { if (fd < 0) {
if (!optional) { 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); "PREFLIGHT FAIL: NO MAG SENSOR #%u", instance);
} }
@ -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); int ret = check_calibration(fd, "CAL_MAG%u_ID", device_id);
if (ret) { 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); "PREFLIGHT FAIL: MAG #%u UNCALIBRATED", instance);
success = false; success = false;
goto out; goto out;
@ -143,7 +143,7 @@ static bool magnometerCheck(int mavlink_fd, unsigned instance, bool optional, in
ret = px4_ioctl(fd, MAGIOCSELFTEST, 0); ret = px4_ioctl(fd, MAGIOCSELFTEST, 0);
if (ret != OK) { 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); "PREFLIGHT FAIL: MAG #%u SELFTEST FAILED", instance);
success = false; success = false;
goto out; goto out;
@ -154,7 +154,7 @@ out:
return success; 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; bool success = true;
@ -164,7 +164,7 @@ static bool accelerometerCheck(int mavlink_fd, unsigned instance, bool optional,
if (fd < 0) { if (fd < 0) {
if (!optional) { 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); "PREFLIGHT FAIL: NO ACCEL SENSOR #%u", instance);
} }
@ -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); int ret = check_calibration(fd, "CAL_ACC%u_ID", device_id);
if (ret) { 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); "PREFLIGHT FAIL: ACCEL #%u UNCALIBRATED", instance);
success = false; success = false;
goto out; goto out;
@ -183,7 +183,7 @@ static bool accelerometerCheck(int mavlink_fd, unsigned instance, bool optional,
ret = px4_ioctl(fd, ACCELIOCSELFTEST, 0); ret = px4_ioctl(fd, ACCELIOCSELFTEST, 0);
if (ret != OK) { 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); "PREFLIGHT FAIL: ACCEL #%u SELFTEST FAILED", instance);
success = false; success = false;
goto out; goto out;
@ -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); 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 (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 */ /* this is frickin' fatal */
success = false; success = false;
goto out; goto out;
} }
} else { } 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 */ /* this is frickin' fatal */
success = false; success = false;
goto out; goto out;
@ -219,7 +219,7 @@ out:
return success; 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; bool success = true;
@ -229,7 +229,7 @@ static bool gyroCheck(int mavlink_fd, unsigned instance, bool optional, int &dev
if (fd < 0) { if (fd < 0) {
if (!optional) { 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); "PREFLIGHT FAIL: NO GYRO SENSOR #%u", instance);
} }
@ -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); int ret = check_calibration(fd, "CAL_GYRO%u_ID", device_id);
if (ret) { 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); "PREFLIGHT FAIL: GYRO #%u UNCALIBRATED", instance);
success = false; success = false;
goto out; goto out;
@ -248,7 +248,7 @@ static bool gyroCheck(int mavlink_fd, unsigned instance, bool optional, int &dev
ret = px4_ioctl(fd, GYROIOCSELFTEST, 0); ret = px4_ioctl(fd, GYROIOCSELFTEST, 0);
if (ret != OK) { 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); "PREFLIGHT FAIL: GYRO #%u SELFTEST FAILED", instance);
success = false; success = false;
goto out; goto out;
@ -259,7 +259,7 @@ out:
return success; 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; bool success = true;
@ -269,7 +269,7 @@ static bool baroCheck(int mavlink_fd, unsigned instance, bool optional, int &dev
if (fd < 0) { if (fd < 0) {
if (!optional) { 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); "PREFLIGHT FAIL: NO BARO SENSOR #%u", instance);
} }
@ -294,7 +294,7 @@ static bool baroCheck(int mavlink_fd, unsigned instance, bool optional, int &dev
return success; return success;
} }
static bool airspeedCheck(int mavlink_fd, bool optional) static bool airspeedCheck(int mavlink_fd, bool optional, bool report_fail)
{ {
bool success = true; bool success = true;
int ret; int ret;
@ -304,13 +304,13 @@ static bool airspeedCheck(int mavlink_fd, bool optional)
if ((ret = orb_copy(ORB_ID(airspeed), fd, &airspeed)) || if ((ret = orb_copy(ORB_ID(airspeed), fd, &airspeed)) ||
(hrt_elapsed_time(&airspeed.timestamp) > (500 * 1000))) { (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; success = false;
goto out; goto out;
} }
if (fabsf(airspeed.indicated_airspeed_m_s) > 6.0f) { 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 // XXX do not make this fatal yet
} }
@ -319,7 +319,7 @@ out:
return success; return success;
} }
static bool gnssCheck(int mavlink_fd) static bool gnssCheck(int mavlink_fd, bool report_fail)
{ {
bool success = true; bool success = true;
@ -342,7 +342,7 @@ static bool gnssCheck(int mavlink_fd)
//Report failure to detect module //Report failure to detect module
if(!success) { 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); px4_close(gpsSub);
@ -350,7 +350,7 @@ static bool gnssCheck(int mavlink_fd)
} }
bool preflightCheck(int mavlink_fd, bool checkMag, bool checkAcc, bool checkGyro, 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; bool failed = false;
@ -365,7 +365,7 @@ bool preflightCheck(int mavlink_fd, bool checkMag, bool checkAcc, bool checkGyro
bool required = (i < max_mandatory_mag_count); bool required = (i < max_mandatory_mag_count);
int device_id = -1; 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; failed = true;
} }
@ -376,7 +376,7 @@ bool preflightCheck(int mavlink_fd, bool checkMag, bool checkAcc, bool checkGyro
/* check if the primary device is present */ /* check if the primary device is present */
if (!prime_found && prime_id != 0) { 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; failed = true;
} }
} }
@ -392,7 +392,7 @@ bool preflightCheck(int mavlink_fd, bool checkMag, bool checkAcc, bool checkGyro
bool required = (i < max_mandatory_accel_count); bool required = (i < max_mandatory_accel_count);
int device_id = -1; 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; failed = true;
} }
@ -403,7 +403,7 @@ bool preflightCheck(int mavlink_fd, bool checkMag, bool checkAcc, bool checkGyro
/* check if the primary device is present */ /* check if the primary device is present */
if (!prime_found && prime_id != 0) { 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; failed = true;
} }
} }
@ -419,7 +419,7 @@ bool preflightCheck(int mavlink_fd, bool checkMag, bool checkAcc, bool checkGyro
bool required = (i < max_mandatory_gyro_count); bool required = (i < max_mandatory_gyro_count);
int device_id = -1; 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; failed = true;
} }
@ -430,7 +430,7 @@ bool preflightCheck(int mavlink_fd, bool checkMag, bool checkAcc, bool checkGyro
/* check if the primary device is present */ /* check if the primary device is present */
if (!prime_found && prime_id != 0) { 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; failed = true;
} }
} }
@ -446,7 +446,7 @@ bool preflightCheck(int mavlink_fd, bool checkMag, bool checkAcc, bool checkGyro
bool required = (i < max_mandatory_baro_count); bool required = (i < max_mandatory_baro_count);
int device_id = -1; 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; failed = true;
} }
@ -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 // TODO there is no logic in place to calibrate the primary baro yet
// // check if the primary device is present // // check if the primary device is present
if (!prime_found && prime_id != 0) { 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; failed = true;
} }
} }
/* ---- AIRSPEED ---- */ /* ---- AIRSPEED ---- */
if (checkAirspeed) { if (checkAirspeed) {
if (!airspeedCheck(mavlink_fd, true)) { if (!airspeedCheck(mavlink_fd, true, reportFailures)) {
failed = true; failed = true;
} }
} }
/* ---- RC CALIBRATION ---- */ /* ---- RC CALIBRATION ---- */
if (checkRC) { if (checkRC) {
if (rc_calibration_check(mavlink_fd) != OK) { if (rc_calibration_check(mavlink_fd, reportFailures) != OK) {
mavlink_log_critical(mavlink_fd, "RC calibration check failed"); if(reportFailures) mavlink_log_critical(mavlink_fd, "RC calibration check failed");
failed = true; failed = true;
} }
} }
/* ---- Global Navigation Satellite System receiver ---- */ /* ---- Global Navigation Satellite System receiver ---- */
if (checkGNSS) { if (checkGNSS) {
if(!gnssCheck(mavlink_fd)) { if(!gnssCheck(mavlink_fd, reportFailures)) {
failed = true; failed = true;
} }
} }

2
src/modules/commander/PreflightCheck.h
View File

@ -67,7 +67,7 @@ namespace Commander
* true if the GNSS receiver should be checked * true if the GNSS receiver should be checked
**/ **/
bool preflightCheck(int mavlink_fd, bool checkMag, bool checkAcc, 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_mandatory_gyro_count = 1;
const unsigned max_optional_gyro_count = 3; const unsigned max_optional_gyro_count = 3;

76
src/modules/commander/commander.cpp
View File

@ -148,6 +148,8 @@ static constexpr uint8_t COMMANDER_MAX_GPS_NOISE = 60; /**< Maximum percentage
#define OFFBOARD_TIMEOUT 500000 #define OFFBOARD_TIMEOUT 500000
#define DIFFPRESS_TIMEOUT 2000000 #define DIFFPRESS_TIMEOUT 2000000
#define HOTPLUG_SENS_TIMEOUT (8 * 1000 * 1000) /**< wait for hotplug sensors to come online for upto 10 seconds */
#define PRINT_INTERVAL 5000000 #define PRINT_INTERVAL 5000000
#define PRINT_MODE_REJECT_INTERVAL 10000000 #define PRINT_MODE_REJECT_INTERVAL 10000000
@ -377,9 +379,12 @@ int commander_main(int argc, char *argv[])
if (!strcmp(argv[1], "check")) { if (!strcmp(argv[1], "check")) {
int mavlink_fd_local = px4_open(MAVLINK_LOG_DEVICE, 0); int mavlink_fd_local = px4_open(MAVLINK_LOG_DEVICE, 0);
int checkres = prearm_check(&status, mavlink_fd_local); int checkres = 0;
checkres = preflight_check(&status, mavlink_fd_local, false);
warnx("Preflight check: %s", (checkres == 0) ? "OK" : "FAILED");
checkres = preflight_check(&status, mavlink_fd_local, true);
warnx("Prearm check: %s", (checkres == 0) ? "OK" : "FAILED");
px4_close(mavlink_fd_local); px4_close(mavlink_fd_local);
warnx("FINAL RESULT: %s", (checkres == 0) ? "OK" : "FAILED");
return 0; return 0;
} }
@ -891,6 +896,7 @@ int commander_thread_main(int argc, char *argv[])
/* not yet initialized */ /* not yet initialized */
commander_initialized = false; commander_initialized = false;
bool sensor_fail_tune_played = false;
bool arm_tune_played = false; bool arm_tune_played = false;
bool was_landed = true; bool was_landed = true;
bool was_armed = false; bool was_armed = false;
@ -1020,6 +1026,9 @@ int commander_thread_main(int argc, char *argv[])
// XXX for now just set sensors as initialized // XXX for now just set sensors as initialized
status.condition_system_sensors_initialized = true; status.condition_system_sensors_initialized = true;
status.condition_system_prearm_error_reported = true;
status.condition_system_hotplug_timeout = false;
status.counter++; status.counter++;
status.timestamp = hrt_absolute_time(); status.timestamp = hrt_absolute_time();
@ -1101,7 +1110,7 @@ int commander_thread_main(int argc, char *argv[])
bool updated = false; bool updated = false;
rc_calibration_check(mavlink_fd); rc_calibration_check(mavlink_fd, true);
/* Subscribe to safety topic */ /* Subscribe to safety topic */
int safety_sub = orb_subscribe(ORB_ID(safety)); int safety_sub = orb_subscribe(ORB_ID(safety));
@ -1243,6 +1252,7 @@ int commander_thread_main(int argc, char *argv[])
// Run preflight check // Run preflight check
int32_t rc_in_off = 0; int32_t rc_in_off = 0;
bool hotplug_timeout = hrt_elapsed_time(&commander_boot_timestamp) > HOTPLUG_SENS_TIMEOUT;
param_get(_param_autostart_id, &autostart_id); param_get(_param_autostart_id, &autostart_id);
param_get(_param_rc_in_off, &rc_in_off); param_get(_param_rc_in_off, &rc_in_off);
status.rc_input_mode = rc_in_off; status.rc_input_mode = rc_in_off;
@ -1251,14 +1261,10 @@ int commander_thread_main(int argc, char *argv[])
status.condition_system_sensors_initialized = false; status.condition_system_sensors_initialized = false;
set_tune_override(TONE_STARTUP_TUNE); //normal boot tune set_tune_override(TONE_STARTUP_TUNE); //normal boot tune
} else { } else {
status.condition_system_sensors_initialized = Commander::preflightCheck(mavlink_fd, true, true, true, true, // sensor diagnostics done continiously, not just at boot so don't warn about any issues just yet
checkAirspeed, (status.rc_input_mode == vehicle_status_s::RC_IN_MODE_DEFAULT), !status.circuit_breaker_engaged_gpsfailure_check); status.condition_system_sensors_initialized = Commander::preflightCheck(mavlink_fd, true, true, true, true,
if (!status.condition_system_sensors_initialized) { checkAirspeed, (status.rc_input_mode == vehicle_status_s::RC_IN_MODE_DEFAULT), !status.circuit_breaker_engaged_gpsfailure_check, false);
set_tune_override(TONE_GPS_WARNING_TUNE); //sensor fail tune
}
else {
set_tune_override(TONE_STARTUP_TUNE); //normal boot tune set_tune_override(TONE_STARTUP_TUNE); //normal boot tune
}
} }
commander_boot_timestamp = hrt_absolute_time(); commander_boot_timestamp = hrt_absolute_time();
@ -1354,11 +1360,11 @@ int commander_thread_main(int argc, char *argv[])
if (map_mode_sw == 0 && map_mode_sw != map_mode_sw_new && map_mode_sw_new < input_rc_s::RC_INPUT_MAX_CHANNELS && map_mode_sw_new > 0) { if (map_mode_sw == 0 && map_mode_sw != map_mode_sw_new && map_mode_sw_new < input_rc_s::RC_INPUT_MAX_CHANNELS && map_mode_sw_new > 0) {
status.condition_system_sensors_initialized = Commander::preflightCheck(mavlink_fd, true, true, true, true, checkAirspeed, status.condition_system_sensors_initialized = 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); !(status.rc_input_mode == vehicle_status_s::RC_IN_MODE_OFF), !status.circuit_breaker_engaged_gpsfailure_check, hotplug_timeout);
} }
/* re-check RC calibration */ /* re-check RC calibration */
rc_calibration_check(mavlink_fd); rc_calibration_check(mavlink_fd, true);
} }
/* Safety parameters */ /* Safety parameters */
@ -1446,6 +1452,8 @@ int commander_thread_main(int argc, char *argv[])
!armed.armed) { !armed.armed) {
bool chAirspeed = false; bool chAirspeed = false;
bool hotplug_timeout = hrt_elapsed_time(&commander_boot_timestamp) > HOTPLUG_SENS_TIMEOUT;
/* Perform airspeed check only if circuit breaker is not /* Perform airspeed check only if circuit breaker is not
* engaged and it's not a rotary wing */ * engaged and it's not a rotary wing */
if (!status.circuit_breaker_engaged_airspd_check && !status.is_rotary_wing) { if (!status.circuit_breaker_engaged_airspd_check && !status.is_rotary_wing) {
@ -1456,11 +1464,11 @@ int commander_thread_main(int argc, char *argv[])
if (is_hil_setup(autostart_id)) { if (is_hil_setup(autostart_id)) {
/* HIL configuration: check only RC input */ /* HIL configuration: check only RC input */
(void)Commander::preflightCheck(mavlink_fd, false, false, false, false, false, (void)Commander::preflightCheck(mavlink_fd, false, false, false, false, false,
!(status.rc_input_mode == vehicle_status_s::RC_IN_MODE_OFF), false); !(status.rc_input_mode == vehicle_status_s::RC_IN_MODE_OFF), false, true);
} else { } else {
/* check sensors also */ /* check sensors also */
(void)Commander::preflightCheck(mavlink_fd, true, true, true, true, chAirspeed, (void)Commander::preflightCheck(mavlink_fd, true, true, true, true, chAirspeed,
!(status.rc_input_mode == vehicle_status_s::RC_IN_MODE_OFF), !status.circuit_breaker_engaged_gpsfailure_check); !(status.rc_input_mode == vehicle_status_s::RC_IN_MODE_OFF), !status.circuit_breaker_engaged_gpsfailure_check, hotplug_timeout);
} }
} }
@ -2177,9 +2185,14 @@ int commander_thread_main(int argc, char *argv[])
if (telemetry_lost[i] && if (telemetry_lost[i] &&
hrt_elapsed_time(&telemetry_last_dl_loss[i]) > datalink_regain_timeout * 1e6) { hrt_elapsed_time(&telemetry_last_dl_loss[i]) > datalink_regain_timeout * 1e6) {
/* only report a regain */ /* report a regain */
if (telemetry_last_dl_loss[i] > 0) { if (telemetry_last_dl_loss[i] > 0) {
mavlink_and_console_log_info(mavlink_fd, "data link #%i regained", i); mavlink_and_console_log_info(mavlink_fd, "data link #%i regained", i);
} else if (telemetry_last_dl_loss[i] == 0){
/* report a new link */
mavlink_and_console_log_info(mavlink_fd, "new data link connected : #%i", i);
status.data_link_found_new = true;
status_changed = true;
} }
telemetry_lost[i] = false; telemetry_lost[i] = false;
@ -2189,6 +2202,10 @@ int commander_thread_main(int argc, char *argv[])
/* telemetry was healthy also in last iteration /* telemetry was healthy also in last iteration
* we don't have to check a timeout */ * we don't have to check a timeout */
have_link = true; have_link = true;
if(status.data_link_found_new) {
status.data_link_found_new = false;
status_changed = true;
}
} }
} else { } else {
@ -2414,7 +2431,7 @@ int commander_thread_main(int argc, char *argv[])
} else { } else {
set_tune(TONE_STOP_TUNE); set_tune(TONE_STOP_TUNE);
} }
/* reset arm_tune_played when disarmed */ /* reset arm_tune_played when disarmed */
if (!armed.armed || (safety.safety_switch_available && !safety.safety_off)) { if (!armed.armed || (safety.safety_switch_available && !safety.safety_off)) {
@ -2425,6 +2442,21 @@ int commander_thread_main(int argc, char *argv[])
arm_tune_played = false; arm_tune_played = false;
} }
/* play sensor failure tunes if we already waited for hotplug sensors to come up and failed */
hotplug_timeout = hrt_elapsed_time(&commander_boot_timestamp) > HOTPLUG_SENS_TIMEOUT;
if (!sensor_fail_tune_played && (!status.condition_system_sensors_initialized && hotplug_timeout)) {
set_tune_override(TONE_GPS_WARNING_TUNE);
sensor_fail_tune_played = true;
status_changed = true;
}
/* update timeout flag */
if(!(hotplug_timeout == status.condition_system_hotplug_timeout)) {
status.condition_system_hotplug_timeout = hotplug_timeout;
status_changed = true;
}
counter++; counter++;
@ -2508,19 +2540,24 @@ control_status_leds(vehicle_status_s *status_local, const actuator_armed_s *actu
/* driving rgbled */ /* driving rgbled */
if (changed) { if (changed) {
bool set_normal_color = false; bool set_normal_color = false;
bool hotplug_timeout = hrt_elapsed_time(&commander_boot_timestamp) > HOTPLUG_SENS_TIMEOUT;
/* set mode */ /* set mode */
if (status_local->arming_state == vehicle_status_s::ARMING_STATE_ARMED) { if (status_local->arming_state == vehicle_status_s::ARMING_STATE_ARMED) {
rgbled_set_mode(RGBLED_MODE_ON); rgbled_set_mode(RGBLED_MODE_ON);
set_normal_color = true; set_normal_color = true;
} else if (status_local->arming_state == vehicle_status_s::ARMING_STATE_ARMED_ERROR || !status.condition_system_sensors_initialized) { } else if (status_local->arming_state == vehicle_status_s::ARMING_STATE_ARMED_ERROR || (!status.condition_system_sensors_initialized && hotplug_timeout)) {
rgbled_set_mode(RGBLED_MODE_BLINK_FAST); rgbled_set_mode(RGBLED_MODE_BLINK_FAST);
rgbled_set_color(RGBLED_COLOR_RED); rgbled_set_color(RGBLED_COLOR_RED);
} else if (status_local->arming_state == vehicle_status_s::ARMING_STATE_STANDBY) { } else if (status_local->arming_state == vehicle_status_s::ARMING_STATE_STANDBY) {
rgbled_set_mode(RGBLED_MODE_BREATHE); rgbled_set_mode(RGBLED_MODE_BREATHE);
set_normal_color = true; set_normal_color = true;
} else if (!status.condition_system_sensors_initialized && !hotplug_timeout) {
rgbled_set_mode(RGBLED_MODE_BREATHE);
set_normal_color = true;
} else { // STANDBY_ERROR and other states } else { // STANDBY_ERROR and other states
rgbled_set_mode(RGBLED_MODE_BLINK_NORMAL); rgbled_set_mode(RGBLED_MODE_BLINK_NORMAL);
@ -3197,6 +3234,7 @@ void *commander_low_prio_loop(void *arg)
// so this would be prone to false negatives. // so this would be prone to false negatives.
bool checkAirspeed = false; bool checkAirspeed = false;
bool hotplug_timeout = hrt_elapsed_time(&commander_boot_timestamp) > HOTPLUG_SENS_TIMEOUT;
/* Perform airspeed check only if circuit breaker is not /* Perform airspeed check only if circuit breaker is not
* engaged and it's not a rotary wing */ * engaged and it's not a rotary wing */
if (!status.circuit_breaker_engaged_airspd_check && !status.is_rotary_wing) { if (!status.circuit_breaker_engaged_airspd_check && !status.is_rotary_wing) {
@ -3204,7 +3242,7 @@ void *commander_low_prio_loop(void *arg)
} }
status.condition_system_sensors_initialized = Commander::preflightCheck(mavlink_fd, true, true, true, true, checkAirspeed, status.condition_system_sensors_initialized = 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); !(status.rc_input_mode == vehicle_status_s::RC_IN_MODE_OFF), !status.circuit_breaker_engaged_gpsfailure_check, hotplug_timeout);
arming_state_transition(&status, &safety, vehicle_status_s::ARMING_STATE_STANDBY, &armed, false /* fRunPreArmChecks */, mavlink_fd); arming_state_transition(&status, &safety, vehicle_status_s::ARMING_STATE_STANDBY, &armed, false /* fRunPreArmChecks */, mavlink_fd);

46
src/modules/commander/state_machine_helper.cpp
View File

@ -93,8 +93,6 @@ static const char * const state_names[vehicle_status_s::ARMING_STATE_MAX] = {
"ARMING_STATE_IN_AIR_RESTORE", "ARMING_STATE_IN_AIR_RESTORE",
}; };
static bool sensor_feedback_provided = false;
transition_result_t transition_result_t
arming_state_transition(struct vehicle_status_s *status, ///< current vehicle status arming_state_transition(struct vehicle_status_s *status, ///< current vehicle status
const struct safety_s *safety, ///< current safety settings const struct safety_s *safety, ///< current safety settings
@ -122,10 +120,18 @@ arming_state_transition(struct vehicle_status_s *status, ///< current vehicle s
*/ */
int prearm_ret = OK; int prearm_ret = OK;
/* only perform the check if we have to */ /* only perform the pre-arm check if we have to */
if (fRunPreArmChecks && new_arming_state == vehicle_status_s::ARMING_STATE_ARMED if (fRunPreArmChecks && new_arming_state == vehicle_status_s::ARMING_STATE_ARMED
&& status->hil_state == vehicle_status_s::HIL_STATE_OFF) { && status->hil_state == vehicle_status_s::HIL_STATE_OFF) {
prearm_ret = prearm_check(status, mavlink_fd); prearm_ret = preflight_check(status, mavlink_fd, true /* pre-arm */ );
}
/* re-run the pre-flight check as long as sensors are failing */
if (!status->condition_system_sensors_initialized
&& (new_arming_state == vehicle_status_s::ARMING_STATE_ARMED ||
new_arming_state == vehicle_status_s::ARMING_STATE_STANDBY)
&& status->hil_state == vehicle_status_s::HIL_STATE_OFF) {
prearm_ret = preflight_check(status, mavlink_fd, false /* pre-flight */);
status->condition_system_sensors_initialized = !prearm_ret;
} }
/* /*
@ -243,9 +249,11 @@ arming_state_transition(struct vehicle_status_s *status, ///< current vehicle s
(new_arming_state == vehicle_status_s::ARMING_STATE_STANDBY) && (new_arming_state == vehicle_status_s::ARMING_STATE_STANDBY) &&
(status->arming_state != vehicle_status_s::ARMING_STATE_STANDBY_ERROR) && (status->arming_state != vehicle_status_s::ARMING_STATE_STANDBY_ERROR) &&
(!status->condition_system_sensors_initialized)) { (!status->condition_system_sensors_initialized)) {
if (!sensor_feedback_provided || (new_arming_state == vehicle_status_s::ARMING_STATE_ARMED)) { if ((!status->condition_system_prearm_error_reported &&
status->condition_system_hotplug_timeout) ||
(new_arming_state == vehicle_status_s::ARMING_STATE_ARMED)) {
mavlink_and_console_log_critical(mavlink_fd, "Not ready to fly: Sensors need inspection"); mavlink_and_console_log_critical(mavlink_fd, "Not ready to fly: Sensors need inspection");
sensor_feedback_provided = true; status->condition_system_prearm_error_reported = true;
} }
feedback_provided = true; feedback_provided = true;
valid_transition = false; valid_transition = false;
@ -262,7 +270,11 @@ arming_state_transition(struct vehicle_status_s *status, ///< current vehicle s
/* reset feedback state */ /* reset feedback state */
if (status->arming_state != vehicle_status_s::ARMING_STATE_STANDBY_ERROR && if (status->arming_state != vehicle_status_s::ARMING_STATE_STANDBY_ERROR &&
valid_transition) { valid_transition) {
sensor_feedback_provided = false; status->condition_system_prearm_error_reported = false;
}
if(status->data_link_found_new == true)
{
status->condition_system_prearm_error_reported = false;
} }
/* end of atomic state update */ /* end of atomic state update */
@ -731,11 +743,13 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en
return status->nav_state != nav_state_old; return status->nav_state != nav_state_old;
} }
int prearm_check(const struct vehicle_status_s *status, const int mavlink_fd) int preflight_check(const struct vehicle_status_s *status, const int mavlink_fd, bool prearm)
{ {
/* at this point this equals the preflight check, but might add additional /*
* quantities later.
*/ */
bool reportFailures = false;
reportFailures = !status->condition_system_prearm_error_reported && status->condition_system_hotplug_timeout;
bool checkAirspeed = false; bool checkAirspeed = false;
/* Perform airspeed check only if circuit breaker is not /* Perform airspeed check only if circuit breaker is not
* engaged and it's not a rotary wing */ * engaged and it's not a rotary wing */
@ -743,12 +757,12 @@ int prearm_check(const struct vehicle_status_s *status, const int mavlink_fd)
checkAirspeed = true; checkAirspeed = true;
} }
bool prearm_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); 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->usb_connected) { if (status->usb_connected && prearm) {
prearm_ok = false; preflight_ok = false;
mavlink_log_critical(mavlink_fd, "NOT ARMING: Flying with USB connected prohibited"); if(reportFailures) mavlink_log_critical(mavlink_fd, "NOT ARMING: Flying with USB connected prohibited");
} }
return !prearm_ok; return !preflight_ok;
} }

2
src/modules/commander/state_machine_helper.h
View File

@ -65,6 +65,6 @@ transition_result_t hil_state_transition(hil_state_t new_state, orb_advert_t sta
bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_enabled, const bool mission_finished, const bool stay_in_failsafe); bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_enabled, const bool mission_finished, const bool stay_in_failsafe);
int prearm_check(const struct vehicle_status_s *status, const int mavlink_fd); int preflight_check(const struct vehicle_status_s *status, const int mavlink_fd, bool prearm);
#endif /* STATE_MACHINE_HELPER_H_ */ #endif /* STATE_MACHINE_HELPER_H_ */

37
src/modules/systemlib/rc_check.c
View File

@ -52,7 +52,7 @@
#define RC_INPUT_MAP_UNMAPPED 0 #define RC_INPUT_MAP_UNMAPPED 0
int rc_calibration_check(int mavlink_fd) int rc_calibration_check(int mavlink_fd, bool report_fail)
{ {
char nbuf[20]; char nbuf[20];
@ -74,7 +74,8 @@ int rc_calibration_check(int mavlink_fd)
param_t map_parm = param_find(rc_map_mandatory[j]); param_t map_parm = param_find(rc_map_mandatory[j]);
if (map_parm == PARAM_INVALID) { if (map_parm == PARAM_INVALID) {
mavlink_log_critical(mavlink_fd, "RC ERR: PARAM %s MISSING", rc_map_mandatory[j]); if (report_fail) { mavlink_log_critical(mavlink_fd, "RC ERR: PARAM %s MISSING", rc_map_mandatory[j]); }
/* give system time to flush error message in case there are more */ /* give system time to flush error message in case there are more */
usleep(100000); usleep(100000);
map_fail_count++; map_fail_count++;
@ -86,14 +87,16 @@ int rc_calibration_check(int mavlink_fd)
param_get(map_parm, &mapping); param_get(map_parm, &mapping);
if (mapping > RC_INPUT_MAX_CHANNELS) { if (mapping > RC_INPUT_MAX_CHANNELS) {
mavlink_log_critical(mavlink_fd, "RC ERR: %s >= # CHANS", rc_map_mandatory[j]); if (report_fail) { mavlink_log_critical(mavlink_fd, "RC ERR: %s >= # CHANS", rc_map_mandatory[j]); }
/* give system time to flush error message in case there are more */ /* give system time to flush error message in case there are more */
usleep(100000); usleep(100000);
map_fail_count++; map_fail_count++;
} }
if (mapping == 0) { if (mapping == 0) {
mavlink_log_critical(mavlink_fd, "RC ERR: Mandatory %s is unmapped", rc_map_mandatory[j]); if (report_fail) { mavlink_log_critical(mavlink_fd, "RC ERR: Mandatory %s is unmapped", rc_map_mandatory[j]); }
/* give system time to flush error message in case there are more */ /* give system time to flush error message in case there are more */
usleep(100000); usleep(100000);
map_fail_count++; map_fail_count++;
@ -144,35 +147,44 @@ int rc_calibration_check(int mavlink_fd)
/* assert min..center..max ordering */ /* assert min..center..max ordering */
if (param_min < RC_INPUT_LOWEST_MIN_US) { if (param_min < RC_INPUT_LOWEST_MIN_US) {
count++; count++;
mavlink_log_critical(mavlink_fd, "RC ERR: RC_%d_MIN < %u", i + 1, RC_INPUT_LOWEST_MIN_US);
if (report_fail) { mavlink_log_critical(mavlink_fd, "RC ERR: RC_%d_MIN < %u", i + 1, RC_INPUT_LOWEST_MIN_US); }
/* give system time to flush error message in case there are more */ /* give system time to flush error message in case there are more */
usleep(100000); usleep(100000);
} }
if (param_max > RC_INPUT_HIGHEST_MAX_US) { if (param_max > RC_INPUT_HIGHEST_MAX_US) {
count++; count++;
mavlink_log_critical(mavlink_fd, "RC ERR: RC_%d_MAX > %u", i + 1, RC_INPUT_HIGHEST_MAX_US);
if (report_fail) { mavlink_log_critical(mavlink_fd, "RC ERR: RC_%d_MAX > %u", i + 1, RC_INPUT_HIGHEST_MAX_US); }
/* give system time to flush error message in case there are more */ /* give system time to flush error message in case there are more */
usleep(100000); usleep(100000);
} }
if (param_trim < param_min) { if (param_trim < param_min) {
count++; count++;
mavlink_log_critical(mavlink_fd, "RC ERR: RC_%d_TRIM < MIN (%d/%d)", i + 1, (int)param_trim, (int)param_min);
if (report_fail) { mavlink_log_critical(mavlink_fd, "RC ERR: RC_%d_TRIM < MIN (%d/%d)", i + 1, (int)param_trim, (int)param_min); }
/* give system time to flush error message in case there are more */ /* give system time to flush error message in case there are more */
usleep(100000); usleep(100000);
} }
if (param_trim > param_max) { if (param_trim > param_max) {
count++; count++;
mavlink_log_critical(mavlink_fd, "RC ERR: RC_%d_TRIM > MAX (%d/%d)", i + 1, (int)param_trim, (int)param_max);
if (report_fail) { mavlink_log_critical(mavlink_fd, "RC ERR: RC_%d_TRIM > MAX (%d/%d)", i + 1, (int)param_trim, (int)param_max); }
/* give system time to flush error message in case there are more */ /* give system time to flush error message in case there are more */
usleep(100000); usleep(100000);
} }
/* assert deadzone is sane */ /* assert deadzone is sane */
if (param_dz > RC_INPUT_MAX_DEADZONE_US) { if (param_dz > RC_INPUT_MAX_DEADZONE_US) {
mavlink_log_critical(mavlink_fd, "RC ERR: RC_%d_DZ > %u", i + 1, RC_INPUT_MAX_DEADZONE_US); if (report_fail) { mavlink_log_critical(mavlink_fd, "RC ERR: RC_%d_DZ > %u", i + 1, RC_INPUT_MAX_DEADZONE_US); }
/* give system time to flush error message in case there are more */ /* give system time to flush error message in case there are more */
usleep(100000); usleep(100000);
count++; count++;
@ -187,8 +199,11 @@ int rc_calibration_check(int mavlink_fd)
if (channels_failed) { if (channels_failed) {
sleep(2); sleep(2);
mavlink_and_console_log_critical(mavlink_fd, "%d config error%s for %d RC channel%s.", total_fail_count,
(total_fail_count > 1) ? "s" : "", channels_failed, (channels_failed > 1) ? "s" : ""); if (report_fail) mavlink_and_console_log_critical(mavlink_fd, "%d config error%s for %d RC channel%s.",
total_fail_count,
(total_fail_count > 1) ? "s" : "", channels_failed, (channels_failed > 1) ? "s" : "");
usleep(100000); usleep(100000);
} }

2
src/modules/systemlib/rc_check.h
View File

@ -47,6 +47,6 @@ __BEGIN_DECLS
* @return 0 / OK if RC calibration is ok, index + 1 of the first * @return 0 / OK if RC calibration is ok, index + 1 of the first
* channel that failed else (so 1 == first channel failed) * channel that failed else (so 1 == first channel failed)
*/ */
__EXPORT int rc_calibration_check(int mavlink_fd); __EXPORT int rc_calibration_check(int mavlink_fd, bool report_fail);
__END_DECLS __END_DECLS