rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-03-22 14:19:42 +01:00
Code Issues Releases Activity

OP-1739 Add CFG_GNSS message to autoconfig, with GPS and GLONASS configurable

Browse Source
This commit is contained in:
Erik Gustavsson 2015-02-22 14:29:05 +01:00
parent aa99c56eb3
commit 2029255538
3 changed files with 98 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
flight/modules/GPS/inc/UBX.h
View File

@ -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 {

41
flight/modules/GPS/inc/ubx_autoconfig.h
View File

@ -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;

57
flight/modules/GPS/ubx_autoconfig.c
View File

@ -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;