From 5fcfdb759c2a83c3e13235b0f71d8d6299d46c33 Mon Sep 17 00:00:00 2001 From: Kabir Mohammed Date: Tue, 10 Nov 2015 19:33:14 +0530 Subject: [PATCH] commander : hotplug sensor support, better failure reporting --- msg/vehicle_status.msg | 3 + src/modules/commander/PreflightCheck.cpp | 68 ++++++++--------- src/modules/commander/PreflightCheck.h | 2 +- src/modules/commander/commander.cpp | 76 ++++++++++++++----- .../commander/state_machine_helper.cpp | 46 +++++++---- src/modules/commander/state_machine_helper.h | 2 +- src/modules/systemlib/rc_check.c | 37 ++++++--- src/modules/systemlib/rc_check.h | 2 +- 8 files changed, 153 insertions(+), 83 deletions(-) diff --git a/msg/vehicle_status.msg b/msg/vehicle_status.msg index e575b9d5e3..6d01b3b58e 100644 --- a/msg/vehicle_status.msg +++ b/msg/vehicle_status.msg @@ -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 @@ -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 diff --git a/src/modules/commander/PreflightCheck.cpp b/src/modules/commander/PreflightCheck.cpp index 2a8a0695cf..bfeabb9e7e 100644 --- a/src/modules/commander/PreflightCheck.cpp +++ b/src/modules/commander/PreflightCheck.cpp @@ -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; @@ -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); } @@ -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; @@ -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; @@ -154,7 +154,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; @@ -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); } @@ -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; @@ -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; @@ -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; @@ -219,7 +219,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; @@ -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); } @@ -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; @@ -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; @@ -259,7 +259,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; @@ -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); } @@ -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; @@ -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 } @@ -319,7 +319,7 @@ out: return success; } -static bool gnssCheck(int mavlink_fd) +static bool gnssCheck(int mavlink_fd, bool report_fail) { bool success = true; @@ -342,7 +342,7 @@ 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); @@ -350,7 +350,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; @@ -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; } @@ -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; } } @@ -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; } @@ -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; } } @@ -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; } @@ -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; } } @@ -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; } @@ -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; } } diff --git a/src/modules/commander/PreflightCheck.h b/src/modules/commander/PreflightCheck.h index b6423a4d9a..058d6b8956 100644 --- a/src/modules/commander/PreflightCheck.h +++ b/src/modules/commander/PreflightCheck.h @@ -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; diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 3573f5d6eb..32b24a0be3 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -148,6 +148,8 @@ static constexpr uint8_t COMMANDER_MAX_GPS_NOISE = 60; /**< Maximum percentage #define OFFBOARD_TIMEOUT 500000 #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_MODE_REJECT_INTERVAL 10000000 @@ -377,9 +379,12 @@ int commander_main(int argc, char *argv[]) if (!strcmp(argv[1], "check")) { 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); - warnx("FINAL RESULT: %s", (checkres == 0) ? "OK" : "FAILED"); return 0; } @@ -891,6 +896,7 @@ int commander_thread_main(int argc, char *argv[]) /* not yet initialized */ commander_initialized = false; + bool sensor_fail_tune_played = false; bool arm_tune_played = false; bool was_landed = true; bool was_armed = false; @@ -1020,6 +1026,9 @@ 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_hotplug_timeout = false; status.counter++; status.timestamp = hrt_absolute_time(); @@ -1101,7 +1110,7 @@ int commander_thread_main(int argc, char *argv[]) bool updated = false; - rc_calibration_check(mavlink_fd); + rc_calibration_check(mavlink_fd, true); /* Subscribe to safety topic */ int safety_sub = orb_subscribe(ORB_ID(safety)); @@ -1243,6 +1252,7 @@ int commander_thread_main(int argc, char *argv[]) // Run preflight check 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_rc_in_off, &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; set_tune_override(TONE_STARTUP_TUNE); //normal boot tune } else { - status.condition_system_sensors_initialized = Commander::preflightCheck(mavlink_fd, true, true, true, true, - checkAirspeed, (status.rc_input_mode == vehicle_status_s::RC_IN_MODE_DEFAULT), !status.circuit_breaker_engaged_gpsfailure_check); - if (!status.condition_system_sensors_initialized) { - set_tune_override(TONE_GPS_WARNING_TUNE); //sensor fail tune - } - else { + // sensor diagnostics done continiously, not just at boot so don't warn about any issues just yet + status.condition_system_sensors_initialized = Commander::preflightCheck(mavlink_fd, true, true, true, true, + checkAirspeed, (status.rc_input_mode == vehicle_status_s::RC_IN_MODE_DEFAULT), !status.circuit_breaker_engaged_gpsfailure_check, false); set_tune_override(TONE_STARTUP_TUNE); //normal boot tune - } } 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) { 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 */ - rc_calibration_check(mavlink_fd); + rc_calibration_check(mavlink_fd, true); } /* Safety parameters */ @@ -1446,6 +1452,8 @@ int commander_thread_main(int argc, char *argv[]) !armed.armed) { bool chAirspeed = false; + bool hotplug_timeout = hrt_elapsed_time(&commander_boot_timestamp) > HOTPLUG_SENS_TIMEOUT; + /* Perform airspeed check only if circuit breaker is not * engaged and it's not a 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)) { /* HIL configuration: check only RC input */ (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 { /* check sensors also */ (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] && 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) { 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; @@ -2189,6 +2202,10 @@ int commander_thread_main(int argc, char *argv[]) /* telemetry was healthy also in last iteration * we don't have to check a timeout */ have_link = true; + if(status.data_link_found_new) { + status.data_link_found_new = false; + status_changed = true; + } } } else { @@ -2414,7 +2431,7 @@ int commander_thread_main(int argc, char *argv[]) } else { set_tune(TONE_STOP_TUNE); } - + /* reset arm_tune_played when disarmed */ 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; } + + /* 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++; @@ -2508,19 +2540,24 @@ control_status_leds(vehicle_status_s *status_local, const actuator_armed_s *actu /* driving rgbled */ if (changed) { bool set_normal_color = false; - + bool hotplug_timeout = hrt_elapsed_time(&commander_boot_timestamp) > HOTPLUG_SENS_TIMEOUT; + /* set mode */ if (status_local->arming_state == vehicle_status_s::ARMING_STATE_ARMED) { rgbled_set_mode(RGBLED_MODE_ON); 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_color(RGBLED_COLOR_RED); } else if (status_local->arming_state == vehicle_status_s::ARMING_STATE_STANDBY) { rgbled_set_mode(RGBLED_MODE_BREATHE); 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 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. bool checkAirspeed = false; + bool hotplug_timeout = hrt_elapsed_time(&commander_boot_timestamp) > HOTPLUG_SENS_TIMEOUT; /* Perform airspeed check only if circuit breaker is not * engaged and it's not a 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.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); diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 21789c9c66..bdf88920fa 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -93,8 +93,6 @@ static const char * const state_names[vehicle_status_s::ARMING_STATE_MAX] = { "ARMING_STATE_IN_AIR_RESTORE", }; -static bool sensor_feedback_provided = false; - transition_result_t arming_state_transition(struct vehicle_status_s *status, ///< current vehicle status 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; - /* 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 && 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) && (status->arming_state != vehicle_status_s::ARMING_STATE_STANDBY_ERROR) && (!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"); - sensor_feedback_provided = true; + status->condition_system_prearm_error_reported = true; } feedback_provided = true; valid_transition = false; @@ -262,7 +270,11 @@ 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 && 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 */ @@ -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; } -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; /* Perform airspeed check only if circuit breaker is not * 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; } - 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); - - if (status->usb_connected) { - prearm_ok = false; - mavlink_log_critical(mavlink_fd, "NOT ARMING: Flying with USB connected prohibited"); + 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 && prearm) { + preflight_ok = false; + if(reportFailures) mavlink_log_critical(mavlink_fd, "NOT ARMING: Flying with USB connected prohibited"); } - return !prearm_ok; + return !preflight_ok; } diff --git a/src/modules/commander/state_machine_helper.h b/src/modules/commander/state_machine_helper.h index 084813f8cb..bdb7b13f28 100644 --- a/src/modules/commander/state_machine_helper.h +++ b/src/modules/commander/state_machine_helper.h @@ -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); -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_ */ diff --git a/src/modules/systemlib/rc_check.c b/src/modules/systemlib/rc_check.c index 25dea7a9b0..0f5d96f2b8 100644 --- a/src/modules/systemlib/rc_check.c +++ b/src/modules/systemlib/rc_check.c @@ -52,7 +52,7 @@ #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]; @@ -74,7 +74,8 @@ int rc_calibration_check(int mavlink_fd) param_t map_parm = param_find(rc_map_mandatory[j]); 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 */ usleep(100000); map_fail_count++; @@ -86,14 +87,16 @@ int rc_calibration_check(int mavlink_fd) param_get(map_parm, &mapping); 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 */ usleep(100000); map_fail_count++; } 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 */ usleep(100000); map_fail_count++; @@ -144,35 +147,44 @@ int rc_calibration_check(int mavlink_fd) /* assert min..center..max ordering */ if (param_min < RC_INPUT_LOWEST_MIN_US) { 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 */ usleep(100000); } if (param_max > RC_INPUT_HIGHEST_MAX_US) { 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 */ usleep(100000); } if (param_trim < param_min) { 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 */ usleep(100000); } if (param_trim > param_max) { 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 */ usleep(100000); } /* assert deadzone is sane */ 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 */ usleep(100000); count++; @@ -187,8 +199,11 @@ int rc_calibration_check(int mavlink_fd) if (channels_failed) { 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); } diff --git a/src/modules/systemlib/rc_check.h b/src/modules/systemlib/rc_check.h index 035f63bef4..239c8a6a13 100644 --- a/src/modules/systemlib/rc_check.h +++ b/src/modules/systemlib/rc_check.h @@ -47,6 +47,6 @@ __BEGIN_DECLS * @return 0 / OK if RC calibration is ok, index + 1 of the first * 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