diff --git a/msg/vehicle_status_flags.msg b/msg/vehicle_status_flags.msg index 7abfeece12..d53d5e2ea6 100644 --- a/msg/vehicle_status_flags.msg +++ b/msg/vehicle_status_flags.msg @@ -31,5 +31,4 @@ bool offboard_control_loss_timeout # true if offboard is lost for bool rc_signal_found_once bool rc_input_blocked # set if RC input should be ignored temporarily bool vtol_transition_failure # Set to true if vtol transition failed -bool gps_failure # Set to true if a gps failure is detected bool usb_connected # status of the USB power supply diff --git a/src/modules/commander/PreflightCheck.cpp b/src/modules/commander/PreflightCheck.cpp index e6879fd79e..2830a2afb0 100644 --- a/src/modules/commander/PreflightCheck.cpp +++ b/src/modules/commander/PreflightCheck.cpp @@ -448,42 +448,6 @@ out: return success; } -static bool gnssCheck(orb_advert_t *mavlink_log_pub, bool report_fail, bool &lock_detected) -{ - bool success = true; - lock_detected = false; - int gpsSub = orb_subscribe(ORB_ID(vehicle_gps_position)); - - //Wait up to 2000ms to allow the driver to detect a GNSS receiver module - px4_pollfd_struct_t fds[1]; - fds[0].fd = gpsSub; - fds[0].events = POLLIN; - - if (px4_poll(fds, 1, 2000) <= 0) { - success = false; - - } else { - struct vehicle_gps_position_s gps; - - if ((OK != orb_copy(ORB_ID(vehicle_gps_position), gpsSub, &gps)) || - (hrt_elapsed_time(&gps.timestamp) > 1000000)) { - success = false; - } else if (gps.fix_type >= 3) { - lock_detected = true; - } - } - - //Report failure to detect module - if (!success) { - if (report_fail) { - mavlink_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: GPS RECEIVER MISSING"); - } - } - - orb_unsubscribe(gpsSub); - return success; -} - static bool ekf2Check(orb_advert_t *mavlink_log_pub, bool optional, bool report_fail, bool enforce_gps_required) { bool success = true; // start with a pass and change to a fail if any test fails @@ -779,14 +743,6 @@ bool preflightCheck(orb_advert_t *mavlink_log_pub, bool checkSensors, bool check } } - /* ---- Global Navigation Satellite System receiver ---- */ - if (checkGNSS) { - bool lock_detected = false; - if (!gnssCheck(mavlink_log_pub, reportFailures, lock_detected)) { - failed = true; - } - } - /* ---- Navigation EKF ---- */ // only check EKF2 data if EKF2 is selected as the estimator and GNSS checking is enabled int32_t estimator_type; diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 90db3fd2da..3af49db8c5 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -136,8 +136,6 @@ typedef enum VEHICLE_MODE_FLAG VEHICLE_MODE_FLAG_ENUM_END=129, /* | */ } VEHICLE_MODE_FLAG; -static constexpr uint8_t COMMANDER_MAX_GPS_NOISE = 60; /**< Maximum percentage signal to noise ratio allowed for GPS reception */ - /* Decouple update interval and hysteresis counters, all depends on intervals */ #define COMMANDER_MONITORING_INTERVAL 10000 #define COMMANDER_MONITORING_LOOPSPERMSEC (1/(COMMANDER_MONITORING_INTERVAL/1000.0f)) @@ -1344,13 +1342,6 @@ Commander::run() status_flags.condition_local_velocity_valid = false; status_flags.condition_local_altitude_valid = false; - // initialize gps failure to false if circuit breaker enabled - if (status_flags.circuit_breaker_engaged_gpsfailure_check) { - status_flags.gps_failure = false; - } else { - status_flags.gps_failure = true; - } - /* publish initial state */ status_pub = orb_advertise(ORB_ID(vehicle_status), &status); @@ -1457,10 +1448,6 @@ Commander::run() /* Subscribe to GPS topic */ int gps_sub = orb_subscribe(ORB_ID(vehicle_gps_position)); - struct vehicle_gps_position_s gps_position; - memset(&gps_position, 0, sizeof(gps_position)); - gps_position.eph = FLT_MAX; - gps_position.epv = FLT_MAX; /* Subscribe to differential pressure topic */ int diff_pres_sub = orb_subscribe(ORB_ID(differential_pressure)); @@ -2339,54 +2326,22 @@ Commander::run() orb_check(gps_sub, &updated); if (updated) { + + vehicle_gps_position_s gps_position = {}; + gps_position.eph = FLT_MAX; + gps_position.epv = FLT_MAX; + orb_copy(ORB_ID(vehicle_gps_position), gps_sub, &gps_position); - } - /* Initialize map projection if gps is valid */ - if (!map_projection_global_initialized() - && (gps_position.eph < eph_threshold) - && (gps_position.epv < epv_threshold) - && hrt_elapsed_time((hrt_abstime *)&gps_position.timestamp) < 1e6) { - /* set reference for global coordinates <--> local coordiantes conversion and map_projection */ - globallocalconverter_init((double)gps_position.lat * 1.0e-7, (double)gps_position.lon * 1.0e-7, - (float)gps_position.alt * 1.0e-3f, hrt_absolute_time()); - } + /* Initialize map projection if gps is valid */ + if (!map_projection_global_initialized() + && (gps_position.eph < eph_threshold) + && (gps_position.epv < epv_threshold) + && hrt_elapsed_time((hrt_abstime *)&gps_position.timestamp) < 1e6) { - /* check if GPS is ok */ - if (!status_flags.circuit_breaker_engaged_gpsfailure_check) { - bool gpsIsNoisy = gps_position.noise_per_ms > 0 && gps_position.noise_per_ms < COMMANDER_MAX_GPS_NOISE; - - //Check if GPS receiver is too noisy while we are disarmed - if (!armed.armed && gpsIsNoisy) { - if (!status_flags.gps_failure) { - mavlink_log_critical(&mavlink_log_pub, "GPS signal noisy"); - set_tune_override(TONE_GPS_WARNING_TUNE); - - //GPS suffers from signal jamming or excessive noise, disable GPS-aided flight - status_flags.gps_failure = true; - status_changed = true; - } + /* set reference for global coordinates <--> local coordiantes conversion and map_projection */ + globallocalconverter_init(gps_position.lat * 1.0e-7, gps_position.lon * 1.0e-7, gps_position.alt * 1.0e-3f, hrt_absolute_time()); } - - // Check fix type and data freshness - if (gps_position.fix_type >= 3 && hrt_elapsed_time(&gps_position.timestamp) < FAILSAFE_DEFAULT_TIMEOUT) { - /* handle the case where gps was regained */ - if (status_flags.gps_failure && !gpsIsNoisy) { - status_flags.gps_failure = false; - status_changed = true; - if (status_flags.condition_home_position_valid) { - mavlink_log_critical(&mavlink_log_pub, "GPS fix regained"); - } - } - - } else if (!status_flags.gps_failure) { - status_flags.gps_failure = true; - status_changed = true; - if (status.arming_state == vehicle_status_s::ARMING_STATE_ARMED) { - mavlink_log_critical(&mavlink_log_pub, "GPS fix lost"); - } - } - } /* start mission result check */ @@ -2890,7 +2845,7 @@ Commander::run() internal_state.main_state != commander_state_s::MAIN_STATE_STAB && internal_state.main_state != commander_state_s::MAIN_STATE_ALTCTL && internal_state.main_state != commander_state_s::MAIN_STATE_POSCTL && - ((status.data_link_lost && status_flags.gps_failure))) { + status.data_link_lost) { armed.force_failsafe = true; status_changed = true; @@ -2915,7 +2870,7 @@ Commander::run() internal_state.main_state == commander_state_s::MAIN_STATE_STAB || internal_state.main_state == commander_state_s::MAIN_STATE_ALTCTL || internal_state.main_state == commander_state_s::MAIN_STATE_POSCTL) && - ((status.rc_signal_lost && status_flags.gps_failure))) { + status.rc_signal_lost) { armed.force_failsafe = true; status_changed = true; diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 3274893c1c..50d9f72477 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -71,7 +71,6 @@ using namespace DriverFramework; static const char reason_no_rc[] = "no RC"; static const char reason_no_offboard[] = "no offboard"; static const char reason_no_rc_and_no_offboard[] = "no RC and no offboard"; -static const char reason_no_gps[] = "no gps"; static const char reason_no_local_position[] = "no local position"; static const char reason_no_global_position[] = "no global position"; static const char reason_no_datalink[] = "no datalink"; @@ -696,18 +695,7 @@ bool set_nav_state(struct vehicle_status_s *status, * - if we have vtol transition failure * - depending on datalink, RC and if the mission is finished */ - if (status_flags->gps_failure) { - if (status->is_rotary_wing) { - status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND; - - } else { - // TODO: FW position controller doesn't run without condition_global_position_valid - status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDGPSFAIL; - } - - enable_failsafe(status, old_failsafe, mavlink_log_pub, reason_no_gps); - - } else if (is_armed && check_invalid_pos_nav_state(status, old_failsafe, mavlink_log_pub, status_flags, false, true)) { + if (is_armed && check_invalid_pos_nav_state(status, old_failsafe, mavlink_log_pub, status_flags, false, true)) { // nothing to do - everything done in check_invalid_pos_nav_state } else if (status->engine_failure) { status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL; @@ -747,16 +735,6 @@ bool set_nav_state(struct vehicle_status_s *status, if (status->engine_failure) { status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL; - } else if (status_flags->gps_failure) { - if (status->is_rotary_wing) { - status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND; - - } else { - status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDGPSFAIL; - } - - enable_failsafe(status, old_failsafe, mavlink_log_pub, reason_no_gps); - } else if (is_armed && check_invalid_pos_nav_state(status, old_failsafe, mavlink_log_pub, status_flags, false, true)) { // nothing to do - everything done in check_invalid_pos_nav_state } else if (status->data_link_lost && data_link_loss_act_configured && !landed) { @@ -791,17 +769,6 @@ bool set_nav_state(struct vehicle_status_s *status, if (status->engine_failure) { status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL; - } else if (status_flags->gps_failure) { - if (status->is_rotary_wing) { - status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND; - - } else { - // TODO: FW position controller doesn't run without condition_global_position_valid - status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDGPSFAIL; - } - - enable_failsafe(status, old_failsafe, mavlink_log_pub, reason_no_gps); - } else if (is_armed && check_invalid_pos_nav_state(status, old_failsafe, mavlink_log_pub, status_flags, false, true)) { // nothing to do - everything done in check_invalid_pos_nav_state } else {