mirror of
https://bitbucket.org/librepilot/librepilot.git
synced 2025-03-15 07:29:15 +01:00
OP-1739 Add CFG_GNSS message to autoconfig, with GPS and GLONASS configurable
This commit is contained in:
parent
aa99c56eb3
commit
2029255538
@ -102,6 +102,7 @@ typedef enum {
|
||||
UBX_ID_CFG_MSG = 0x01,
|
||||
UBX_ID_CFG_CFG = 0x09,
|
||||
UBX_ID_CFG_SBAS = 0x16,
|
||||
UBX_ID_CFG_GNSS = 0x3E
|
||||
} ubx_class_cfg_id;
|
||||
|
||||
typedef enum {
|
||||
|
@ -88,6 +88,9 @@ typedef struct {
|
||||
|
||||
int8_t navRate;
|
||||
ubx_config_dynamicmodel_t dynamicModel;
|
||||
|
||||
bool enableGPS;
|
||||
bool enableGLONASS;
|
||||
} ubx_autoconfig_settings_t;
|
||||
|
||||
// Mask for "all supported devices": battery backed RAM, Flash, EEPROM, SPI Flash
|
||||
@ -164,6 +167,43 @@ typedef struct {
|
||||
uint32_t scanmode1;
|
||||
} __attribute__((packed)) ubx_cfg_sbas_t;
|
||||
|
||||
typedef enum {
|
||||
UBX_GNSS_ID_GPS = 0,
|
||||
UBX_GNSS_ID_SBAS = 1,
|
||||
UBX_GNSS_ID_GALILEO = 2,
|
||||
UBX_GNSS_ID_BEIDOU = 3,
|
||||
UBX_GNSS_ID_IMES = 4,
|
||||
UBX_GNSS_ID_QZSS = 5,
|
||||
UBX_GNSS_ID_GLONASS = 6,
|
||||
UBX_GNSS_ID_MAX
|
||||
} ubx_config_gnss_id_t;
|
||||
|
||||
#define UBX_CFG_GNSS_FLAGS_ENABLED 0x01
|
||||
#define UBX_CFG_GNSS_FLAGS_GPS_L1CA 0x010000
|
||||
#define UBX_CFG_GNSS_FLAGS_SBAS_L1CA 0x010000
|
||||
#define UBX_CFG_GNSS_FLAGS_QZSS_L1CA 0x010000
|
||||
#define UBX_CFG_GNSS_FLAGS_QZSS_L1SAIF 0x040000
|
||||
#define UBX_CFG_GNSS_FLAGS_GLONASS_L1OF 0x010000
|
||||
|
||||
#define UBX_CFG_GNSS_NUMCH_VER7 22
|
||||
#define UBX_CFG_GNSS_NUMCH_VER8 32
|
||||
|
||||
typedef struct {
|
||||
uint8_t gnssId;
|
||||
uint8_t resTrkCh;
|
||||
uint8_t maxTrkCh;
|
||||
uint8_t resvd;
|
||||
uint32_t flags;
|
||||
} __attribute__((packed)) ubx_cfg_gnss_cfgblock_t;
|
||||
|
||||
typedef struct {
|
||||
uint8_t msgVer;
|
||||
uint8_t numTrkChHw;
|
||||
uint8_t numTrkChUse;
|
||||
uint8_t numConfigBlocks;
|
||||
ubx_cfg_gnss_cfgblock_t cfgBlocks[UBX_GNSS_ID_MAX];
|
||||
} __attribute__((packed)) ubx_cfg_gnss_t;
|
||||
|
||||
typedef struct {
|
||||
uint8_t prolog[2];
|
||||
uint8_t class;
|
||||
@ -181,6 +221,7 @@ typedef union {
|
||||
ubx_cfg_nav5_t cfg_nav5;
|
||||
ubx_cfg_rate_t cfg_rate;
|
||||
ubx_cfg_sbas_t cfg_sbas;
|
||||
ubx_cfg_gnss_t cfg_gnss;
|
||||
} payload;
|
||||
uint8_t resvd[2]; // added space for checksum bytes
|
||||
} message;
|
||||
|
@ -224,6 +224,58 @@ void config_sbas(uint16_t *bytes_to_send)
|
||||
status->requiredAck.msgID = UBX_ID_CFG_SBAS;
|
||||
}
|
||||
|
||||
void config_gnss(uint16_t *bytes_to_send)
|
||||
{
|
||||
int32_t i;
|
||||
|
||||
memset(status->working_packet.buffer, 0, sizeof(UBXSentHeader_t) + sizeof(ubx_cfg_gnss_t));
|
||||
|
||||
status->working_packet.message.payload.cfg_gnss.numConfigBlocks = UBX_GNSS_ID_MAX;
|
||||
status->working_packet.message.payload.cfg_gnss.numTrkChHw = (ubxHwVersion > UBX_HW_VERSION_7) ? UBX_CFG_GNSS_NUMCH_VER8 : UBX_CFG_GNSS_NUMCH_VER7;
|
||||
status->working_packet.message.payload.cfg_gnss.numTrkChUse = status->working_packet.message.payload.cfg_gnss.numTrkChHw;
|
||||
|
||||
for (i = 0; i < UBX_GNSS_ID_MAX; i++) {
|
||||
status->working_packet.message.payload.cfg_gnss.cfgBlocks[i].gnssId = i;
|
||||
|
||||
switch (i) {
|
||||
case UBX_GNSS_ID_GPS:
|
||||
if (status->currentSettings.enableGPS) {
|
||||
status->working_packet.message.payload.cfg_gnss.cfgBlocks[i].flags = UBX_CFG_GNSS_FLAGS_ENABLED | UBX_CFG_GNSS_FLAGS_GPS_L1CA;
|
||||
status->working_packet.message.payload.cfg_gnss.cfgBlocks[i].maxTrkCh = 16;
|
||||
status->working_packet.message.payload.cfg_gnss.cfgBlocks[i].resTrkCh = 8;
|
||||
}
|
||||
break;
|
||||
case UBX_GNSS_ID_QZSS:
|
||||
if (status->currentSettings.enableGPS) {
|
||||
status->working_packet.message.payload.cfg_gnss.cfgBlocks[i].flags = UBX_CFG_GNSS_FLAGS_ENABLED | UBX_CFG_GNSS_FLAGS_QZSS_L1CA;
|
||||
status->working_packet.message.payload.cfg_gnss.cfgBlocks[i].maxTrkCh = 3;
|
||||
status->working_packet.message.payload.cfg_gnss.cfgBlocks[i].resTrkCh = 0;
|
||||
}
|
||||
break;
|
||||
case UBX_GNSS_ID_SBAS:
|
||||
if (status->currentSettings.SBASCorrection || status->currentSettings.SBASIntegrity || status->currentSettings.SBASRanging) {
|
||||
status->working_packet.message.payload.cfg_gnss.cfgBlocks[i].flags = UBX_CFG_GNSS_FLAGS_ENABLED | UBX_CFG_GNSS_FLAGS_SBAS_L1CA;
|
||||
status->working_packet.message.payload.cfg_gnss.cfgBlocks[i].maxTrkCh = status->currentSettings.SBASChannelsUsed < 4 ? status->currentSettings.SBASChannelsUsed : 3;
|
||||
status->working_packet.message.payload.cfg_gnss.cfgBlocks[i].resTrkCh = 1;
|
||||
}
|
||||
break;
|
||||
case UBX_GNSS_ID_GLONASS:
|
||||
if (status->currentSettings.enableGLONASS) {
|
||||
status->working_packet.message.payload.cfg_gnss.cfgBlocks[i].flags = UBX_CFG_GNSS_FLAGS_ENABLED | UBX_CFG_GNSS_FLAGS_GLONASS_L1OF;
|
||||
status->working_packet.message.payload.cfg_gnss.cfgBlocks[i].maxTrkCh = 14;
|
||||
status->working_packet.message.payload.cfg_gnss.cfgBlocks[i].resTrkCh = 8;
|
||||
}
|
||||
break;
|
||||
default:
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
*bytes_to_send = prepare_packet(&status->working_packet, UBX_CLASS_CFG, UBX_ID_CFG_GNSS, sizeof(ubx_cfg_gnss_t));
|
||||
status->requiredAck.clsID = UBX_CLASS_CFG;
|
||||
status->requiredAck.msgID = UBX_ID_CFG_GNSS;
|
||||
}
|
||||
|
||||
void config_save(uint16_t *bytes_to_send)
|
||||
{
|
||||
memset(status->working_packet.buffer, 0, sizeof(UBXSentHeader_t) + sizeof(ubx_cfg_cfg_t));
|
||||
@ -244,13 +296,16 @@ static void configure(uint16_t *bytes_to_send)
|
||||
config_nav(bytes_to_send);
|
||||
break;
|
||||
case LAST_CONFIG_SENT_START + 2:
|
||||
config_gnss(bytes_to_send);
|
||||
break;
|
||||
case LAST_CONFIG_SENT_START + 3:
|
||||
config_sbas(bytes_to_send);
|
||||
if (!status->currentSettings.storeSettings) {
|
||||
// skips saving
|
||||
status->lastConfigSent = LAST_CONFIG_SENT_COMPLETED;
|
||||
}
|
||||
break;
|
||||
case LAST_CONFIG_SENT_START + 3:
|
||||
case LAST_CONFIG_SENT_START + 4:
|
||||
config_save(bytes_to_send);
|
||||
status->lastConfigSent = LAST_CONFIG_SENT_COMPLETED;
|
||||
return;
|
||||
|
Loading…
x
Reference in New Issue
Block a user