mirror of
https://github.com/iNavFlight/inav.git
synced 2025-07-24 16:55:29 +03:00
update EGNOS PRN mask to latest EGSA definition, fix SBAS for non-Galileo usage. (#6431)
[GPS] update SBAS and Galileo * Update GNSS / SBAS handling * Improve readabily of SBAS scanmask1 / PRN setting * Use current PRNs for all SBAS options * Decouple Galileo dependency for UBX-CFG-GNSS * Only set required GNSS options * Set GNSS options dynamically * Replace magic numbers with #define where appropriate Co-authored-by: Konstantin (DigitalEntity) Sharlaimov <konstantin.sharlaimov@gmail.com> Fixes #6412
This commit is contained in:
Jonathan Hudson
GitHub
parent
f8b12837db
commit
e90c76aef3
1 changed files with 96 additions and 25 deletions
|
|
@ -21,6 +21,7 @@
|
|||
#include <ctype.h>
|
||||
#include <string.h>
|
||||
#include <math.h>
|
||||
#include <stdarg.h>
|
||||
|
||||
#include "platform.h"
|
||||
#include "build/build_config.h"
|
||||
|
|
@ -67,8 +68,18 @@
|
|||
#define UBX_VALID_GPS_DATE_TIME(valid) (UBX_VALID_GPS_DATE(valid) && UBX_VALID_GPS_TIME(valid))
|
||||
|
||||
// SBAS_AUTO, SBAS_EGNOS, SBAS_WAAS, SBAS_MSAS, SBAS_GAGAN, SBAS_NONE
|
||||
// note PRNs last upadted 2020-12-18
|
||||
|
||||
#define SBASMASK1_BASE 120
|
||||
#define SBASMASK1_BITS(prn) (1 << (prn-SBASMASK1_BASE))
|
||||
|
||||
static const uint32_t ubloxScanMode1[] = {
|
||||
0x00000000, 0x00000851, 0x0004E004, 0x00020200, 0x00000180, 0x00000000,
|
||||
0x00000000, // AUTO
|
||||
(SBASMASK1_BITS(123) | SBASMASK1_BITS(126) | SBASMASK1_BITS(136)), // SBAS
|
||||
(SBASMASK1_BITS(131) | SBASMASK1_BITS(133) | SBASMASK1_BITS(138)), // WAAS
|
||||
(SBASMASK1_BITS(129) | SBASMASK1_BITS(137)), // MAAS
|
||||
(SBASMASK1_BITS(127) | SBASMASK1_BITS(128)), // GAGAN
|
||||
0x00000000, // NONE
|
||||
};
|
||||
|
||||
static const char * baudInitDataNMEA[GPS_BAUDRATE_COUNT] = {
|
||||
|
|
@ -101,11 +112,35 @@ typedef struct {
|
|||
uint16_t time;
|
||||
} ubx_rate;
|
||||
|
||||
typedef struct {
|
||||
uint8_t gnssId;
|
||||
uint8_t resTrkCh;
|
||||
uint8_t maxTrkCh;
|
||||
uint8_t reserved1;
|
||||
// flags
|
||||
uint8_t enabled;
|
||||
uint8_t undefined0;
|
||||
uint8_t sigCfgMask;
|
||||
uint8_t undefined1;
|
||||
} ubx_gnss_element_t;
|
||||
|
||||
typedef struct {
|
||||
uint8_t msgVer;
|
||||
uint8_t numTrkChHw;
|
||||
uint8_t numTrkChUse;
|
||||
uint8_t numConfigBlocks;
|
||||
ubx_gnss_element_t config[0];
|
||||
} ubx_gnss_msg_t;
|
||||
|
||||
#define MAX_GNSS 7
|
||||
#define MAX_GNSS_SIZE_BYTES (sizeof(ubx_gnss_msg_t) + sizeof(ubx_gnss_element_t)*MAX_GNSS)
|
||||
|
||||
typedef union {
|
||||
uint8_t bytes[60]; // sizeof Galileo config
|
||||
uint8_t bytes[MAX_GNSS_SIZE_BYTES]; // placeholder
|
||||
ubx_sbas sbas;
|
||||
ubx_msg msg;
|
||||
ubx_rate rate;
|
||||
ubx_gnss_msg_t gnss;
|
||||
} ubx_payload;
|
||||
|
||||
// UBX support
|
||||
|
|
@ -409,25 +444,63 @@ static const uint8_t default_payload[] = {
|
|||
0x00, 0xC8, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00
|
||||
};
|
||||
|
||||
// Note the organisation of the bytes reflects the structure of the payload
|
||||
// 4 bytes then 8*number of elements (7)
|
||||
static const uint8_t galileo_payload[] = {
|
||||
0x00, 0x00, 0x20, 0x07, // GNSS / min / max / enable
|
||||
0x00, 0x08, 0x10, 0x00, 0x01, 0x00, 0x01, 0x01, // GPS / 8 / 16 / Y
|
||||
0x01, 0x01, 0x03, 0x00, 0x01, 0x00, 0x01, 0x01, // SBAS / 1 / 3 / Y
|
||||
0x02, 0x04, 0x08, 0x00, 0x01, 0x00, 0x01, 0x01, // Galileo / 4 / 8 / Y
|
||||
0x03, 0x08, 0x10, 0x00, 0x00, 0x00, 0x01, 0x01, // BeiDou / 8 / 16 / N
|
||||
0x04, 0x00, 0x08, 0x00, 0x00, 0x00, 0x01, 0x01, // IMES / 0 / 8 / N
|
||||
0x05, 0x00, 0x03, 0x00, 0x00, 0x00, 0x01, 0x01, // QZSS / 0 / 3 / N
|
||||
0x06, 0x08, 0x0e, 0x00, 0x01, 0x00, 0x01, 0x01 // GLONASS / 8 / 14 / Y
|
||||
};
|
||||
#define GNSSID_SBAS 1
|
||||
#define GNSSID_GALILEO 2
|
||||
|
||||
static void configureGalileo(void)
|
||||
static int configureGNSS_SBAS(ubx_gnss_element_t * gnss_block)
|
||||
{
|
||||
gnss_block->gnssId = GNSSID_SBAS;
|
||||
gnss_block->maxTrkCh = 3;
|
||||
gnss_block->sigCfgMask = 1;
|
||||
if (gpsState.gpsConfig->sbasMode == SBAS_NONE) {
|
||||
gnss_block->enabled = 0;
|
||||
gnss_block->resTrkCh = 0;
|
||||
} else {
|
||||
gnss_block->enabled = 1;
|
||||
gnss_block->resTrkCh = 1;
|
||||
}
|
||||
|
||||
return 1;
|
||||
}
|
||||
|
||||
static int configureGNSS_GALILEO(ubx_gnss_element_t * gnss_block)
|
||||
{
|
||||
if (!capGalileo) {
|
||||
return 0;
|
||||
}
|
||||
|
||||
gnss_block->gnssId = GNSSID_GALILEO;
|
||||
gnss_block->maxTrkCh = 8;
|
||||
gnss_block->sigCfgMask = 1;
|
||||
if (gpsState.gpsConfig->ubloxUseGalileo) {
|
||||
gnss_block->enabled = 1;
|
||||
gnss_block->resTrkCh = 4;
|
||||
} else {
|
||||
gnss_block->enabled = 0;
|
||||
gnss_block->resTrkCh = 0;
|
||||
}
|
||||
|
||||
return 1;
|
||||
}
|
||||
|
||||
static void configureGNSS(void)
|
||||
{
|
||||
int blocksUsed = 0;
|
||||
|
||||
send_buffer.message.header.msg_class = CLASS_CFG;
|
||||
send_buffer.message.header.msg_id = MSG_CFG_GNSS;
|
||||
send_buffer.message.header.length = sizeof(galileo_payload);
|
||||
memcpy(send_buffer.message.payload.bytes, galileo_payload, sizeof(galileo_payload));
|
||||
send_buffer.message.payload.gnss.msgVer = 0;
|
||||
send_buffer.message.payload.gnss.numTrkChHw = 0; // read only, so unset
|
||||
send_buffer.message.payload.gnss.numTrkChUse = 32;
|
||||
|
||||
/* SBAS, always generated */
|
||||
blocksUsed += configureGNSS_SBAS(&send_buffer.message.payload.gnss.config[blocksUsed]);
|
||||
|
||||
/* Galileo */
|
||||
blocksUsed += configureGNSS_GALILEO(&send_buffer.message.payload.gnss.config[blocksUsed]);
|
||||
|
||||
send_buffer.message.payload.gnss.numConfigBlocks = blocksUsed;
|
||||
send_buffer.message.header.length = (sizeof(ubx_gnss_msg_t) + sizeof(ubx_gnss_element_t)* blocksUsed);
|
||||
sendConfigMessageUBLOX();
|
||||
}
|
||||
|
||||
|
|
@ -741,7 +814,7 @@ STATIC_PROTOTHREAD(gpsConfigure)
|
|||
// Configure UBX binary messages
|
||||
gpsSetProtocolTimeout(GPS_SHORT_TIMEOUT);
|
||||
|
||||
// u-Blox 9
|
||||
// u-Blox 9
|
||||
// M9N does not support some of the UBX 6/7/8 messages, so we have to configure it using special sequence
|
||||
if (gpsState.hwVersion >= 190000) {
|
||||
configureMSG(MSG_CLASS_UBX, MSG_POSLLH, 0);
|
||||
|
|
@ -840,13 +913,11 @@ STATIC_PROTOTHREAD(gpsConfigure)
|
|||
configureSBAS();
|
||||
ptWaitTimeout((_ack_state == UBX_ACK_GOT_ACK || _ack_state == UBX_ACK_GOT_NAK), GPS_CFG_CMD_TIMEOUT_MS);
|
||||
|
||||
// Enable GALILEO
|
||||
if (gpsState.gpsConfig->ubloxUseGalileo && capGalileo) {
|
||||
// If GALILEO is not supported by the hardware we'll get a NAK,
|
||||
// however GPS would otherwise be perfectly initialized, so we are just waiting for any response
|
||||
gpsSetProtocolTimeout(GPS_SHORT_TIMEOUT);
|
||||
configureGalileo();
|
||||
ptWaitTimeout((_ack_state == UBX_ACK_GOT_ACK || _ack_state == UBX_ACK_GOT_NAK), GPS_CFG_CMD_TIMEOUT_MS);
|
||||
// Configure GNSS for M8N and later
|
||||
if (gpsState.hwVersion >= 80000) {
|
||||
gpsSetProtocolTimeout(GPS_SHORT_TIMEOUT);
|
||||
configureGNSS();
|
||||
ptWaitTimeout((_ack_state == UBX_ACK_GOT_ACK || _ack_state == UBX_ACK_GOT_NAK), GPS_CFG_CMD_TIMEOUT_MS);
|
||||
}
|
||||
|
||||
ptEnd(0);
|
||||
|
|
|
|||
Loading…
Add table
Add a link
Reference in a new issue