Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_GPS: u-blox verify ublox settings 

The ublox driver will now continuosly poll for the settings from the GPS and correct any that are found to be in correct.
This status is then reported to the arming library as an additional arming check, allowing the user to be sure that the
gps is correctly configured before using it. If a user has a GPS2 configured that is not present they will fail the arming
checks until after they have disabled the second GPS.

2 new parameters were introduced as well:
  -GPS_AUTO_CONFIG: Will not request any configuration packets to attempt to change them. (If the packet is recieved then
    a update will be sent to it, but in testing this scenario never occured. This is set to 1 or 0 to change the setting.
    (Defaults to 1 enabling auto config)
  -GPS_GNSS_MODE2: Behaves the same way as GPS_GNSS_MODE but only applies to the second GPS.

GPS drivers are now allowed 2 seconds of non responsiveness before being unloaded
This commit is contained in:
Michael du Breuil 2016-02-02 00:58:33 -07:00 committed by Tom Pittenger
parent 7259ac33f0
commit 4251ee0e4b
5 changed files with 623 additions and 230 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

37
libraries/AP_GPS/AP_GPS.cpp
View File

@ -42,7 +42,6 @@ const AP_Param::GroupInfo AP_GPS::var_info[] = {
// @DisplayName: Navigation filter setting
// @Description: Navigation filter engine setting
// @Values: 0:Portable,2:Stationary,3:Pedestrian,4:Automotive,5:Sea,6:Airborne1G,7:Airborne2G,8:Airborne4G
// @RebootRequired: True
AP_GROUPINFO("NAVFILTER", 2, AP_GPS, _navfilter, GPS_ENGINE_AIRBORNE_4G),
// @Param: AUTO_SWITCH
@ -65,7 +64,6 @@ const AP_Param::GroupInfo AP_GPS::var_info[] = {
// @Description: This sets the SBAS (satellite based augmentation system) mode if available on this GPS. If set to 2 then the SBAS mode is not changed in the GPS. Otherwise the GPS will be reconfigured to enable/disable SBAS. Disabling SBAS may be worthwhile in some parts of the world where an SBAS signal is available but the baseline is too long to be useful.
// @Values: 0:Disabled,1:Enabled,2:NoChange
// @User: Advanced
// @RebootRequired: True
AP_GROUPINFO("SBAS_MODE", 5, AP_GPS, _sbas_mode, 2),
// @Param: MIN_ELEV
@ -74,7 +72,6 @@ const AP_Param::GroupInfo AP_GPS::var_info[] = {
// @Range: -100 90
// @Units: Degrees
// @User: Advanced
// @RebootRequired: True
AP_GROUPINFO("MIN_ELEV", 6, AP_GPS, _min_elevation, -100),
// @Param: INJECT_TO
@ -99,12 +96,11 @@ const AP_Param::GroupInfo AP_GPS::var_info[] = {
// @Param: GNSS_MODE
// @DisplayName: GNSS system configuration
// @Description: Bitmask for what GNSS system to use (all unchecked or zero to leave GPS as configured)
// @Description: Bitmask for what GNSS system to use on the first GPS (all unchecked or zero to leave GPS as configured)
// @Values: 0:Leave as currently configured, 1:GPS-NoSBAS, 3:GPS+SBAS, 4:Galileo-NoSBAS, 6:Galileo+SBAS, 8:Beidou, 51:GPS+IMES+QZSS+SBAS (Japan Only), 64:GLONASS, 66:GLONASS+SBAS, 67:GPS+GLONASS+SBAS
// @Bitmask: 0:GPS,1:SBAS,2:Galileo,3:Beidou,4:IMES,5:QZSS,6:GLOSNASS
// @User: Advanced
// @RebootRequired: True
AP_GROUPINFO("GNSS_MODE", 10, AP_GPS, _gnss_mode, 0),
AP_GROUPINFO("GNSS_MODE", 10, AP_GPS, _gnss_mode[0], 0),
// @Param: SAVE_CFG
// @DisplayName: Save GPS configuration
@ -113,6 +109,21 @@ const AP_Param::GroupInfo AP_GPS::var_info[] = {
// @User: Advanced
AP_GROUPINFO("SAVE_CFG", 11, AP_GPS, _save_config, 0),
// @Param: GNSS_MODE2
// @DisplayName: GNSS system configuration
// @Description: Bitmask for what GNSS system to use on the second GPS (all unchecked or zero to leave GPS as configured)
// @Values: 0:Leave as currently configured, 1:GPS-NoSBAS, 3:GPS+SBAS, 4:Galileo-NoSBAS, 6:Galileo+SBAS, 8:Beidou, 51:GPS+IMES+QZSS+SBAS (Japan Only), 64:GLONASS, 66:GLONASS+SBAS, 67:GPS+GLONASS+SBAS
// @Bitmask: 0:GPS,1:SBAS,2:Galileo,3:Beidou,4:IMES,5:QZSS,6:GLOSNASS
// @User: Advanced
AP_GROUPINFO("GNSS_MODE2", 12, AP_GPS, _gnss_mode[1], 0),
// @Param: AUTO_CONFIG
// @DisplayName: Automatic GPS configuration
// @Description: Controls if the autopilot should automatically configure the GPS based on the parameters and default settings
// @Values: 0:Disables automatic configuration,1:Enable automatic configuration
// @User: Advanced
AP_GROUPINFO("AUTO_CONFIG", 13, AP_GPS, _auto_config, 1),
AP_GROUPEND
};
@ -355,11 +366,11 @@ AP_GPS::update_instance(uint8_t instance)
bool result = drivers[instance]->read();
uint32_t tnow = AP_HAL::millis();
// if we did not get a message, and the idle timer of 1.2 seconds
// if we did not get a message, and the idle timer of 2 seconds
// has expired, re-initialise the GPS. This will cause GPS
// detection to run again
if (!result) {
if (tnow - timing[instance].last_message_time_ms > 1200) {
if (tnow - timing[instance].last_message_time_ms > 2000) {
// free the driver before we run the next detection, so we
// don't end up with two allocated at any time
delete drivers[instance];
@ -580,3 +591,13 @@ AP_GPS::send_mavlink_gps2_rtk(mavlink_channel_t chan)
drivers[1]->send_mavlink_gps_rtk(chan);
}
}
uint8_t
AP_GPS::first_unconfigured_gps(void) const {
for(int i = 0; i < GPS_MAX_INSTANCES; i++) {
if(_type[i] != GPS_TYPE_NONE && (drivers[i] == NULL || !drivers[i]->is_configured())) {
return i;
}
}
return GPS_ALL_CONFIGURED;
}

10
libraries/AP_GPS/AP_GPS.h
View File

@ -96,6 +96,10 @@ public:
GPS_ENGINE_AIRBORNE_4G = 8
};
enum GPS_Config {
GPS_ALL_CONFIGURED = 255
};
/*
The GPS_State structure is filled in by the backend driver as it
parses each message from the GPS.
@ -317,8 +321,9 @@ public:
AP_Int8 _sbas_mode;
AP_Int8 _min_elevation;
AP_Int8 _raw_data;
AP_Int8 _gnss_mode;
AP_Int8 _gnss_mode[2];
AP_Int8 _save_config;
AP_Int8 _auto_config;
// handle sending of initialisation strings to the GPS
void send_blob_start(uint8_t instance, const char *_blob, uint16_t size);
@ -338,6 +343,9 @@ public:
void send_mavlink_gps_rtk(mavlink_channel_t chan);
void send_mavlink_gps2_rtk(mavlink_channel_t chan);
// Returns the index of the first unconfigured GPS (returns GPS_ALL_CONFIGURED if all instances report as being configured)
uint8_t first_unconfigured_gps(void) const;
private:
struct GPS_timing {
// the time we got our last fix in system milliseconds

530
libraries/AP_GPS/AP_GPS_UBLOX.cpp
View File

@ -21,13 +21,15 @@
//
#include "AP_GPS.h"
#include "AP_GPS_UBLOX.h"
#include <AP_HAL/Util.h>
#include <DataFlash/DataFlash.h>
#include <GCS_MAVLink/GCS.h>
#if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_NAVIO || \
CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_BH
#define UBLOX_VERSION_AUTODETECTION 1
#define UBLOX_SPEED_CHANGE 1
#else
#define UBLOX_VERSION_AUTODETECTION 0
#define UBLOX_SPEED_CHANGE 0
#endif
@ -48,100 +50,258 @@ AP_GPS_UBLOX::AP_GPS_UBLOX(AP_GPS &_gps, AP_GPS::GPS_State &_state, AP_HAL::UART
_msg_id(0),
_payload_length(0),
_payload_counter(0),
_fix_count(0),
_class(0),
_cfg_saved(false),
_last_cfg_sent_time(0),
_num_cfg_save_tries(0),
_last_config_time(0),
_delay_time(0),
_next_message(STEP_RATE_NAV),
_ublox_port(255),
_have_version(false),
_unconfigured_messages(CONFIG_ALL),
_hardware_generation(0),
_new_position(0),
_new_speed(0),
need_rate_update(false),
_disable_counter(0),
next_fix(AP_GPS::NO_FIX),
rate_update_step(0),
_last_5hz_time(0),
noReceivedHdop(true)
{
// stop any config strings that are pending
gps.send_blob_start(state.instance, NULL, 0);
// configure the GPS for the messages we want
_configure_gps();
_last_5hz_time = AP_HAL::millis();
// start the process of updating the GPS rates
_request_next_config();
}
/*
send the next step of rate updates to the GPS. This reconfigures the
GPS on the fly to have the right message rates. It needs to be
careful to only send a message if there is sufficient buffer space
available on the serial port to avoid it blocking the CPU
*/
void
AP_GPS_UBLOX::send_next_rate_update(void)
AP_GPS_UBLOX::_request_next_config(void)
{
// don't request config if we shouldn't configure the GPS
if (gps._auto_config == 0) {
return;
}
// Ensure there is enough space for the largest possible outgoing message
if (port->txspace() < (int16_t)(sizeof(struct ubx_header)+sizeof(struct ubx_cfg_nav_rate)+2)) {
// not enough space - do it next time
return;
}
//hal.console->printf("next_rate: %u\n", (unsigned)rate_update_step);
Debug("Unconfigured messages: %d Current message: %d\n", _unconfigured_messages, _next_message);
switch (rate_update_step++) {
case 0:
_configure_navigation_rate(200);
switch (_next_message++) {
case STEP_RATE_NAV:
_configure_rate();
break;
case 1:
_configure_message_rate(CLASS_NAV, MSG_POSLLH, 1); // 28+8 bytes
case STEP_RATE_POSLLH:
if(!_configure_message_rate(CLASS_NAV, MSG_POSLLH, RATE_POSLLH)) {
_next_message--;
}
break;
case 2:
_configure_message_rate(CLASS_NAV, MSG_STATUS, 1); // 16+8 bytes
case STEP_RATE_VELNED:
if(!_configure_message_rate(CLASS_NAV, MSG_VELNED, RATE_VELNED)) {
_next_message--;
}
break;
case 3:
_configure_message_rate(CLASS_NAV, MSG_SOL, 1); // 52+8 bytes
case STEP_PORT:
_request_port();
break;
case 4:
_configure_message_rate(CLASS_NAV, MSG_VELNED, 1); // 36+8 bytes
case STEP_POLL_SVINFO:
// not required once we know what generation we are on
if(_hardware_generation == 0) {
_send_message(CLASS_NAV, MSG_NAV_SVINFO, 0, 0);
}
break;
case 5:
_configure_message_rate(CLASS_NAV, MSG_DOP, 1); // 18+8 bytes
case STEP_POLL_SBAS:
_send_message(CLASS_CFG, MSG_CFG_SBAS, NULL, 0);
break;
case 6:
#if UBLOX_HW_LOGGING
// gather MON_HW at 0.5Hz
_configure_message_rate(CLASS_MON, MSG_MON_HW, 2); // 64+8 bytes
#endif
case STEP_POLL_NAV:
_send_message(CLASS_CFG, MSG_CFG_NAV_SETTINGS, NULL, 0);
break;
case 7:
#if UBLOX_HW_LOGGING
// gather MON_HW2 at 0.5Hz
_configure_message_rate(CLASS_MON, MSG_MON_HW2, 2); // 24+8 bytes
#endif
case STEP_POLL_GNSS:
_send_message(CLASS_CFG, MSG_CFG_GNSS, NULL, 0);
break;
case 8:
case STEP_NAV_RATE:
_request_navigation_rate();
break;
case STEP_POSLLH:
if(!_request_message_rate(CLASS_NAV, MSG_POSLLH)) {
_next_message--;
}
break;
case STEP_STATUS:
if(!_request_message_rate(CLASS_NAV, MSG_STATUS)) {
_next_message--;
}
break;
case STEP_SOL:
if(!_request_message_rate(CLASS_NAV, MSG_SOL)) {
_next_message--;
}
break;
case STEP_VELNED:
if(!_request_message_rate(CLASS_NAV, MSG_VELNED)) {
_next_message--;
}
break;
case STEP_DOP:
if(! _request_message_rate(CLASS_NAV, MSG_DOP)) {
_next_message--;
}
break;
case STEP_MON_HW:
if(!_request_message_rate(CLASS_MON, MSG_MON_HW)) {
_next_message--;
}
break;
case STEP_MON_HW2:
if(!_request_message_rate(CLASS_MON, MSG_MON_HW2)) {
_next_message--;
}
break;
case STEP_RAW:
#if UBLOX_RXM_RAW_LOGGING
_configure_message_rate(CLASS_RXM, MSG_RXM_RAW, gps._raw_data);
if(gps._raw_data == 0) {
_unconfigured_messages &= ~CONFIG_RATE_RAW;
} else if(!_request_message_rate(CLASS_RXM, MSG_RXM_RAW)) {
_next_message--;
}
#else
_unconfigured_messages & = ~COONFIG_RATE_RAW;
#endif
break;
case 9:
case STEP_RAWX:
#if UBLOX_RXM_RAW_LOGGING
_configure_message_rate(CLASS_RXM, MSG_RXM_RAWX, gps._raw_data);
if(gps._raw_data == 0) {
_unconfigured_messages &= ~CONFIG_RATE_RAW;
} else if(!_request_message_rate(CLASS_RXM, MSG_RXM_RAWX)) {
_next_message--;
}
#else
_unconfigured_messages & = ~COONFIG_RATE_RAW;
#endif
break;
case 10:
#if UBLOX_VERSION_AUTODETECTION
case STEP_VERSION:
if(!_have_version && !hal.util->get_soft_armed()) {
_request_version();
#endif
}
// no need to send the initial rates, move to checking only
_next_message = STEP_PORT;
break;
default:
need_rate_update = false;
rate_update_step = 0;
// this case should never be reached, do a full reset if it is hit
_next_message = STEP_RATE_NAV;
break;
}
}
void
AP_GPS_UBLOX::_verify_rate(uint8_t msg_class, uint8_t msg_id, uint8_t rate) {
switch(msg_class) {
case CLASS_NAV:
switch(msg_id) {
case MSG_POSLLH:
if(rate == RATE_POSLLH) {
_unconfigured_messages &= ~CONFIG_RATE_POSLLH;
} else {
_configure_message_rate(msg_class, msg_id, RATE_POSLLH);
_unconfigured_messages |= CONFIG_RATE_POSLLH;
}
break;
case MSG_STATUS:
if(rate == RATE_STATUS) {
_unconfigured_messages &= ~CONFIG_RATE_STATUS;
} else {
_configure_message_rate(msg_class, msg_id, RATE_STATUS);
_unconfigured_messages |= CONFIG_RATE_STATUS;
}
break;
case MSG_SOL:
if(rate == RATE_SOL) {
_unconfigured_messages &= ~CONFIG_RATE_SOL;
} else {
_configure_message_rate(msg_class, msg_id, RATE_SOL);
_unconfigured_messages |= CONFIG_RATE_SOL;
}
break;
case MSG_VELNED:
if(rate == RATE_VELNED) {
_unconfigured_messages &= ~CONFIG_RATE_VELNED;
} else {
_configure_message_rate(msg_class, msg_id, RATE_VELNED);
_unconfigured_messages |= CONFIG_RATE_VELNED;
}
break;
case MSG_DOP:
if(rate == RATE_DOP) {
_unconfigured_messages &= ~CONFIG_RATE_DOP;
} else {
_configure_message_rate(msg_class, msg_id, RATE_DOP);
_unconfigured_messages |= CONFIG_RATE_DOP;
}
break;
}
break;
case CLASS_MON:
switch(msg_id) {
case MSG_MON_HW:
if(rate == RATE_HW) {
_unconfigured_messages &= ~CONFIG_RATE_MON_HW;
} else {
_configure_message_rate(msg_class, msg_id, RATE_HW);
_unconfigured_messages |= CONFIG_RATE_MON_HW;
}
break;
case MSG_MON_HW2:
if(rate == RATE_HW2) {
_unconfigured_messages &= ~CONFIG_RATE_MON_HW2;
} else {
_configure_message_rate(msg_class, msg_id, RATE_HW2);
_unconfigured_messages |= CONFIG_RATE_MON_HW2;
}
break;
}
break;
#if UBLOX_RXM_RAW_LOGGING
case CLASS_RXM:
switch(msg_id) {
case MSG_RXM_RAW:
if(rate == gps._raw_data) {
_unconfigured_messages &= ~CONFIG_RATE_RAW;
} else {
_configure_message_rate(msg_class, msg_id, gps._raw_data);
_unconfigured_messages |= CONFIG_RATE_RAW;
}
break;
case MSG_RXM_RAWX:
if(rate == gps._raw_data) {
_unconfigured_messages &= ~CONFIG_RATE_RAW;
} else {
_configure_message_rate(msg_class, msg_id, gps._raw_data);
_unconfigured_messages |= CONFIG_RATE_RAW;
}
break;
}
break;
#endif // UBLOX_RXM_RAW_LOGGING
}
}
// Requests the ublox driver to identify what port we are using to communicate
void
AP_GPS_UBLOX::_request_port(void)
{
if (port->txspace() < (int16_t)(sizeof(struct ubx_header)+2)) {
// not enough space - do it next time
return;
}
_send_message(CLASS_CFG, MSG_CFG_PRT, NULL, 0);
}
// Ensure there is enough space for the largest possible outgoing message
// Process bytes available from the stream
//
// The stream is assumed to contain only messages we recognise. If it
@ -159,12 +319,26 @@ AP_GPS_UBLOX::read(void)
bool parsed = false;
uint32_t millis_now = AP_HAL::millis();
if (need_rate_update) {
send_next_rate_update();
}else if(!_cfg_saved && gps._save_config && _num_cfg_save_tries < 5 && (millis_now - _last_cfg_sent_time) > 5000) { //save the configuration sent until now
_last_cfg_sent_time = millis_now;
// walk through the gps configuration at 1 message per second
if (millis_now - _last_config_time >= _delay_time) {
_request_next_config();
_last_config_time = millis_now;
if (_unconfigured_messages) { // send the updates faster until fully configured
if (_next_message < STEP_PORT) { // blast the initial settings out
_delay_time = 0;
} else {
_delay_time = 750;
}
} else {
_delay_time = 2000;
}
}
if(!_unconfigured_messages && gps._save_config &&
_num_cfg_save_tries < 5 && (millis_now - _last_cfg_sent_time) > 5000 &&
!hal.util->get_soft_armed()) {
//save the configuration sent until now
_save_cfg();
_num_cfg_save_tries++;
}
numc = port->available();
@ -275,8 +449,6 @@ AP_GPS_UBLOX::read(void)
}
// Private Methods /////////////////////////////////////////////////////////////
#if UBLOX_HW_LOGGING
void AP_GPS_UBLOX::log_mon_hw(void)
{
if (gps._DataFlash == NULL || !gps._DataFlash->logging_started()) {
@ -318,8 +490,6 @@ void AP_GPS_UBLOX::log_mon_hw2(void)
gps._DataFlash->WriteBlock(&pkt, sizeof(pkt));
}
#endif // UBLOX_HW_LOGGING
#if UBLOX_RXM_RAW_LOGGING
void AP_GPS_UBLOX::log_rxm_raw(const struct ubx_rxm_raw &raw)
{
@ -405,13 +575,50 @@ AP_GPS_UBLOX::_parse_gps(void)
if (_class == CLASS_ACK) {
Debug("ACK %u", (unsigned)_msg_id);
if(_msg_id == MSG_ACK_ACK && _buffer.ack.clsID == CLASS_CFG && _buffer.ack.msgID == MSG_CFG_CFG) {
if(_msg_id == MSG_ACK_ACK) {
switch(_buffer.ack.clsID) {
case CLASS_CFG:
switch(_buffer.ack.msgID) {
case MSG_CFG_CFG:
_cfg_saved = true;
break;
case MSG_CFG_GNSS:
_unconfigured_messages &= ~CONFIG_GNSS;
break;
case MSG_CFG_MSG:
// There is no way to know what MSG config was ack'ed, assume it was the last
// one requested. To verify it rerequest the last config we sent. If we miss
// the actual ack we will catch it next time through the poll loop, but that
// will be a good chunk of time later.
break;
case MSG_CFG_NAV_SETTINGS:
_unconfigured_messages &= ~CONFIG_NAV_SETTINGS;
break;
case MSG_CFG_RATE:
_unconfigured_messages &= ~CONFIG_RATE_NAV;
break;
case MSG_CFG_SBAS:
_unconfigured_messages &= ~CONFIG_SBAS;
break;
}
break;
case CLASS_MON:
switch(_buffer.ack.msgID) {
case MSG_MON_HW:
_unconfigured_messages &= ~CONFIG_RATE_MON_HW;
break;
case MSG_MON_HW2:
_unconfigured_messages &= ~CONFIG_RATE_MON_HW2;
break;
}
}
}
return false;
}
if (_class == CLASS_CFG && _msg_id == MSG_CFG_NAV_SETTINGS) {
if (_class == CLASS_CFG) {
switch(_msg_id) {
case MSG_CFG_NAV_SETTINGS:
Debug("Got settings %u min_elev %d drLimit %u\n",
(unsigned)_buffer.nav_settings.dynModel,
(int)_buffer.nav_settings.minElev,
@ -436,13 +643,16 @@ AP_GPS_UBLOX::_parse_gps(void)
_send_message(CLASS_CFG, MSG_CFG_NAV_SETTINGS,
&_buffer.nav_settings,
sizeof(_buffer.nav_settings));
_cfg_saved = false; //save configuration
_unconfigured_messages |= CONFIG_NAV_SETTINGS;
} else {
_unconfigured_messages &= ~CONFIG_NAV_SETTINGS;
}
return false;
}
#if UBLOX_GNSS_SETTINGS
if (_class == CLASS_CFG && _msg_id == MSG_CFG_GNSS && gps._gnss_mode != 0) {
case MSG_CFG_GNSS:
if (gps._gnss_mode[state.instance] != 0) {
struct ubx_cfg_gnss start_gnss = _buffer.gnss;
uint8_t gnssCount = 0;
Debug("Got GNSS Settings %u %u %u %u:\n",
(unsigned)_buffer.gnss.msgVer,
@ -460,14 +670,14 @@ AP_GPS_UBLOX::_parse_gps(void)
#endif
for(int i = 0; i < UBLOX_MAX_GNSS_CONFIG_BLOCKS; i++) {
if((gps._gnss_mode & (1 << i)) && i != GNSS_SBAS) {
if((gps._gnss_mode[state.instance] & (1 << i)) && i != GNSS_SBAS) {
gnssCount++;
}
}
for(int i = 0; i < _buffer.gnss.numConfigBlocks; i++) {
// Reserve an equal portion of channels for all enabled systems
if(gps._gnss_mode & (1 << _buffer.gnss.configBlock[i].gnssId)) {
if(gps._gnss_mode[state.instance] & (1 << _buffer.gnss.configBlock[i].gnssId)) {
if(GNSS_SBAS !=_buffer.gnss.configBlock[i].gnssId) {
_buffer.gnss.configBlock[i].resTrkCh = (_buffer.gnss.numTrkChHw - 3) / (gnssCount * 2);
_buffer.gnss.configBlock[i].maxTrkCh = _buffer.gnss.numTrkChHw;
@ -482,12 +692,20 @@ AP_GPS_UBLOX::_parse_gps(void)
_buffer.gnss.configBlock[i].flags = _buffer.gnss.configBlock[i].flags & 0xFFFFFFFE;
}
}
if (!memcmp(&start_gnss, &_buffer.gnss, sizeof(start_gnss))) {
_send_message(CLASS_CFG, MSG_CFG_GNSS, &_buffer.gnss, 4 + (8 * _buffer.gnss.numConfigBlocks));
return false;
_unconfigured_messages |= CONFIG_GNSS;
} else {
_unconfigured_messages &= ~CONFIG_GNSS;
}
} else {
_unconfigured_messages &= ~CONFIG_GNSS;
}
return false;
#endif
if (_class == CLASS_CFG && _msg_id == MSG_CFG_SBAS && gps._sbas_mode != 2) {
case MSG_CFG_SBAS:
if (gps._sbas_mode != 2) {
Debug("Got SBAS settings %u %u %u 0x%x 0x%x\n",
(unsigned)_buffer.sbas.mode,
(unsigned)_buffer.sbas.usage,
@ -499,26 +717,76 @@ AP_GPS_UBLOX::_parse_gps(void)
_send_message(CLASS_CFG, MSG_CFG_SBAS,
&_buffer.sbas,
sizeof(_buffer.sbas));
_cfg_saved = false;
_unconfigured_messages |= CONFIG_SBAS;
} else {
_unconfigured_messages &= ~CONFIG_SBAS;
}
} else {
_unconfigured_messages &= ~CONFIG_SBAS;
}
return false;
case MSG_CFG_MSG:
if(_payload_length == sizeof(ubx_cfg_msg_rate_6)) {
// can't verify the setting without knowing the port
// request the port again
if(_ublox_port >= UBLOX_MAX_PORTS) {
_request_port();
return false;
}
_verify_rate(_buffer.msg_rate_6.msg_class, _buffer.msg_rate_6.msg_id,
_buffer.msg_rate_6.rates[_ublox_port]);
} else {
_verify_rate(_buffer.msg_rate.msg_class, _buffer.msg_rate.msg_id,
_buffer.msg_rate.rate);
}
return false;
case MSG_CFG_PRT:
_ublox_port = _buffer.prt.portID;
return false;
case MSG_CFG_RATE:
if(_buffer.nav_rate.measure_rate_ms != MEASURE_RATE ||
_buffer.nav_rate.nav_rate != 1 ||
_buffer.nav_rate.timeref != 0) {
_configure_rate();
_unconfigured_messages |= CONFIG_RATE_NAV;
} else {
_unconfigured_messages &= ~CONFIG_RATE_NAV;
}
return false;
}
}
#if UBLOX_HW_LOGGING
if (_class == CLASS_MON) {
if (_msg_id == MSG_MON_HW) {
switch(_msg_id) {
case MSG_MON_HW:
if (_payload_length == 60 || _payload_length == 68) {
log_mon_hw();
}
} else if (_msg_id == MSG_MON_HW2) {
break;
case MSG_MON_HW2:
if (_payload_length == 28) {
log_mon_hw2();
}
} else {
break;
case MSG_MON_VER:
char version_buffer[46]; //17 string swVersion 30 hwVersion 10
_have_version = true;
hal.util->snprintf(version_buffer, sizeof(version_buffer),
"u-blox %d HW: %s SW: %s",
state.instance,
_buffer.mon_ver.hwVersion,
_buffer.mon_ver.swVersion);
GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_INFO, version_buffer);
if (gps._DataFlash != NULL && gps._DataFlash->logging_started()) {
gps._DataFlash->Log_Write_Message(version_buffer);
}
break;
default:
unexpected_message();
}
return false;
}
#endif // UBLOX_HW_LOGGING
#if UBLOX_RXM_RAW_LOGGING
if (_class == CLASS_RXM && _msg_id == MSG_RXM_RAW && gps._raw_data != 0) {
@ -652,31 +920,33 @@ AP_GPS_UBLOX::_parse_gps(void)
#endif
_new_speed = true;
break;
#if UBLOX_VERSION_AUTODETECTION
case MSG_NAV_SVINFO:
{
Debug("MSG_NAV_SVINFO\n");
static const uint8_t HardwareGenerationMask = 0x07;
uint8_t hardware_generation = _buffer.svinfo_header.globalFlags & HardwareGenerationMask;
switch (hardware_generation) {
_hardware_generation = _buffer.svinfo_header.globalFlags & HardwareGenerationMask;
switch (_hardware_generation) {
case UBLOX_5:
case UBLOX_6:
/*speed already configured */;
// only 7 and newer support CONFIG_GNSS
_unconfigured_messages &= ~CONFIG_GNSS;
break;
case UBLOX_7:
case UBLOX_M8:
#if UBLOX_SPEED_CHANGE
port->begin(4000000U);
Debug("Changed speed to 5Mhzfor SPI-driven UBlox\n");
Debug("Changed speed to 4Mhz for SPI-driven UBlox\n");
#endif
break;
default:
hal.console->printf("Wrong Ublox' Hardware Version%u\n", hardware_generation);
hal.console->printf("Wrong Ublox Hardware Version%u\n", _hardware_generation);
break;
};
_unconfigured_messages &= ~CONFIG_VERSION;
/* We don't need that anymore */
_configure_message_rate(CLASS_NAV, MSG_NAV_SVINFO, 0);
break;
}
#endif
default:
Debug("Unexpected NAV message 0x%02x", (unsigned)_msg_id);
if (++_disable_counter == 0) {
@ -690,26 +960,15 @@ AP_GPS_UBLOX::_parse_gps(void)
// this ensures we don't use stale data
if (_new_position && _new_speed && _last_vel_time == _last_pos_time) {
_new_speed = _new_position = false;
_fix_count++;
if ((AP_HAL::millis() - _last_5hz_time) > 15000U && !need_rate_update) {
// the GPS is running slow. It possibly browned out and
// restarted with incorrect parameters. We will slowly
// send out new parameters to fix it
need_rate_update = true;
rate_update_step = 0;
if (AP_HAL::millis() - _last_5hz_time > 10000U) {
// the gps seems to be slow, possibly due to a brown out
// invalidate the config so it can be reset
_last_5hz_time = AP_HAL::millis();
}
if (_fix_count == 50 && gps._sbas_mode != 2) {
// ask for SBAS settings every 20 seconds
Debug("Asking for SBAS setting\n");
_send_message(CLASS_CFG, MSG_CFG_SBAS, NULL, 0);
}
if (_fix_count == 100) {
// ask for nav settings every 20 seconds
Debug("Asking for engine setting\n");
_send_message(CLASS_CFG, MSG_CFG_NAV_SETTINGS, NULL, 0);
_fix_count = 0;
_unconfigured_messages = CONFIG_ALL;
_request_next_config();
GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_CRITICAL,
"GPS: u-blox %d is not maintaining a 5Hz update",
state.instance);
}
return true;
}
@ -756,49 +1015,54 @@ AP_GPS_UBLOX::_send_message(uint8_t msg_class, uint8_t msg_id, void *msg, uint16
port->write((const uint8_t *)&ck_b, 1);
}
/*
* requests the given message rate for a specific message class
* and msg_id
* returns true if it sent the request, false if waiting on knowing the port
*/
bool
AP_GPS_UBLOX::_request_message_rate(uint8_t msg_class, uint8_t msg_id)
{
// Without knowing what communication port is being used it isn't possible to verify
// always ensure we have a port before sending the request
if(_ublox_port >= UBLOX_MAX_PORTS) {
_request_port();
return false;
} else {
struct ubx_cfg_msg msg;
msg.msg_class = msg_class;
msg.msg_id = msg_id;
_send_message(CLASS_CFG, MSG_CFG_MSG, &msg, sizeof(msg));
return true;
}
}
/*
* configure a UBlox GPS for the given message rate for a specific
* message class and msg_id
*/
void
bool
AP_GPS_UBLOX::_configure_message_rate(uint8_t msg_class, uint8_t msg_id, uint8_t rate)
{
if (port->txspace() < (int16_t)(sizeof(struct ubx_header)+sizeof(struct ubx_cfg_msg_rate)+2)) {
return false;
}
struct ubx_cfg_msg_rate msg;
msg.msg_class = msg_class;
msg.msg_id = msg_id;
msg.rate = rate;
_send_message(CLASS_CFG, MSG_CFG_SET_RATE, &msg, sizeof(msg));
_send_message(CLASS_CFG, MSG_CFG_MSG, &msg, sizeof(msg));
return true;
}
/*
* configure a UBlox GPS navigation solution rate of 200ms
* request the current naviation solution rate
*/
void
AP_GPS_UBLOX::_configure_navigation_rate(uint16_t rate_ms)
AP_GPS_UBLOX::_request_navigation_rate(void)
{
struct ubx_cfg_nav_rate msg;
msg.measure_rate_ms = rate_ms;
msg.nav_rate = 1;
msg.timeref = 0; // UTC time
_send_message(CLASS_CFG, MSG_CFG_RATE, &msg, sizeof(msg));
}
/*
* configure a UBlox GPS for the given message rate
*/
void
AP_GPS_UBLOX::_configure_gps(void)
{
// start the process of updating the GPS rates
need_rate_update = true;
_last_5hz_time = AP_HAL::millis();
rate_update_step = 0;
// ask for the current navigation settings
Debug("Asking for engine setting\n");
_send_message(CLASS_CFG, MSG_CFG_NAV_SETTINGS, NULL, 0);
_send_message(CLASS_CFG, MSG_CFG_GNSS, NULL, 0);
_send_message(CLASS_CFG, MSG_CFG_RATE, 0, 0);
}
/*
@ -813,6 +1077,8 @@ AP_GPS_UBLOX::_save_cfg()
save_cfg.saveMask = SAVE_CFG_ALL;
save_cfg.loadMask = 0;
_send_message(CLASS_CFG, MSG_CFG_CFG, &save_cfg, sizeof(save_cfg));
_last_cfg_sent_time = AP_HAL::millis();
_num_cfg_save_tries++;
}
/*
@ -880,5 +1146,15 @@ reset:
void
AP_GPS_UBLOX::_request_version(void)
{
_configure_message_rate(CLASS_NAV, MSG_NAV_SVINFO, 2);
_send_message(CLASS_MON, MSG_MON_VER, NULL, 0);
}
void
AP_GPS_UBLOX::_configure_rate(void)
{
struct ubx_cfg_nav_rate msg;
msg.measure_rate_ms = MEASURE_RATE;
msg.nav_rate = 1;
msg.timeref = 0; // UTC time
_send_message(CLASS_CFG, MSG_CFG_RATE, &msg, sizeof(msg));
}

116
libraries/AP_GPS/AP_GPS_UBLOX.h
View File

@ -45,11 +45,39 @@
#define UBLOX_MAX_RXM_RAW_SATS 22
#define UBLOX_MAX_RXM_RAWX_SATS 32
#define UBLOX_GNSS_SETTINGS 1
#define UBLOX_HW_LOGGING 1
#define UBLOX_MAX_GNSS_CONFIG_BLOCKS 7
#define UBX_MSG_TYPES 2
#define UBLOX_MAX_PORTS 6
#define MEASURE_RATE 200
#define RATE_POSLLH 1
#define RATE_STATUS 1
#define RATE_SOL 1
#define RATE_VELNED 1
#define RATE_DOP 1
#define RATE_HW 5
#define RATE_HW2 5
#define CONFIG_RATE_NAV (1<<0)
#define CONFIG_RATE_POSLLH (1<<1)
#define CONFIG_RATE_STATUS (1<<2)
#define CONFIG_RATE_SOL (1<<3)
#define CONFIG_RATE_VELNED (1<<4)
#define CONFIG_RATE_DOP (1<<5)
#define CONFIG_RATE_MON_HW (1<<6)
#define CONFIG_RATE_MON_HW2 (1<<7)
#define CONFIG_RATE_RAW (1<<8)
#define CONFIG_VERSION (1<<9)
#define CONFIG_NAV_SETTINGS (1<<10)
#define CONFIG_GNSS (1<<11)
#define CONFIG_SBAS (1<<12)
#define CONFIG_ALL (CONFIG_RATE_NAV | CONFIG_RATE_POSLLH | CONFIG_RATE_STATUS | CONFIG_RATE_SOL | CONFIG_RATE_VELNED \
| CONFIG_RATE_DOP | CONFIG_RATE_MON_HW | CONFIG_RATE_MON_HW2 | CONFIG_RATE_RAW | CONFIG_VERSION \
| CONFIG_NAV_SETTINGS | CONFIG_GNSS | CONFIG_SBAS)
//Configuration Sub-Sections
#define SAVE_CFG_IO (1<<0)
#define SAVE_CFG_MSG (1<<1)
@ -72,6 +100,14 @@ public:
static bool _detect(struct UBLOX_detect_state &state, uint8_t data);
bool is_configured(void) {
if (!gps._auto_config) {
return true;
} else {
return !_unconfigured_messages;
}
}
private:
// u-blox UBX protocol essentials
struct PACKED ubx_header {
@ -101,11 +137,20 @@ private:
uint16_t nav_rate;
uint16_t timeref;
};
struct PACKED ubx_cfg_msg {
uint8_t msg_class;
uint8_t msg_id;
};
struct PACKED ubx_cfg_msg_rate {
uint8_t msg_class;
uint8_t msg_id;
uint8_t rate;
};
struct PACKED ubx_cfg_msg_rate_6 {
uint8_t msg_class;
uint8_t msg_id;
uint8_t rates[6];
};
struct PACKED ubx_cfg_nav_settings {
uint16_t mask;
uint8_t dynModel;
@ -124,6 +169,9 @@ private:
uint32_t res3;
uint32_t res4;
};
struct PACKED ubx_cfg_prt {
uint8_t portID;
};
struct PACKED ubx_cfg_sbas {
uint8_t mode;
uint8_t usage;
@ -190,7 +238,6 @@ private:
uint32_t heading_accuracy;
};
#if UBLOX_HW_LOGGING
// Lea6 uses a 60 byte message
struct PACKED ubx_mon_hw_60 {
uint32_t pinSel;
@ -243,7 +290,11 @@ private:
uint32_t postStatus;
uint32_t reserved2;
};
#endif
struct PACKED ubx_mon_ver {
char swVersion[30];
char hwVersion[10];
char extension; // extensions are not enabled
};
struct PACKED ubx_nav_svinfo_header {
uint32_t itow;
uint8_t numCh;
@ -311,12 +362,15 @@ private:
ubx_nav_dop dop;
ubx_nav_solution solution;
ubx_nav_velned velned;
ubx_cfg_msg_rate msg_rate;
ubx_cfg_msg_rate_6 msg_rate_6;
ubx_cfg_nav_settings nav_settings;
#if UBLOX_HW_LOGGING
ubx_cfg_nav_rate nav_rate;
ubx_cfg_prt prt;
ubx_mon_hw_60 mon_hw_60;
ubx_mon_hw_68 mon_hw_68;
ubx_mon_hw2 mon_hw2;
#endif
ubx_mon_ver mon_ver;
#if UBLOX_GNSS_SETTINGS
ubx_cfg_gnss gnss;
#endif
@ -346,14 +400,15 @@ private:
MSG_SOL = 0x6,
MSG_VELNED = 0x12,
MSG_CFG_CFG = 0x09,
MSG_CFG_PRT = 0x00,
MSG_CFG_RATE = 0x08,
MSG_CFG_SET_RATE = 0x01,
MSG_CFG_MSG = 0x01,
MSG_CFG_NAV_SETTINGS = 0x24,
MSG_CFG_PRT = 0x00,
MSG_CFG_SBAS = 0x16,
MSG_CFG_GNSS = 0x3E,
MSG_MON_HW = 0x09,
MSG_MON_HW2 = 0x0B,
MSG_MON_VER = 0x04,
MSG_NAV_SVINFO = 0x30,
MSG_RXM_RAW = 0x10,
MSG_RXM_RAWX = 0x15
@ -387,6 +442,29 @@ private:
UBLOX_M8
};
enum config_step {
STEP_RATE_NAV = 0,
STEP_RATE_POSLLH,
STEP_RATE_VELNED,
STEP_PORT,
STEP_POLL_SVINFO,
STEP_POLL_SBAS,
STEP_POLL_NAV,
STEP_POLL_GNSS,
STEP_NAV_RATE,
STEP_POSLLH,
STEP_STATUS,
STEP_SOL,
STEP_VELNED,
STEP_DOP,
STEP_MON_HW,
STEP_MON_HW2,
STEP_RAW,
STEP_RAWX,
STEP_VERSION,
STEP_LAST
};
// Packet checksum accumulators
uint8_t _ck_a;
uint8_t _ck_b;
@ -397,9 +475,6 @@ private:
uint16_t _payload_length;
uint16_t _payload_counter;
// 8 bit count of fix messages processed, used for periodic
// processing
uint8_t _fix_count;
uint8_t _class;
bool _cfg_saved;
@ -407,12 +482,19 @@ private:
uint32_t _last_pos_time;
uint32_t _last_cfg_sent_time;
uint8_t _num_cfg_save_tries;
uint32_t _last_config_time;
uint16_t _delay_time;
uint8_t _next_message;
uint8_t _ublox_port;
bool _have_version;
uint32_t _unconfigured_messages;
uint8_t _hardware_generation;
// do we have new position information?
bool _new_position:1;
// do we have new speed information?
bool _new_speed:1;
bool need_rate_update:1;
uint8_t _disable_counter;
@ -422,25 +504,29 @@ private:
// used to update fix between status and position packets
AP_GPS::GPS_Status next_fix;
uint8_t rate_update_step;
uint32_t _last_5hz_time;
bool noReceivedHdop;
void _configure_navigation_rate(uint16_t rate_ms);
void _configure_message_rate(uint8_t msg_class, uint8_t msg_id, uint8_t rate);
void _configure_gps(void);
bool _configure_message_rate(uint8_t msg_class, uint8_t msg_id, uint8_t rate);
void _configure_rate(void);
void _configure_sbas(bool enable);
void _update_checksum(uint8_t *data, uint16_t len, uint8_t &ck_a, uint8_t &ck_b);
void _send_message(uint8_t msg_class, uint8_t msg_id, void *msg, uint16_t size);
void send_next_rate_update(void);
bool _request_message_rate(uint8_t msg_class, uint8_t msg_id);
void _request_navigation_rate(void);
void _request_next_config(void);
void _request_port(void);
void _request_version(void);
void _save_cfg(void);
void _verify_rate(uint8_t msg_class, uint8_t msg_id, uint8_t rate);
void unexpected_message(void);
void write_logging_headers(void);
void log_mon_hw(void);
void log_mon_hw2(void);
void log_mon_ver(void);
void log_rxm_raw(const struct ubx_rxm_raw &raw);
void log_rxm_rawx(const struct ubx_rxm_rawx &raw);

6
libraries/AP_GPS/GPS_Backend.h
View File

@ -37,12 +37,14 @@ public:
// valid packet from the GPS.
virtual bool read() = 0;
virtual void inject_data(uint8_t *data, uint8_t len) { return; }
// Highest status supported by this GPS.
// Allows external system to identify type of receiver connected.
virtual AP_GPS::GPS_Status highest_supported_status(void) { return AP_GPS::GPS_OK_FIX_3D; }
virtual bool is_configured(void) { return true; }
virtual void inject_data(uint8_t *data, uint8_t len) { return; }
//MAVLink methods
virtual void send_mavlink_gps_rtk(mavlink_channel_t chan) { return ; }