fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-25 07:23:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

commander remove gps receiver checks

Browse Source
This commit is contained in:
Daniel Agar
2018-02-19 15:28:47 +01:00
committed by Lorenz Meier
parent a6e863ac89
commit 4e45d7959c
4 changed files with 15 additions and 138 deletions
Download Patch File Download Diff File Expand all files Collapse all files
msg/vehicle_status_flags.msg
-1
View File
@@ -31,5 +31,4 @@ bool offboard_control_loss_timeout # true if offboard is lost for
bool rc_signal_found_once bool rc_signal_found_once
bool rc_input_blocked # set if RC input should be ignored temporarily bool rc_input_blocked # set if RC input should be ignored temporarily
bool vtol_transition_failure # Set to true if vtol transition failed 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 bool usb_connected # status of the USB power supply
src/modules/commander/PreflightCheck.cpp
-44
View File
@@ -448,42 +448,6 @@ out:
return success; 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) 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 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 ---- */ /* ---- Navigation EKF ---- */
// only check EKF2 data if EKF2 is selected as the estimator and GNSS checking is enabled // only check EKF2 data if EKF2 is selected as the estimator and GNSS checking is enabled
int32_t estimator_type; int32_t estimator_type;
src/modules/commander/commander.cpp
+14 -59
View File
@@ -136,8 +136,6 @@ typedef enum VEHICLE_MODE_FLAG
VEHICLE_MODE_FLAG_ENUM_END=129, /* | */ VEHICLE_MODE_FLAG_ENUM_END=129, /* | */
} VEHICLE_MODE_FLAG; } 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 */ /* Decouple update interval and hysteresis counters, all depends on intervals */
#define COMMANDER_MONITORING_INTERVAL 10000 #define COMMANDER_MONITORING_INTERVAL 10000
#define COMMANDER_MONITORING_LOOPSPERMSEC (1/(COMMANDER_MONITORING_INTERVAL/1000.0f)) #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_velocity_valid = false;
status_flags.condition_local_altitude_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 */ /* publish initial state */
status_pub = orb_advertise(ORB_ID(vehicle_status), &status); status_pub = orb_advertise(ORB_ID(vehicle_status), &status);
@@ -1457,10 +1448,6 @@ Commander::run()
/* Subscribe to GPS topic */ /* Subscribe to GPS topic */
int gps_sub = orb_subscribe(ORB_ID(vehicle_gps_position)); 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 */ /* Subscribe to differential pressure topic */
int diff_pres_sub = orb_subscribe(ORB_ID(differential_pressure)); int diff_pres_sub = orb_subscribe(ORB_ID(differential_pressure));
@@ -2339,54 +2326,22 @@ Commander::run()
orb_check(gps_sub, &updated); orb_check(gps_sub, &updated);
if (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); orb_copy(ORB_ID(vehicle_gps_position), gps_sub, &gps_position);
}
/* Initialize map projection if gps is valid */ /* Initialize map projection if gps is valid */
if (!map_projection_global_initialized() if (!map_projection_global_initialized()
&& (gps_position.eph < eph_threshold) && (gps_position.eph < eph_threshold)
&& (gps_position.epv < epv_threshold) && (gps_position.epv < epv_threshold)
&& hrt_elapsed_time((hrt_abstime *)&gps_position.timestamp) < 1e6) { && 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());
}
/* check if GPS is ok */ /* set reference for global coordinates <--> local coordiantes conversion and map_projection */
if (!status_flags.circuit_breaker_engaged_gpsfailure_check) { globallocalconverter_init(gps_position.lat * 1.0e-7, gps_position.lon * 1.0e-7, gps_position.alt * 1.0e-3f, hrt_absolute_time());
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;
}
} }
// 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 */ /* 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_STAB &&
internal_state.main_state != commander_state_s::MAIN_STATE_ALTCTL && internal_state.main_state != commander_state_s::MAIN_STATE_ALTCTL &&
internal_state.main_state != commander_state_s::MAIN_STATE_POSCTL && 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; armed.force_failsafe = true;
status_changed = 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_STAB ||
internal_state.main_state == commander_state_s::MAIN_STATE_ALTCTL || internal_state.main_state == commander_state_s::MAIN_STATE_ALTCTL ||
internal_state.main_state == commander_state_s::MAIN_STATE_POSCTL) && 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; armed.force_failsafe = true;
status_changed = true; status_changed = true;
src/modules/commander/state_machine_helper.cpp
+1 -34
View File
@@ -71,7 +71,6 @@ using namespace DriverFramework;
static const char reason_no_rc[] = "no RC"; static const char reason_no_rc[] = "no RC";
static const char reason_no_offboard[] = "no offboard"; 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_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_local_position[] = "no local position";
static const char reason_no_global_position[] = "no global position"; static const char reason_no_global_position[] = "no global position";
static const char reason_no_datalink[] = "no datalink"; 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 * - if we have vtol transition failure
* - depending on datalink, RC and if the mission is finished */ * - depending on datalink, RC and if the mission is finished */
if (status_flags->gps_failure) { if (is_armed && check_invalid_pos_nav_state(status, old_failsafe, mavlink_log_pub, status_flags, false, true)) {
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 // nothing to do - everything done in check_invalid_pos_nav_state
} else if (status->engine_failure) { } else if (status->engine_failure) {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL; 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) { if (status->engine_failure) {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL; 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)) { } 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 // nothing to do - everything done in check_invalid_pos_nav_state
} else if (status->data_link_lost && data_link_loss_act_configured && !landed) { } 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) { if (status->engine_failure) {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL; 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)) { } 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 // nothing to do - everything done in check_invalid_pos_nav_state
} else { } else {