diff --git a/src/modules/commander/PreflightCheck.cpp b/src/modules/commander/PreflightCheck.cpp index 4d9bd8ae0c..893586346e 100644 --- a/src/modules/commander/PreflightCheck.cpp +++ b/src/modules/commander/PreflightCheck.cpp @@ -61,6 +61,7 @@ #include #include +#include #include @@ -269,8 +270,24 @@ out: return success; } +static bool gnssCheck(int mavlink_fd) +{ + bool success = true; + int gpsSub = orb_subscribe(ORB_ID(vehicle_gps_position)); + struct vehicle_gps_position_s gps; + + if (!orb_copy(ORB_ID(vehicle_gps_position), gpsSub, &gps) || + (hrt_elapsed_time(&gps.timestamp_position) > 500000)) { + mavlink_and_console_log_critical(mavlink_fd, "PREFLIGHT FAIL: GPS RECEIVER MISSING"); + success = false; + } + + close(gpsSub); + return success; +} + bool preflightCheck(int mavlink_fd, bool checkMag, bool checkAcc, bool checkGyro, - bool checkBaro, bool checkAirspeed, bool checkRC, bool checkDynamic) + bool checkBaro, bool checkAirspeed, bool checkRC, bool checkGNSS, bool checkDynamic) { bool failed = false; @@ -336,6 +353,13 @@ bool preflightCheck(int mavlink_fd, bool checkMag, bool checkAcc, bool checkGyro } } + /* ---- Global Navigation Satellite System receiver ---- */ + if(checkGNSS) { + if(!gnssCheck(mavlink_fd)) { + failed = true; + } + } + /* Report status */ return !failed; } diff --git a/src/modules/commander/PreflightCheck.h b/src/modules/commander/PreflightCheck.h index 66947a3470..b6423a4d9a 100644 --- a/src/modules/commander/PreflightCheck.h +++ b/src/modules/commander/PreflightCheck.h @@ -59,11 +59,15 @@ namespace Commander * true if the gyroscopes should be checked * @param checkBaro * true if the barometer should be checked +* @param checkAirspeed +* true if the airspeed sensor should be checked * @param checkRC * true if the Remote Controller should be checked +* @param checkGNSS +* 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 checkDynamic = false); + bool checkGyro, bool checkBaro, bool checkAirspeed, bool checkRC, bool checkGNSS, bool checkDynamic = 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 7fbb0a2dac..806951cf8d 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -916,6 +916,7 @@ int commander_thread_main(int argc, char *argv[]) status.circuit_breaker_engaged_airspd_check = false; status.circuit_breaker_engaged_enginefailure_check = false; status.circuit_breaker_engaged_gpsfailure_check = false; + get_circuit_breaker_params(); /* publish initial state */ status_pub = orb_advertise(ORB_ID(vehicle_status), &status); @@ -1117,8 +1118,6 @@ int commander_thread_main(int argc, char *argv[]) param_get(_param_sys_type, &(status.system_type)); // get system type status.is_rotary_wing = is_rotary_wing(&status) || is_vtol(&status); - get_circuit_breaker_params(); - bool checkAirspeed = false; /* Perform airspeed check only if circuit breaker is not * engaged and it's not a rotary wing */ @@ -1127,7 +1126,7 @@ int commander_thread_main(int argc, char *argv[]) } // Run preflight check - status.condition_system_sensors_initialized = Commander::preflightCheck(mavlink_fd, true, true, true, true, checkAirspeed, true); + status.condition_system_sensors_initialized = Commander::preflightCheck(mavlink_fd, true, true, true, true, checkAirspeed, status.circuit_breaker_engaged_gpsfailure_check, true); if (!status.condition_system_sensors_initialized) { set_tune_override(TONE_GPS_WARNING_TUNE); //sensor fail tune } @@ -1300,7 +1299,7 @@ int commander_thread_main(int argc, char *argv[]) } /* provide RC and sensor status feedback to the user */ - (void)Commander::preflightCheck(mavlink_fd, true, true, true, true, chAirspeed, true); + (void)Commander::preflightCheck(mavlink_fd, true, true, true, true, chAirspeed, status.circuit_breaker_engaged_gpsfailure_check, true); } telemetry_last_heartbeat[i] = telemetry.heartbeat_time; @@ -2749,7 +2748,7 @@ void *commander_low_prio_loop(void *arg) checkAirspeed = true; } - status.condition_system_sensors_initialized = Commander::preflightCheck(mavlink_fd, true, true, true, true, checkAirspeed, true); + status.condition_system_sensors_initialized = Commander::preflightCheck(mavlink_fd, true, true, true, true, checkAirspeed, status.circuit_breaker_engaged_gpsfailure_check, true); arming_state_transition(&status, &safety, vehicle_status_s::ARMING_STATE_STANDBY, &armed, true /* fRunPreArmChecks */, mavlink_fd);