fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +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-18 17:24:01 -05:00 committed by Lorenz Meier
parent a6e863ac89
commit 4e45d7959c
4 changed files with 15 additions and 138 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
msg/vehicle_status_flags.msg
View File

@ -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

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

@ -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;

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

@ -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;

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

@ -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 {