aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_GPS: Enable selecting what GNSS system to use if the reciever supports it

Browse Source 
Ublox 7 and 8 seires use a UBX-CFG-GNSS message to enable satellite constellations. The default value does not enable any additional ones, and any constellations the reciever doesn't report knowing about are not configured.
This commit is contained in:
Michael du Breuil 2015-07-14 14:12:47 -07:00 committed by Andrew Tridgell
parent 47592a1953
commit 22b16b4532
4 changed files with 79 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
libraries/AP_GPS/AP_GPS.cpp
View File

@ -106,6 +106,13 @@ const AP_Param::GroupInfo AP_GPS::var_info[] PROGMEM = {
AP_GROUPINFO("RAW_DATA", 9, AP_GPS, _raw_data, 0),
#endif
// @Param: GNSS_MODE
// @DisplayName: GNSS system configuration
// @Description: Bitmask for what GNSS system to use
// @Values: 0: Leave as currently configured 1: GPS 2: SBAS 4: Galileo 8: Beidou 16: IMES 32: QZSS 64: GLONASS
// @User: Advanced
AP_GROUPINFO("GNSS_MODE", 10, AP_GPS, _gnss_mode, 0),
AP_GROUPEND
};

1
libraries/AP_GPS/AP_GPS.h
View File

@ -338,6 +338,7 @@ public:
AP_Int8 _sbas_mode;
AP_Int8 _min_elevation;
AP_Int8 _raw_data;
AP_Int8 _gnss_mode;
// handle sending of initialisation strings to the GPS
void send_blob_start(uint8_t instance, const prog_char *_blob, uint16_t size);

45
libraries/AP_GPS/AP_GPS_UBLOX.cpp
View File

@ -452,6 +452,50 @@ AP_GPS_UBLOX::_parse_gps(void)
return false;
}
if (_class == CLASS_CFG && _msg_id == MSG_CFG_GNSS && gps._gnss_mode != 0) {
uint8_t gnssCount = 0;
Debug("Got GNSS Settings %u %u %u %u:\n",
(unsigned)_buffer.gnss.msgVer,
(unsigned)_buffer.gnss.numTrkChHw,
(unsigned)_buffer.gnss.numTrkChUse,
(unsigned)_buffer.gnss.numConfigBlocks);
#if UBLOX_DEBUG
for(int i = 0; i < _buffer.gnss.numConfigBlocks; i++) {
Debug(" %u %u %u 0x%08x\n",
(unsigned)_buffer.gnss.configBlock[i].gnssId,
(unsigned)_buffer.gnss.configBlock[i].resTrkCh,
(unsigned)_buffer.gnss.configBlock[i].maxTrkCh,
(unsigned)_buffer.gnss.configBlock[i].flags);
}
#endif
for(int i = 1; i <= UBLOX_MAX_GNSS_CONFIG_BLOCKS; i++) {
if((gps._gnss_mode & (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(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;
} else {
_buffer.gnss.configBlock[i].resTrkCh = 1;
_buffer.gnss.configBlock[i].resTrkCh = 3;
}
_buffer.gnss.configBlock[i].flags = _buffer.gnss.configBlock[i].flags | 0x00000001;
} else {
_buffer.gnss.configBlock[i].resTrkCh = 0;
_buffer.gnss.configBlock[i].maxTrkCh = 0;
_buffer.gnss.configBlock[i].flags = _buffer.gnss.configBlock[i].flags & 0xFFFFFFFE;
}
}
_send_message(CLASS_CFG, MSG_CFG_GNSS, &_buffer.gnss, 4 + (8 * _buffer.gnss.numConfigBlocks));
return false;
}
if (_class == CLASS_CFG && _msg_id == MSG_CFG_SBAS && gps._sbas_mode != 2) {
Debug("Got SBAS settings %u %u %u 0x%x 0x%x\n",
(unsigned)_buffer.sbas.mode,
@ -755,6 +799,7 @@ AP_GPS_UBLOX::_configure_gps(void)
// 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);
}

26
libraries/AP_GPS/AP_GPS_UBLOX.h
View File

@ -49,6 +49,8 @@
#define UBLOX_RXM_RAW_LOGGING 0
#endif
#define UBLOX_MAX_GNSS_CONFIG_BLOCKS 7
class AP_GPS_UBLOX : public AP_GPS_Backend
{
public:
@ -70,6 +72,19 @@ private:
uint8_t msg_id;
uint16_t length;
};
struct PACKED ubx_cfg_gnss {
uint8_t msgVer;
uint8_t numTrkChHw;
uint8_t numTrkChUse;
uint8_t numConfigBlocks;
struct configBlock {
uint8_t gnssId;
uint8_t resTrkCh;
uint8_t maxTrkCh;
uint8_t reserved1;
uint32_t flags;
} configBlock[UBLOX_MAX_GNSS_CONFIG_BLOCKS];
};
struct PACKED ubx_cfg_nav_rate {
uint16_t measure_rate_ms;
uint16_t nav_rate;
@ -272,6 +287,7 @@ private:
ubx_mon_hw_60 mon_hw_60;
ubx_mon_hw_68 mon_hw_68;
ubx_mon_hw2 mon_hw2;
ubx_cfg_gnss gnss;
ubx_cfg_sbas sbas;
ubx_nav_svinfo_header svinfo_header;
#if UBLOX_RXM_RAW_LOGGING
@ -301,12 +317,22 @@ private:
MSG_CFG_SET_RATE = 0x01,
MSG_CFG_NAV_SETTINGS = 0x24,
MSG_CFG_SBAS = 0x16,
MSG_CFG_GNSS = 0x3E,
MSG_MON_HW = 0x09,
MSG_MON_HW2 = 0x0B,
MSG_NAV_SVINFO = 0x30,
MSG_RXM_RAW = 0x10,
MSG_RXM_RAWX = 0x15
};
enum ubx_gnss_identifier {
GNSS_GPS = 0x00,
GNSS_SBAS = 0x01,
GNSS_GALILEO = 0x02,
GNSS_BEIDOU = 0x03,
GNSS_IMES = 0x04,
GNSS_QZSS = 0x05,
GNSS_GLONASS = 0x06
};
enum ubs_nav_fix_type {
FIX_NONE = 0,
FIX_DEAD_RECKONING = 1,