Skip to content

Commit

Permalink
gps_failsafes: temporary parameter to force gps lost events
Browse files Browse the repository at this point in the history
  • Loading branch information
@blandry
blandry committed Aug 24, 2016
1 parent 4a0d5ad commit 4f0272b
Show file tree
Hide file tree
Showing 3 changed files with 50 additions and 35 deletions.
6 changes: 6 additions & 0 deletions libraries/AP_GPS/AP_GPS.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -103,6 +103,12 @@ const AP_Param::GroupInfo AP_GPS::var_info[] PROGMEM = {
// @Description: Set to a non-zero value to override the GPS receivers reported speed accuracy in cm/s
// @User: Advanced
AP_GROUPINFO("SPD_ERR_OW", 9, AP_GPS, _spd_err_ow, 0),

// @Param: UBLOX_NO_FIX
// @DisplayName: uBlox no fix force flag
// @Description: Set to a non-zero value to force uBlox receivers to report no gps fix
// @User: Advanced
AP_GROUPINFO("UBLOX_NO_FIX", 10, AP_GPS, _ublox_no_fix, 0),

AP_GROUPEND
};
Expand Down
1 change: 1 addition & 0 deletions libraries/AP_GPS/AP_GPS.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -334,6 +334,7 @@ class AP_GPS
AP_Int8 _sbas_mode;
AP_Int8 _min_elevation;
AP_Int16 _spd_err_ow;
AP_Int16 _ublox_no_fix;

// handle sending of initialisation strings to the GPS
void send_blob_start(uint8_t instance, const prog_char *_blob, uint16_t size);
Expand Down
78 changes: 43 additions & 35 deletions libraries/AP_GPS/AP_GPS_UBLOX.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -480,28 +480,30 @@ AP_GPS_UBLOX::_parse_gps(void)

switch (_msg_id) {
case MSG_POSLLH:
Debug("MSG_POSLLH next_fix=%u", next_fix);
_last_pos_time = _buffer.posllh.time;
state.location.lng = _buffer.posllh.longitude;
state.location.lat = _buffer.posllh.latitude;
state.location.alt = _buffer.posllh.altitude_msl / 10;
state.status = next_fix;
_new_position = true;
#if UBLOX_FAKE_3DLOCK
state.location.lng = 1491652300L;
state.location.lat = -353632610L;
state.location.alt = 58400;
#endif
state.horizontal_accuracy = _buffer.posllh.horizontal_accuracy*1.0e-3f;
state.vertical_accuracy = _buffer.posllh.vertical_accuracy*1.0e-3f;
state.have_horizontal_accuracy = true;
state.have_vertical_accuracy = true;
break;
if (!gps._ublox_no_fix) {
Debug("MSG_POSLLH next_fix=%u", next_fix);
_last_pos_time = _buffer.posllh.time;
state.location.lng = _buffer.posllh.longitude;
state.location.lat = _buffer.posllh.latitude;
state.location.alt = _buffer.posllh.altitude_msl / 10;
state.status = next_fix;
_new_position = true;
#if UBLOX_FAKE_3DLOCK
state.location.lng = 1491652300L;
state.location.lat = -353632610L;
state.location.alt = 58400;
#endif
state.horizontal_accuracy = _buffer.posllh.horizontal_accuracy*1.0e-3f;
state.vertical_accuracy = _buffer.posllh.vertical_accuracy*1.0e-3f;
state.have_horizontal_accuracy = true;
state.have_vertical_accuracy = true;
}
break;
case MSG_STATUS:
Debug("MSG_STATUS fix_status=%u fix_type=%u",
_buffer.status.fix_status,
_buffer.status.fix_type);
if (_buffer.status.fix_status & NAV_STATUS_FIX_VALID) {
if (_buffer.status.fix_status & NAV_STATUS_FIX_VALID && !gps._ublox_no_fix) {
if( _buffer.status.fix_type == AP_GPS_UBLOX::FIX_3D) {
next_fix = AP_GPS::GPS_OK_FIX_3D;
}else if (_buffer.status.fix_type == AP_GPS_UBLOX::FIX_2D) {
Expand Down Expand Up @@ -529,7 +531,7 @@ AP_GPS_UBLOX::_parse_gps(void)
Debug("MSG_SOL fix_status=%u fix_type=%u",
_buffer.solution.fix_status,
_buffer.solution.fix_type);
if (_buffer.solution.fix_status & NAV_STATUS_FIX_VALID) {
if (_buffer.solution.fix_status & NAV_STATUS_FIX_VALID && !gps._ublox_no_fix) {
if( _buffer.solution.fix_type == AP_GPS_UBLOX::FIX_3D) {
next_fix = AP_GPS::GPS_OK_FIX_3D;
}else if (_buffer.solution.fix_type == AP_GPS_UBLOX::FIX_2D) {
Expand All @@ -542,7 +544,11 @@ AP_GPS_UBLOX::_parse_gps(void)
next_fix = AP_GPS::NO_FIX;
state.status = AP_GPS::NO_FIX;
}
state.num_sats = _buffer.solution.satellites;
if (gps._ublox_no_fix) {
state.num_sats = 0;
} else {
state.num_sats = _buffer.solution.satellites;
}
if (next_fix >= AP_GPS::GPS_OK_FIX_2D) {
state.last_gps_time_ms = hal.scheduler->millis();
if (state.time_week == _buffer.solution.week &&
Expand All @@ -564,22 +570,24 @@ AP_GPS_UBLOX::_parse_gps(void)
#endif
break;
case MSG_VELNED:
Debug("MSG_VELNED");
_last_vel_time = _buffer.velned.time;
state.ground_speed = _buffer.velned.speed_2d*0.01f; // m/s
state.ground_course_cd = _buffer.velned.heading_2d / 1000; // Heading 2D deg * 100000 rescaled to deg * 100
state.have_vertical_velocity = true;
state.velocity.x = _buffer.velned.ned_north * 0.01f;
state.velocity.y = _buffer.velned.ned_east * 0.01f;
state.velocity.z = _buffer.velned.ned_down * 0.01f;
state.have_speed_accuracy = true;
// Allow overwrite of reported speed accuracy to assist with testing GPS glitch logic
if (gps._spd_err_ow == 0) {
state.speed_accuracy = _buffer.velned.speed_accuracy*0.01f;
} else {
state.speed_accuracy = gps._spd_err_ow*0.01f;
if (!gps._ublox_no_fix) {
Debug("MSG_VELNED");
_last_vel_time = _buffer.velned.time;
state.ground_speed = _buffer.velned.speed_2d*0.01f; // m/s
state.ground_course_cd = _buffer.velned.heading_2d / 1000; // Heading 2D deg * 100000 rescaled to deg * 100
state.have_vertical_velocity = true;
state.velocity.x = _buffer.velned.ned_north * 0.01f;
state.velocity.y = _buffer.velned.ned_east * 0.01f;
state.velocity.z = _buffer.velned.ned_down * 0.01f;
state.have_speed_accuracy = true;
// Allow overwrite of reported speed accuracy to assist with testing GPS glitch logic
if (gps._spd_err_ow == 0) {
state.speed_accuracy = _buffer.velned.speed_accuracy*0.01f;
} else {
state.speed_accuracy = gps._spd_err_ow*0.01f;
}
_new_speed = true;
}
_new_speed = true;
break;
#if UBLOX_VERSION_AUTODETECTION
case MSG_NAV_SVINFO:
Expand Down

0 comments on commit 4f0272b

Please sign in to comment.