Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-23 16:25:31 +03:00
Code Issues Wiki Activity

SBAS_NONE, gps_sbas_integrity

Browse source
This commit is contained in:
Tony Cabello 2020-03-22 18:54:03 +01:00
parent bbdc1bd586
commit 5932b01e6b
3 changed files with 144 additions and 52 deletions
Download patch file Download diff file Expand all files Collapse all files

3
src/main/cli/settings.c
View file

@ -194,7 +194,7 @@ static const char * const lookupTableGPSProvider[] = {
}; };
static const char * const lookupTableGPSSBASMode[] = { static const char * const lookupTableGPSSBASMode[] = {
"AUTO", "EGNOS", "WAAS", "MSAS", "GAGAN" "AUTO", "EGNOS", "WAAS", "MSAS", "GAGAN", "NONE"
}; };
static const char * const lookupTableGPSUBLOXMode[] = { static const char * const lookupTableGPSUBLOXMode[] = {
@ -938,6 +938,7 @@ const clivalue_t valueTable[] = {
#ifdef USE_GPS #ifdef USE_GPS
{ "gps_provider", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_GPS_PROVIDER }, PG_GPS_CONFIG, offsetof(gpsConfig_t, provider) }, { "gps_provider", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_GPS_PROVIDER }, PG_GPS_CONFIG, offsetof(gpsConfig_t, provider) },
{ "gps_sbas_mode", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_GPS_SBAS_MODE }, PG_GPS_CONFIG, offsetof(gpsConfig_t, sbasMode) }, { "gps_sbas_mode", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_GPS_SBAS_MODE }, PG_GPS_CONFIG, offsetof(gpsConfig_t, sbasMode) },
{ "gps_sbas_integrity", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_GPS_CONFIG, offsetof(gpsConfig_t, sbas_integrity) },
{ "gps_auto_config", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_GPS_CONFIG, offsetof(gpsConfig_t, autoConfig) }, { "gps_auto_config", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_GPS_CONFIG, offsetof(gpsConfig_t, autoConfig) },
{ "gps_auto_baud", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_GPS_CONFIG, offsetof(gpsConfig_t, autoBaud) }, { "gps_auto_baud", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_GPS_CONFIG, offsetof(gpsConfig_t, autoBaud) },
{ "gps_ublox_use_galileo", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_GPS_CONFIG, offsetof(gpsConfig_t, gps_ublox_use_galileo) }, { "gps_ublox_use_galileo", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_GPS_CONFIG, offsetof(gpsConfig_t, gps_ublox_use_galileo) },

179
src/main/io/gps.c
View file

@ -168,46 +168,51 @@ static const uint8_t ubloxAirborne[] = {
#endif #endif
}; };
// UBlox 6 Protocol documentation - GPS.G6-SW-10018-F typedef struct {
// SBAS Configuration Settings Desciption, Page 4/210 uint8_t preamble1;
// 31.21 CFG-SBAS (0x06 0x16), Page 142/210 uint8_t preamble2;
// A.10 SBAS Configuration (UBX-CFG-SBAS), Page 198/210 - GPS.G6-SW-10018-F uint8_t msg_class;
uint8_t msg_id;
uint16_t length;
} ubx_header;
#define UBLOX_SBAS_PREFIX_LENGTH 10 typedef struct {
uint8_t mode;
uint8_t usage;
uint8_t maxSBAS;
uint8_t scanmode2;
uint32_t scanmode1;
} ubx_sbas;
static const uint8_t ubloxSbasPrefix[UBLOX_SBAS_PREFIX_LENGTH] = { 0xB5, 0x62, 0x06, 0x16, 0x08, 0x00, 0x03, 0x07, 0x03, 0x00 }; typedef union {
ubx_sbas sbas;
} ubx_payload;
#define UBLOX_SBAS_MESSAGE_LENGTH 6 typedef struct {
typedef struct ubloxSbas_s { ubx_header header;
sbasMode_e mode; ubx_payload payload;
uint8_t message[UBLOX_SBAS_MESSAGE_LENGTH]; } __attribute__((packed)) ubx_message;
} ubloxSbas_t;
typedef union {
ubx_message message;
uint8_t bytes[58];
} ubx_tx_buffer;
#define UBLOX_SBAS_MESSAGE_LENGTH 14
// Note: these must be defined in the same order is sbasMode_e since no lookup table is used.
static const ubloxSbas_t ubloxSbas[] = {
// NOTE this could be optimized to save a few bytes of flash space since the same prefix is used for each command.
{ SBAS_AUTO, { 0x00, 0x00, 0x00, 0x00, 0x31, 0xE5}},
{ SBAS_EGNOS, { 0x51, 0x08, 0x00, 0x00, 0x8A, 0x41}},
{ SBAS_WAAS, { 0x04, 0xE0, 0x04, 0x00, 0x19, 0x9D}},
{ SBAS_MSAS, { 0x00, 0x02, 0x02, 0x00, 0x35, 0xEF}},
{ SBAS_GAGAN, { 0x80, 0x01, 0x00, 0x00, 0xB2, 0xE8}}
};
// Remove QZSS and add Galileo (only 3 GNSS systems supported simultaneously) // Remove QZSS and add Galileo (only 3 GNSS systems supported simultaneously)
// Frame captured from uCenter // Frame captured from uCenter
static const uint8_t ubloxGalileoInit[] = { static const uint8_t ubloxGalileoInit[] = {
0xB5, 0x62, 0x06, 0x3E, 0x3C, // UBX-CGF-GNSS 0xB5, 0x62, 0x06, 0x3E, 0x3C, 0x00, // UBX-CFG-GNSS, 60 bytes length
0x00, 0x00, 0x20, 0x20, 0x07, // GNSS 0x00, 0x20, 0x20, 0x07, // 32 tracking channels, 7 config blocks
0x00, 0x08, 0x10, 0x00, 0x01, 0x00, 0x01, 0x01, // GPS 0x00, 0x08, 0x10, 0x00, 0x01, 0x00, 0x01, 0x01, // GPS - [8-16] channels, GPS L1C/A
0x01, 0x01, 0x03, 0x00, 0x01, 0x00, 0x01, 0x01, // SBAS 0x01, 0x01, 0x03, 0x00, 0x01, 0x00, 0x01, 0x01, // SBAS - [1-3] channels, SBAS L1C/A
0x02, 0x04, 0x08, 0x00, 0x01, 0x00, 0x01, 0x01, // Galileo 0x02, 0x04, 0x08, 0x00, 0x01, 0x00, 0x01, 0x01, // Galileo - [4-8] channels, Galileo E1
0x03, 0x08, 0x10, 0x00, 0x00, 0x00, 0x01, 0x01, // BeiDou 0x03, 0x08, 0x10, 0x00, 0x00, 0x00, 0x01, 0x01, // BeiDou - disabled
0x04, 0x00, 0x08, 0x00, 0x00, 0x00, 0x01, 0x03, // IMES 0x04, 0x00, 0x08, 0x00, 0x00, 0x00, 0x01, 0x03, // IMES - disabled
0x05, 0x00, 0x03, 0x00, 0x00, 0x00, 0x01, 0x05, // QZSS 0x05, 0x00, 0x03, 0x00, 0x00, 0x00, 0x01, 0x05, // QZSS - disabled
0x06, 0x08, 0x0E, 0x00, 0x01, 0x00, 0x01, 0x01, // GLONASS 0x06, 0x08, 0x0E, 0x00, 0x01, 0x00, 0x01, 0x01, // GLONASS - [8-14] channels, GLONASS L1
0x55, 0x47 0x55, 0x47 // Checksum
}; };
#endif // USE_GPS_UBLOX #endif // USE_GPS_UBLOX
@ -228,13 +233,14 @@ PG_REGISTER_WITH_RESET_TEMPLATE(gpsConfig_t, gpsConfig, PG_GPS_CONFIG, 0);
PG_RESET_TEMPLATE(gpsConfig_t, gpsConfig, PG_RESET_TEMPLATE(gpsConfig_t, gpsConfig,
.provider = GPS_NMEA, .provider = GPS_NMEA,
.sbasMode = SBAS_AUTO, .sbasMode = SBAS_NONE,
.autoConfig = GPS_AUTOCONFIG_ON, .autoConfig = GPS_AUTOCONFIG_ON,
.autoBaud = GPS_AUTOBAUD_OFF, .autoBaud = GPS_AUTOBAUD_OFF,
.gps_ublox_use_galileo = false, .gps_ublox_use_galileo = false,
.gps_ublox_mode = UBLOX_AIRBORNE, .gps_ublox_mode = UBLOX_AIRBORNE,
.gps_set_home_point_once = false, .gps_set_home_point_once = false,
.gps_use_3d_speed = false .gps_use_3d_speed = false,
.sbas_integrity = false
); );
static void shiftPacketLog(void) static void shiftPacketLog(void)
@ -363,6 +369,35 @@ void gpsInitNmea(void)
#endif // USE_GPS_NMEA #endif // USE_GPS_NMEA
#ifdef USE_GPS_UBLOX #ifdef USE_GPS_UBLOX
static void ubloxSendByteUpdateChecksum(const uint8_t data, uint8_t *checksumA, uint8_t *checksumB)
{
*checksumA += data;
*checksumB += *checksumA;
serialWrite(gpsPort, data);
}
static void ubloxSendDataUpdateChecksum(const uint8_t *data, uint8_t len, uint8_t *checksumA, uint8_t *checksumB)
{
while (len--) {
ubloxSendByteUpdateChecksum(*data, checksumA, checksumB);
data++;
}
}
static void ubloxSendConfigMessage(const uint8_t *data, uint8_t len)
{
uint8_t checksumA = 0, checksumB = 0;
serialWrite(gpsPort, data[0]);
serialWrite(gpsPort, data[1]);
ubloxSendDataUpdateChecksum(&data[2], len-2, &checksumA, &checksumB);
serialWrite(gpsPort, checksumA);
serialWrite(gpsPort, checksumB);
// Save state for ACK waiting
gpsData.ackWaitingMsgId = data[3]; //save message id for ACK
gpsData.ackState = UBLOX_ACK_WAITING;
}
void gpsInitUblox(void) void gpsInitUblox(void)
{ {
uint32_t now; uint32_t now;
@ -431,19 +466,55 @@ void gpsInitUblox(void)
} else { } else {
gpsData.state_position = 0; gpsData.state_position = 0;
gpsData.messageState++; gpsData.messageState++;
gpsData.ackState = UBLOX_ACK_IDLE;
} }
} }
if (gpsData.messageState == GPS_MESSAGE_STATE_SBAS) { if (gpsData.messageState == GPS_MESSAGE_STATE_SBAS) {
if (gpsData.state_position < UBLOX_SBAS_PREFIX_LENGTH) { switch (gpsData.ackState) {
serialWrite(gpsPort, ubloxSbasPrefix[gpsData.state_position]); case UBLOX_ACK_IDLE:
gpsData.state_position++; {
} else if (gpsData.state_position < UBLOX_SBAS_PREFIX_LENGTH + UBLOX_SBAS_MESSAGE_LENGTH) { ubx_tx_buffer tx_buffer;
serialWrite(gpsPort, ubloxSbas[gpsConfig()->sbasMode].message[gpsData.state_position - UBLOX_SBAS_PREFIX_LENGTH]); tx_buffer.message.header.preamble1 = 0xB5;
gpsData.state_position++; tx_buffer.message.header.preamble2 = 0x62;
} else { tx_buffer.message.header.msg_class = 0x06;
gpsData.state_position = 0; tx_buffer.message.header.msg_id = 0x16;
gpsData.messageState++; tx_buffer.message.header.length = 8;
tx_buffer.message.payload.sbas.mode = (gpsConfig()->sbasMode == SBAS_NONE) ? 2 : 3;
tx_buffer.message.payload.sbas.usage = (gpsConfig()->sbas_integrity) ? 7 : 3;
tx_buffer.message.payload.sbas.maxSBAS = 3;
tx_buffer.message.payload.sbas.scanmode2 = 0;
switch (gpsConfig()->sbasMode) {
case SBAS_AUTO:
tx_buffer.message.payload.sbas.scanmode1 = 0;
break;
case SBAS_EGNOS:
tx_buffer.message.payload.sbas.scanmode1 = 0x00010048; //PRN123, PRN126, PRN136
break;
case SBAS_WAAS:
tx_buffer.message.payload.sbas.scanmode1 = 0x0004A800; //PRN131, PRN133, PRN135, PRN138
break;
case SBAS_MSAS:
tx_buffer.message.payload.sbas.scanmode1 = 0x00020200; //PRN129, PRN137
break;
case SBAS_GAGAN:
tx_buffer.message.payload.sbas.scanmode1 = 0x00001180; //PRN127, PRN128, PRN132
break;
default:
tx_buffer.message.payload.sbas.scanmode1 = 0;
break;
}
ubloxSendConfigMessage(tx_buffer.bytes, UBLOX_SBAS_MESSAGE_LENGTH);
}
break;
case UBLOX_ACK_GOT_NACK: //TODO: implement a retry count, for now just ignore errors as the rest of the code is doing
case UBLOX_ACK_GOT_ACK:
gpsData.state_position = 0;
gpsData.ackState = UBLOX_ACK_IDLE;
gpsData.messageState++;
break;
default:
break;
} }
} }
@ -915,14 +986,6 @@ static bool gpsNewFrameNMEA(char c)
#ifdef USE_GPS_UBLOX #ifdef USE_GPS_UBLOX
// UBX support // UBX support
typedef struct {
uint8_t preamble1;
uint8_t preamble2;
uint8_t msg_class;
uint8_t msg_id;
uint16_t length;
} ubx_header;
typedef struct { typedef struct {
uint32_t time; // GPS msToW uint32_t time; // GPS msToW
int32_t longitude; int32_t longitude;
@ -994,6 +1057,11 @@ typedef struct {
ubx_nav_svinfo_channel channel[16]; // 16 satellites * 12 byte ubx_nav_svinfo_channel channel[16]; // 16 satellites * 12 byte
} ubx_nav_svinfo; } ubx_nav_svinfo;
typedef struct {
uint8_t clsId; // Class ID of the acknowledged message
uint8_t msgId; // Message ID of the acknowledged message
} ubx_ack;
enum { enum {
PREAMBLE1 = 0xb5, PREAMBLE1 = 0xb5,
PREAMBLE2 = 0x62, PREAMBLE2 = 0x62,
@ -1072,6 +1140,7 @@ static union {
ubx_nav_solution solution; ubx_nav_solution solution;
ubx_nav_velned velned; ubx_nav_velned velned;
ubx_nav_svinfo svinfo; ubx_nav_svinfo svinfo;
ubx_ack ack;
uint8_t bytes[UBLOX_PAYLOAD_SIZE]; uint8_t bytes[UBLOX_PAYLOAD_SIZE];
} _buffer; } _buffer;
@ -1153,6 +1222,16 @@ static bool UBLOX_parse_gps(void)
} }
GPS_svInfoReceivedCount++; GPS_svInfoReceivedCount++;
break; break;
case MSG_ACK_ACK:
if ((gpsData.ackState == UBLOX_ACK_WAITING) && (_buffer.ack.msgId == gpsData.ackWaitingMsgId)) {
gpsData.ackState = UBLOX_ACK_GOT_ACK;
}
break;
case MSG_ACK_NACK:
if ((gpsData.ackState == UBLOX_ACK_WAITING) && (_buffer.ack.msgId == gpsData.ackWaitingMsgId)) {
gpsData.ackState = UBLOX_ACK_GOT_NACK;
}
break;
default: default:
return false; return false;
} }

14
src/main/io/gps.h
View file

@ -43,7 +43,8 @@ typedef enum {
SBAS_EGNOS, SBAS_EGNOS,
SBAS_WAAS, SBAS_WAAS,
SBAS_MSAS, SBAS_MSAS,
SBAS_GAGAN SBAS_GAGAN,
SBAS_NONE
} sbasMode_e; } sbasMode_e;
#define SBAS_MODE_MAX SBAS_GAGAN #define SBAS_MODE_MAX SBAS_GAGAN
@ -72,6 +73,13 @@ typedef enum {
GPS_AUTOBAUD_ON GPS_AUTOBAUD_ON
} gpsAutoBaud_e; } gpsAutoBaud_e;
typedef enum {
UBLOX_ACK_IDLE = 0,
UBLOX_ACK_WAITING,
UBLOX_ACK_GOT_ACK,
UBLOX_ACK_GOT_NACK
} ubloxAckState_e;
#define GPS_BAUDRATE_MAX GPS_BAUDRATE_9600 #define GPS_BAUDRATE_MAX GPS_BAUDRATE_9600
typedef struct gpsConfig_s { typedef struct gpsConfig_s {
@ -83,6 +91,7 @@ typedef struct gpsConfig_s {
ubloxMode_e gps_ublox_mode; ubloxMode_e gps_ublox_mode;
uint8_t gps_set_home_point_once; uint8_t gps_set_home_point_once;
uint8_t gps_use_3d_speed; uint8_t gps_use_3d_speed;
uint8_t sbas_integrity;
} gpsConfig_t; } gpsConfig_t;
PG_DECLARE(gpsConfig_t, gpsConfig); PG_DECLARE(gpsConfig_t, gpsConfig);
@ -129,6 +138,9 @@ typedef struct gpsData_s {
uint8_t state; // GPS thread state. Used for detecting cable disconnects and configuring attached devices uint8_t state; // GPS thread state. Used for detecting cable disconnects and configuring attached devices
uint8_t baudrateIndex; // index into auto-detecting or current baudrate uint8_t baudrateIndex; // index into auto-detecting or current baudrate
gpsMessageState_e messageState; gpsMessageState_e messageState;
uint8_t ackWaitingMsgId; // Message id when waiting for ACK
ubloxAckState_e ackState;
} gpsData_t; } gpsData_t;
#define GPS_PACKET_LOG_ENTRY_COUNT 21 // To make this useful we should log as many packets as we can fit characters a single line of a OLED display. #define GPS_PACKET_LOG_ENTRY_COUNT 21 // To make this useful we should log as many packets as we can fit characters a single line of a OLED display.