diff --git a/docs/Settings.md b/docs/Settings.md
index ccab19fc074..dda075cee64 100644
--- a/docs/Settings.md
+++ b/docs/Settings.md
@@ -1432,6 +1432,16 @@ Navigation update rate for UBLOX7 receivers. Some receivers may limit the maximu
---
+### gps_ublox_use_beidou
+
+Enable use of Beidou satellites. This is at the expense of other regional constellations, so benefit may also be regional. Requires gps hardware support [OFF/ON].
+
+| Default | Min | Max |
+| --- | --- | --- |
+| OFF | OFF | ON |
+
+---
+
### gps_ublox_use_galileo
Enable use of Galileo satellites. This is at the expense of other regional constellations, so benefit may also be regional. Requires M8N and Ublox firmware 3.x (or later) [OFF/ON].
@@ -1442,6 +1452,16 @@ Enable use of Galileo satellites. This is at the expense of other regional const
---
+### gps_ublox_use_glonass
+
+Enable use of Glonass satellites. This is at the expense of other regional constellations, so benefit may also be regional. Requires gps haardware support [OFF/ON].
+
+| Default | Min | Max |
+| --- | --- | --- |
+| OFF | OFF | ON |
+
+---
+
### ground_test_mode
For developer ground test use. Disables motors, sets heading status = Trusted on FW.
diff --git a/src/main/CMakeLists.txt b/src/main/CMakeLists.txt
index 2a311443544..e67ea9629db 100644
--- a/src/main/CMakeLists.txt
+++ b/src/main/CMakeLists.txt
@@ -499,6 +499,7 @@ main_sources(COMMON_SRC
io/gps.c
io/gps.h
io/gps_ublox.c
+ io/gps_ublox_utils.c
io/gps_nmea.c
io/gps_msp.c
io/gps_fake.c
diff --git a/src/main/fc/cli.c b/src/main/fc/cli.c
index 1708c796c7b..a14aac38a28 100644
--- a/src/main/fc/cli.c
+++ b/src/main/fc/cli.c
@@ -89,6 +89,7 @@ bool cliMode = false;
#include "io/beeper.h"
#include "io/flashfs.h"
#include "io/gps.h"
+#include "io/gps_ublox.h"
#include "io/ledstrip.h"
#include "io/osd.h"
#include "io/serial.h"
@@ -3468,6 +3469,23 @@ static void cliStatus(char *cmdline)
cliPrintLinefeed();
#endif
+ if (featureConfigured(FEATURE_GPS) && (gpsConfig()->provider == GPS_UBLOX || gpsConfig()->provider == GPS_UBLOX7PLUS)) {
+ cliPrint("GPS: ");
+ cliPrintf("HW Version: %s Proto: %d.%02d Baud: %d", getGpsHwVersion(), getGpsProtoMajorVersion(), getGpsProtoMinorVersion(), getGpsBaudrate());
+ cliPrintLinefeed();
+ //cliPrintLinef(" GNSS Capabilities: %d", gpsUbloxCapLastUpdate());
+ cliPrintLinef(" GNSS Capabilities:");
+ cliPrintLine(" GNSS Provider active/default");
+ cliPrintLine(" GPS 1/1");
+ if(gpsUbloxHasGalileo())
+ cliPrintLinef(" Galileo %d/%d", gpsUbloxGalileoEnabled(), gpsUbloxGalileoDefault());
+ if(gpsUbloxHasBeidou())
+ cliPrintLinef(" BeiDou %d/%d", gpsUbloxBeidouEnabled(), gpsUbloxBeidouDefault());
+ if(gpsUbloxHasGlonass())
+ cliPrintLinef(" Glonass %d/%d", gpsUbloxGlonassEnabled(), gpsUbloxGlonassDefault());
+ cliPrintLinef(" Max concurrent: %d", gpsUbloxMaxGnss());
+ }
+
// If we are blocked by PWM init - provide more information
if (getPwmInitError() != PWM_INIT_ERROR_NONE) {
cliPrintLinef("PWM output init error: %s", getPwmInitErrorMessage());
diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml
index 70d45f6ead3..a0904af6f32 100644
--- a/src/main/fc/settings.yaml
+++ b/src/main/fc/settings.yaml
@@ -1537,6 +1537,16 @@ groups:
default_value: OFF
field: ubloxUseGalileo
type: bool
+ - name: gps_ublox_use_beidou
+ description: "Enable use of Beidou satellites. This is at the expense of other regional constellations, so benefit may also be regional. Requires gps hardware support [OFF/ON]."
+ default_value: OFF
+ field: ubloxUseBeidou
+ type: bool
+ - name: gps_ublox_use_glonass
+ description: "Enable use of Glonass satellites. This is at the expense of other regional constellations, so benefit may also be regional. Requires gps haardware support [OFF/ON]."
+ default_value: OFF
+ field: ubloxUseGlonass
+ type: bool
- name: gps_min_sats
description: "Minimum number of GPS satellites in view to acquire GPS_FIX and consider GPS position valid. Some GPS receivers appeared to be very inaccurate with low satellite count."
default_value: 6
diff --git a/src/main/io/gps.c b/src/main/io/gps.c
index 3dc1df7bd17..fa225110b46 100755
--- a/src/main/io/gps.c
+++ b/src/main/io/gps.c
@@ -51,6 +51,7 @@
#include "io/serial.h"
#include "io/gps.h"
#include "io/gps_private.h"
+#include "io/gps_ublox.h"
#include "navigation/navigation.h"
@@ -122,9 +123,64 @@ PG_RESET_TEMPLATE(gpsConfig_t, gpsConfig,
.dynModel = SETTING_GPS_DYN_MODEL_DEFAULT,
.gpsMinSats = SETTING_GPS_MIN_SATS_DEFAULT,
.ubloxUseGalileo = SETTING_GPS_UBLOX_USE_GALILEO_DEFAULT,
+ .ubloxUseBeidou = SETTING_GPS_UBLOX_USE_BEIDOU_DEFAULT,
+ .ubloxUseGlonass = SETTING_GPS_UBLOX_USE_GLONASS_DEFAULT,
.ubloxNavHz = SETTING_GPS_UBLOX_NAV_HZ_DEFAULT
);
+
+int getGpsBaudrate(void)
+{
+ switch(gpsState.baudrateIndex)
+ {
+ case GPS_BAUDRATE_115200:
+ return 115200;
+ case GPS_BAUDRATE_57600:
+ return 57600;
+ case GPS_BAUDRATE_38400:
+ return 38400;
+ case GPS_BAUDRATE_19200:
+ return 19200;
+ case GPS_BAUDRATE_9600:
+ return 9600;
+ case GPS_BAUDRATE_230400:
+ return 230400;
+ default:
+ return 0;
+ }
+}
+
+const char *getGpsHwVersion(void)
+{
+ switch(gpsState.hwVersion)
+ {
+ case UBX_HW_VERSION_UBLOX5:
+ return "UBLOX5";
+ case UBX_HW_VERSION_UBLOX6:
+ return "UBLOX6";
+ case UBX_HW_VERSION_UBLOX7:
+ return "UBLOX7";
+ case UBX_HW_VERSION_UBLOX8:
+ return "UBLOX8";
+ case UBX_HW_VERSION_UBLOX9:
+ return "UBLOX9";
+ case UBX_HW_VERSION_UBLOX10:
+ return "UBLOX10";
+ default:
+ return "Unknown";
+ }
+}
+
+uint8_t getGpsProtoMajorVersion(void)
+{
+ return gpsState.swVersionMajor;
+}
+
+uint8_t getGpsProtoMinorVersion(void)
+{
+ return gpsState.swVersionMinor;
+}
+
void gpsSetState(gpsState_e state)
{
gpsState.state = state;
diff --git a/src/main/io/gps.h b/src/main/io/gps.h
index ddbc191eb49..0e0780dea3e 100755
--- a/src/main/io/gps.h
+++ b/src/main/io/gps.h
@@ -93,6 +93,8 @@ typedef struct gpsConfig_s {
gpsAutoBaud_e autoBaud;
gpsDynModel_e dynModel;
bool ubloxUseGalileo;
+ bool ubloxUseBeidou;
+ bool ubloxUseGlonass;
uint8_t gpsMinSats;
uint8_t ubloxNavHz;
} gpsConfig_t;
@@ -166,6 +168,12 @@ struct serialPort_s;
void gpsEnablePassthrough(struct serialPort_s *gpsPassthroughPort);
void mspGPSReceiveNewData(const uint8_t * bufferPtr);
+const char *getGpsHwVersion(void);
+uint8_t getGpsProtoMajorVersion(void);
+uint8_t getGpsProtoMinorVersion(void);
+
+int getGpsBaudrate(void);
+
#if defined(USE_GPS_FAKE)
void gpsFakeSet(
gpsFixType_e fixType,
diff --git a/src/main/io/gps_private.h b/src/main/io/gps_private.h
index 6e9abc72ffe..765318d1fce 100755
--- a/src/main/io/gps_private.h
+++ b/src/main/io/gps_private.h
@@ -43,6 +43,8 @@ typedef struct {
serialPort_t * gpsPort; // Serial GPS only
uint32_t hwVersion;
+ uint8_t swVersionMajor;
+ uint8_t swVersionMinor;
gpsState_e state;
gpsBaudRate_e baudrateIndex;
@@ -54,6 +56,8 @@ typedef struct {
timeMs_t lastMessageMs;
timeMs_t timeoutMs;
timeMs_t baseTimeoutMs;
+ timeMs_t lastCapaPoolMs;
+ timeMs_t lastCapaUpdMs;
} gpsReceiverData_t;
extern gpsReceiverData_t gpsState;
diff --git a/src/main/io/gps_ublox.c b/src/main/io/gps_ublox.c
index a9666d07341..9b678a36aff 100755
--- a/src/main/io/gps_ublox.c
+++ b/src/main/io/gps_ublox.c
@@ -42,6 +42,7 @@
#include "fc/config.h"
#include "fc/runtime_config.h"
+#include "fc/settings.h"
#include "io/serial.h"
#include "io/gps.h"
@@ -49,31 +50,9 @@
#include "scheduler/protothreads.h"
-#define GPS_CFG_CMD_TIMEOUT_MS 200
-#define GPS_VERSION_RETRY_TIMES 2
-#define MAX_UBLOX_PAYLOAD_SIZE 256
-#define UBLOX_BUFFER_SIZE MAX_UBLOX_PAYLOAD_SIZE
-#define UBLOX_SBAS_MESSAGE_LENGTH 16
+#include "gps_ublox.h"
+#include "gps_ublox_utils.h"
-#define UBX_DYNMODEL_PEDESTRIAN 3
-#define UBX_DYNMODEL_AIR_1G 6
-#define UBX_DYNMODEL_AIR_4G 8
-
-#define UBX_FIXMODE_2D_ONLY 1
-#define UBX_FIXMODE_3D_ONLY 2
-#define UBX_FIXMODE_AUTO 3
-
-#define UBX_VALID_GPS_DATE(valid) (valid & 1 << 0)
-#define UBX_VALID_GPS_TIME(valid) (valid & 1 << 1)
-#define UBX_VALID_GPS_DATE_TIME(valid) (UBX_VALID_GPS_DATE(valid) && UBX_VALID_GPS_TIME(valid))
-
-#define UBX_HW_VERSION_UNKNOWN 0
-#define UBX_HW_VERSION_UBLOX5 500
-#define UBX_HW_VERSION_UBLOX6 600
-#define UBX_HW_VERSION_UBLOX7 700
-#define UBX_HW_VERSION_UBLOX8 800
-#define UBX_HW_VERSION_UBLOX9 900
-#define UBX_HW_VERSION_UBLOX10 1000
// SBAS_AUTO, SBAS_EGNOS, SBAS_WAAS, SBAS_MSAS, SBAS_GAGAN, SBAS_NONE
// note PRNs last upadted 2020-12-18
@@ -99,253 +78,6 @@ static const char * baudInitDataNMEA[GPS_BAUDRATE_COUNT] = {
"$PUBX,41,1,0003,0001,230400,0*1C\r\n", // GPS_BAUDRATE_230400
};
-// payload types
-typedef struct {
- uint8_t mode;
- uint8_t usage;
- uint8_t maxSBAS;
- uint8_t scanmode2;
- uint32_t scanmode1;
-} ubx_sbas;
-
-typedef struct {
- uint8_t class;
- uint8_t id;
- uint8_t rate;
-} ubx_msg;
-
-typedef struct {
- uint16_t meas;
- uint16_t nav;
- 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[MAX_GNSS_SIZE_BYTES]; // placeholder
- ubx_sbas sbas;
- ubx_msg msg;
- ubx_rate rate;
- ubx_gnss_msg_t gnss;
-} ubx_payload;
-
-// 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 {
- ubx_header header;
- ubx_payload payload;
-} __attribute__((packed)) ubx_message;
-
-typedef struct {
- char swVersion[30]; // Zero-terminated Software Version String
- char hwVersion[10]; // Zero-terminated Hardware Version String
-} ubx_mon_ver;
-
-typedef struct {
- uint32_t time; // GPS msToW
- int32_t longitude;
- int32_t latitude;
- int32_t altitude_ellipsoid;
- int32_t altitude_msl;
- uint32_t horizontal_accuracy;
- uint32_t vertical_accuracy;
-} ubx_nav_posllh;
-
-typedef struct {
- uint32_t time; // GPS msToW
- uint8_t fix_type;
- uint8_t fix_status;
- uint8_t differential_status;
- uint8_t res;
- uint32_t time_to_first_fix;
- uint32_t uptime; // milliseconds
-} ubx_nav_status;
-
-typedef struct {
- uint32_t time;
- int32_t time_nsec;
- int16_t week;
- uint8_t fix_type;
- uint8_t fix_status;
- int32_t ecef_x;
- int32_t ecef_y;
- int32_t ecef_z;
- uint32_t position_accuracy_3d;
- int32_t ecef_x_velocity;
- int32_t ecef_y_velocity;
- int32_t ecef_z_velocity;
- uint32_t speed_accuracy;
- uint16_t position_DOP;
- uint8_t res;
- uint8_t satellites;
- uint32_t res2;
-} ubx_nav_solution;
-
-typedef struct {
- uint32_t time; // GPS msToW
- int32_t ned_north;
- int32_t ned_east;
- int32_t ned_down;
- uint32_t speed_3d;
- uint32_t speed_2d;
- int32_t heading_2d;
- uint32_t speed_accuracy;
- uint32_t heading_accuracy;
-} ubx_nav_velned;
-
-typedef struct {
- uint8_t chn; // Channel number, 255 for SVx not assigned to channel
- uint8_t svid; // Satellite ID
- uint8_t flags; // Bitmask
- uint8_t quality; // Bitfield
- uint8_t cno; // Carrier to Noise Ratio (Signal Strength) // dbHz, 0-55.
- uint8_t elev; // Elevation in integer degrees
- int16_t azim; // Azimuth in integer degrees
- int32_t prRes; // Pseudo range residual in centimetres
-} ubx_nav_svinfo_channel;
-
-typedef struct {
- uint32_t time; // GPS Millisecond time of week
- uint8_t numCh; // Number of channels
- uint8_t globalFlags; // Bitmask, Chip hardware generation 0:Antaris, 1:u-blox 5, 2:u-blox 6
- uint16_t reserved2; // Reserved
- ubx_nav_svinfo_channel channel[16]; // 16 satellites * 12 byte
-} ubx_nav_svinfo;
-
-typedef struct {
- uint32_t time; // GPS msToW
- uint32_t tAcc;
- int32_t nano;
- uint16_t year;
- uint8_t month;
- uint8_t day;
- uint8_t hour;
- uint8_t min;
- uint8_t sec;
- uint8_t valid;
-} ubx_nav_timeutc;
-
-typedef struct {
- uint32_t time; // GPS msToW
- uint16_t year;
- uint8_t month;
- uint8_t day;
- uint8_t hour;
- uint8_t min;
- uint8_t sec;
- uint8_t valid;
- uint32_t tAcc;
- int32_t nano;
- uint8_t fix_type;
- uint8_t fix_status;
- uint8_t reserved1;
- uint8_t satellites;
- int32_t longitude;
- int32_t latitude;
- int32_t altitude_ellipsoid;
- int32_t altitude_msl;
- uint32_t horizontal_accuracy;
- uint32_t vertical_accuracy;
- int32_t ned_north;
- int32_t ned_east;
- int32_t ned_down;
- int32_t speed_2d;
- int32_t heading_2d;
- uint32_t speed_accuracy;
- uint32_t heading_accuracy;
- uint16_t position_DOP;
- uint16_t reserved2;
- uint16_t reserved3;
-} ubx_nav_pvt;
-
-typedef struct {
- uint8_t class;
- uint8_t msg;
-} ubx_ack_ack;
-
-enum {
- PREAMBLE1 = 0xB5,
- PREAMBLE2 = 0x62,
- CLASS_NAV = 0x01,
- CLASS_ACK = 0x05,
- CLASS_CFG = 0x06,
- CLASS_MON = 0x0A,
- MSG_CLASS_UBX = 0x01,
- MSG_CLASS_NMEA = 0xF0,
- MSG_VER = 0x04,
- MSG_ACK_NACK = 0x00,
- MSG_ACK_ACK = 0x01,
- MSG_NMEA_GGA = 0x0,
- MSG_NMEA_GLL = 0x1,
- MSG_NMEA_GSA = 0x2,
- MSG_NMEA_GSV = 0x3,
- MSG_NMEA_RMC = 0x4,
- MSG_NMEA_VGS = 0x5,
- MSG_POSLLH = 0x2,
- MSG_STATUS = 0x3,
- MSG_SOL = 0x6,
- MSG_PVT = 0x7,
- MSG_VELNED = 0x12,
- MSG_TIMEUTC = 0x21,
- MSG_SVINFO = 0x30,
- MSG_NAV_SAT = 0x35,
- MSG_NAV_SIG = 0x35,
- MSG_CFG_PRT = 0x00,
- MSG_CFG_RATE = 0x08,
- MSG_CFG_SET_RATE = 0x01,
- MSG_CFG_NAV_SETTINGS = 0x24,
- MSG_CFG_SBAS = 0x16,
- MSG_CFG_GNSS = 0x3e
-} ubx_protocol_bytes;
-
-enum {
- FIX_NONE = 0,
- FIX_DEAD_RECKONING = 1,
- FIX_2D = 2,
- FIX_3D = 3,
- FIX_GPS_DEAD_RECKONING = 4,
- FIX_TIME = 5
-} ubs_nav_fix_type;
-
-enum {
- NAV_STATUS_FIX_VALID = 1
-} ubx_nav_status_bits;
-
-enum {
- UBX_ACK_WAITING = 0,
- UBX_ACK_GOT_ACK = 1,
- UBX_ACK_GOT_NAK = 2
-} ubx_ack_state;
-
// Packet checksum accumulators
static uint8_t _ck_a;
static uint8_t _ck_b;
@@ -369,7 +101,12 @@ static bool _new_position;
static bool _new_speed;
// Need this to determine if Galileo capable only
-static bool capGalileo;
+static struct {
+ uint8_t supported;
+ int capMaxGnss;
+ uint8_t defaultGnss;
+ uint8_t enabledGnss;
+} ubx_capabilities = { };
// Example packet sizes from UBlox u-center from a Glonass capable GPS receiver.
//15:17:55 R -> UBX NAV-STATUS, Size 24, 'Navigation Status'
@@ -401,16 +138,63 @@ static union {
ubx_mon_ver ver;
ubx_nav_timeutc timeutc;
ubx_ack_ack ack;
+ ubx_mon_gnss gnss;
uint8_t bytes[UBLOX_BUFFER_SIZE];
} _buffer;
-void _update_checksum(uint8_t *data, uint8_t len, uint8_t *ck_a, uint8_t *ck_b)
+bool gpsUbloxHasGalileo(void)
{
- while (len--) {
- *ck_a += *data;
- *ck_b += *ck_a;
- data++;
- }
+ return (ubx_capabilities.supported & UBX_MON_GNSS_GALILEO_MASK);
+}
+
+bool gpsUbloxHasBeidou(void)
+{
+ return ubx_capabilities.supported & UBX_MON_GNSS_BEIDOU_MASK;
+}
+
+bool gpsUbloxHasGlonass(void)
+{
+ return ubx_capabilities.supported & UBX_MON_GNSS_GLONASS_MASK;
+}
+
+bool gpsUbloxGalileoDefault(void)
+{
+ return ubx_capabilities.defaultGnss & UBX_MON_GNSS_GALILEO_MASK;
+}
+
+bool gpsUbloxBeidouDefault(void)
+{
+ return ubx_capabilities.defaultGnss & UBX_MON_GNSS_BEIDOU_MASK;
+}
+
+bool gpsUbloxGlonassDefault(void)
+{
+ return ubx_capabilities.defaultGnss & UBX_MON_GNSS_GLONASS_MASK;
+}
+
+bool gpsUbloxGalileoEnabled(void)
+{
+ return ubx_capabilities.enabledGnss & UBX_MON_GNSS_GALILEO_MASK;
+}
+
+bool gpsUbloxBeidouEnabled(void)
+{
+ return ubx_capabilities.enabledGnss & UBX_MON_GNSS_BEIDOU_MASK;
+}
+
+bool gpsUbloxGlonassEnabled(void)
+{
+ return ubx_capabilities.enabledGnss & UBX_MON_GNSS_GLONASS_MASK;
+}
+
+uint8_t gpsUbloxMaxGnss(void)
+{
+ return ubx_capabilities.capMaxGnss;
+}
+
+timeMs_t gpsUbloxCapLastUpdate(void)
+{
+ return gpsState.lastCapaUpdMs;
}
static uint8_t gpsMapFixType(bool fixValid, uint8_t ubloxFixType)
@@ -427,7 +211,7 @@ static void sendConfigMessageUBLOX(void)
uint8_t ck_a=0, ck_b=0;
send_buffer.message.header.preamble1=PREAMBLE1;
send_buffer.message.header.preamble2=PREAMBLE2;
- _update_checksum(&send_buffer.bytes[2], send_buffer.message.header.length+4, &ck_a, &ck_b);
+ ublox_update_checksum(&send_buffer.bytes[2], send_buffer.message.header.length+4, &ck_a, &ck_b);
send_buffer.bytes[send_buffer.message.header.length+6] = ck_a;
send_buffer.bytes[send_buffer.message.header.length+7] = ck_b;
serialWriteBuf(gpsState.gpsPort, send_buffer.bytes, send_buffer.message.header.length+8);
@@ -445,6 +229,15 @@ static void pollVersion(void)
sendConfigMessageUBLOX();
}
+static void pollGnssCapabilities(void)
+{
+ send_buffer.message.header.msg_class = CLASS_MON;
+ send_buffer.message.header.msg_id = MSG_MON_GNSS;
+ send_buffer.message.header.length = 0;
+ sendConfigMessageUBLOX();
+}
+
+
static const uint8_t default_payload[] = {
0xFF, 0xFF, 0x03, 0x03, 0x00, // CFG-NAV5 - Set engine settings (original MWII code)
0x00, 0x00, 0x00, 0x10, 0x27, 0x00, 0x00, 0x05, 0x00, 0xFA, 0x00, // Collected by resetting a GPS unit to defaults. Changing mode to Pedistrian and
@@ -452,9 +245,28 @@ static const uint8_t default_payload[] = {
0x00, 0xC8, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00
};
-#define GNSSID_SBAS 1
-#define GNSSID_GALILEO 2
+#define GNSSID_SBAS 1
+#define GNSSID_GALILEO 2
+#define GNSSID_BEIDOU 3
+#define GNSSID_GZSS 5
+#define GNSSID_GLONASS 6
+
+// M10 ublox protocol info:
+// https://content.u-blox.com/sites/default/files/u-blox-M10-SPG-5.10_InterfaceDescription_UBX-21035062.pdf
+static void ubloxSendSetCfgBytes(ubx_config_data8_payload_t *kvPairs, uint8_t count)
+{
+ ubx_config_data8_t cfg = {};
+
+ ubloxCfgFillBytes(&cfg, kvPairs, count);
+ serialWriteBuf(gpsState.gpsPort, (uint8_t *)&cfg, cfg.header.length+8);
+ _ack_waiting_msg = cfg.header.msg_id;
+ _ack_state = UBX_ACK_WAITING;
+}
+
+// Info on protocol used by M8-M9, check UBX-CFG-GNSS for gnss configuration
+// https://content.u-blox.com/sites/default/files/products/documents/u-blox8-M8_ReceiverDescrProtSpec_UBX-13003221.pdf
+// https://content.u-blox.com/sites/default/files/documents/u-blox-F9-HPG-1.32_InterfaceDescription_UBX-22008968.pdf
static int configureGNSS_SBAS(ubx_gnss_element_t * gnss_block)
{
gnss_block->gnssId = GNSSID_SBAS;
@@ -473,13 +285,17 @@ static int configureGNSS_SBAS(ubx_gnss_element_t * gnss_block)
static int configureGNSS_GALILEO(ubx_gnss_element_t * gnss_block)
{
- if (!capGalileo) {
+ if (!gpsUbloxHasGalileo()) {
return 0;
}
gnss_block->gnssId = GNSSID_GALILEO;
gnss_block->maxTrkCh = 8;
- gnss_block->sigCfgMask = 1;
+ // sigCfgMask
+ // 0x01 = Galileo E1 (not supported for protocol versions less than 18.00)
+ // 0x10 = Galileo E5a // off by default
+ // 0x20 = Galileo E5b // off by default
+ gnss_block->sigCfgMask = 0x01;
if (gpsState.gpsConfig->ubloxUseGalileo) {
gnss_block->enabled = 1;
gnss_block->resTrkCh = 4;
@@ -491,25 +307,125 @@ static int configureGNSS_GALILEO(ubx_gnss_element_t * gnss_block)
return 1;
}
+static int configureGNSS_BEIDOU(ubx_gnss_element_t * gnss_block)
+{
+ if (!gpsUbloxHasBeidou()) {
+ return 0;
+ }
+
+ gnss_block->gnssId = GNSSID_BEIDOU;
+ gnss_block->maxTrkCh = 8;
+ // sigCfgMask
+ // 0x01 = BeiDou B1I
+ // 0x10 = BeiDou B2I // off by default
+ // 0x80 = BeiDou B2A // off by default
+ gnss_block->sigCfgMask = 0x01;
+ if (gpsState.gpsConfig->ubloxUseBeidou) {
+ gnss_block->enabled = 1;
+ gnss_block->resTrkCh = 4;
+ } else {
+ gnss_block->enabled = 0;
+ gnss_block->resTrkCh = 0;
+ }
+
+ return 1;
+}
+
+/*
+static int configureGNSS_GZSS(ubx_gnss_element_t * gnss_block)
+{
+ if (!ubx_capabilities.capGzss) {
+ return 0;
+ }
+
+ gnss_block->gnssId = GNSSID_GZSS;
+ gnss_block->maxTrkCh = 8;
+ // L1C = 0x01
+ // L1S = 0x04
+ // L2C = 0x10
+ gnss_block->sigCfgMask = 0x01 | 0x04;
+ gnss_block->enabled = 1;
+ gnss_block->resTrkCh = 4;
+
+ return 1;
+}
+*/
+
+static int configureGNSS_GLONASS(ubx_gnss_element_t * gnss_block)
+{
+ if(!gpsUbloxHasGlonass()) {
+ return 0;
+ }
+
+ gnss_block->gnssId = GNSSID_GLONASS;
+ gnss_block->maxTrkCh = 8;
+ // 0x01 = GLONASS L1
+ // 0x10 = GLONASS L2 // off by default
+ gnss_block->sigCfgMask = 0x01;
+ if (gpsState.gpsConfig->ubloxUseGlonass) {
+ gnss_block->enabled = 1;
+ gnss_block->resTrkCh = 4;
+ } else {
+ gnss_block->enabled = 0;
+ gnss_block->resTrkCh = 0;
+ }
+
+ return 1;
+}
+
+static void configureGNSS10(void)
+{
+ ubx_config_data8_payload_t gnssConfigValues[] = {
+ // SBAS
+ {UBLOX_CFG_SIGNAL_SBAS_ENA, 1},
+ {UBLOX_CFG_SIGNAL_SBAS_L1CA_ENA, 1},
+
+ // Galileo
+ {UBLOX_CFG_SIGNAL_GAL_ENA, gpsState.gpsConfig->ubloxUseGalileo},
+ {UBLOX_CFG_SIGNAL_GAL_E1_ENA, gpsState.gpsConfig->ubloxUseGalileo},
+
+ // Beidou
+ {UBLOX_CFG_SIGNAL_BDS_ENA, gpsState.gpsConfig->ubloxUseBeidou},
+ {UBLOX_CFG_SIGNAL_BDS_B1_ENA, gpsState.gpsConfig->ubloxUseBeidou},
+ {UBLOX_CFG_SIGNAL_BDS_B1C_ENA, 0},
+
+ // Should be enabled with GPS
+ {UBLOX_CFG_QZSS_ENA, 1},
+ {UBLOX_CFG_QZSS_L1CA_ENA, 1},
+ {UBLOX_CFG_QZSS_L1S_ENA, 1},
+
+ // Glonass
+ {UBLOX_CFG_GLO_ENA, gpsState.gpsConfig->ubloxUseGlonass},
+ {UBLOX_CFG_GLO_L1_ENA, gpsState.gpsConfig->ubloxUseGlonass}
+ };
+
+ ubloxSendSetCfgBytes(gnssConfigValues, 12);
+}
+
static void configureGNSS(void)
{
- int blocksUsed = 0;
+ int blocksUsed = 0;
+ send_buffer.message.header.msg_class = CLASS_CFG;
+ send_buffer.message.header.msg_id = MSG_CFG_GNSS; // message deprecated in protocol > 23.01, should use UBX-CFG-VALSET/UBX-CFG-VALGET
+ send_buffer.message.payload.gnss.msgVer = 0;
+ send_buffer.message.payload.gnss.numTrkChHw = 0; // read only, so unset
+ send_buffer.message.payload.gnss.numTrkChUse = 0xFF; // If set to 0xFF will use hardware max
- send_buffer.message.header.msg_class = CLASS_CFG;
- send_buffer.message.header.msg_id = MSG_CFG_GNSS;
- 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]);
- /* SBAS, always generated */
- blocksUsed += configureGNSS_SBAS(&send_buffer.message.payload.gnss.config[blocksUsed]);
+ /* Galileo */
+ blocksUsed += configureGNSS_GALILEO(&send_buffer.message.payload.gnss.config[blocksUsed]);
- /* Galileo */
- blocksUsed += configureGNSS_GALILEO(&send_buffer.message.payload.gnss.config[blocksUsed]);
+ /* BeiDou */
+ blocksUsed += configureGNSS_BEIDOU(&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();
+ /* GLONASS */
+ blocksUsed += configureGNSS_GLONASS(&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();
}
static void configureNAV5(uint8_t dynModel, uint8_t fixMode)
@@ -523,12 +439,12 @@ static void configureNAV5(uint8_t dynModel, uint8_t fixMode)
sendConfigMessageUBLOX();
}
-static void configureMSG(uint8_t class, uint8_t id, uint8_t rate)
+static void configureMSG(uint8_t msg_class, uint8_t id, uint8_t rate)
{
send_buffer.message.header.msg_class = CLASS_CFG;
send_buffer.message.header.msg_id = MSG_CFG_SET_RATE;
send_buffer.message.header.length = 3;
- send_buffer.message.payload.msg.class = class;
+ send_buffer.message.payload.msg.msg_class = msg_class;
send_buffer.message.payload.msg.id = id;
send_buffer.message.payload.msg.rate = rate;
sendConfigMessageUBLOX();
@@ -565,6 +481,18 @@ static void configureSBAS(void)
sendConfigMessageUBLOX();
}
+static void gpsDecodeProtocolVersion(const char *proto, size_t bufferLength)
+{
+ if (bufferLength > 13 && !strncmp(proto, "PROTVER=", 8)) {
+ proto+=8;
+
+ float ver = atof(proto);
+
+ gpsState.swVersionMajor = (uint8_t)ver;
+ gpsState.swVersionMinor = (uint8_t)((ver - gpsState.swVersionMajor) * 100.0f);
+ }
+}
+
static uint32_t gpsDecodeHardwareVersion(const char * szBuf, unsigned nBufSize)
{
// ublox_5 hwVersion 00040005
@@ -690,18 +618,51 @@ static bool gpsParceFrameUBLOX(void)
case MSG_VER:
if (_class == CLASS_MON) {
gpsState.hwVersion = gpsDecodeHardwareVersion(_buffer.ver.hwVersion, sizeof(_buffer.ver.hwVersion));
- if ((gpsState.hwVersion >= UBX_HW_VERSION_UBLOX8) && (_buffer.ver.swVersion[9] > '2')) {
- // check extensions;
- // after hw + sw vers; each is 30 bytes
- for(int j = 40; j < _payload_length; j += 30) {
- if (strnstr((const char *)(_buffer.bytes+j), "GAL", 30)) {
- capGalileo = true;
+ if (gpsState.hwVersion >= UBX_HW_VERSION_UBLOX8) {
+ if (_buffer.ver.swVersion[9] > '2' || true) {
+ // check extensions;
+ // after hw + sw vers; each is 30 bytes
+ bool found = false;
+ for (int j = 40; j < _payload_length && !found; j += 30)
+ {
+ // Example content: GPS;GAL;BDS;GLO
+ if (strnstr((const char *)(_buffer.bytes + j), "GAL", 30))
+ {
+ ubx_capabilities.supported |= UBX_MON_GNSS_GALILEO_MASK;
+ found = true;
+ }
+ if (strnstr((const char *)(_buffer.bytes + j), "BDS", 30))
+ {
+ ubx_capabilities.supported |= UBX_MON_GNSS_BEIDOU_MASK;
+ found = true;
+ }
+ if (strnstr((const char *)(_buffer.bytes + j), "GLO", 30))
+ {
+ ubx_capabilities.supported |= UBX_MON_GNSS_GLONASS_MASK;
+ found = true;
+ }
+ }
+ }
+ for (int j = 40; j < _payload_length; j += 30) {
+ if (strnstr((const char *)(_buffer.bytes + j), "PROTVER=", 30)) {
+ gpsDecodeProtocolVersion((const char *)(_buffer.bytes + j), 30);
break;
}
}
}
}
break;
+ case MSG_MON_GNSS:
+ if(_class == CLASS_MON) {
+ if (_buffer.gnss.version == 0) {
+ ubx_capabilities.supported = _buffer.gnss.supported;
+ ubx_capabilities.defaultGnss = _buffer.gnss.defaultGnss;
+ ubx_capabilities.enabledGnss = _buffer.gnss.enabled;
+ ubx_capabilities.capMaxGnss = _buffer.gnss.maxConcurrent;
+ gpsState.lastCapaUpdMs = millis();
+ }
+ }
+ break;
case MSG_ACK_ACK:
if ((_ack_state == UBX_ACK_WAITING) && (_buffer.ack.msg == _ack_waiting_msg)) {
_ack_state = UBX_ACK_GOT_ACK;
@@ -893,12 +854,13 @@ STATIC_PROTOTHREAD(gpsConfigure)
if ((gpsState.gpsConfig->provider == GPS_UBLOX7PLUS) && (gpsState.hwVersion >= UBX_HW_VERSION_UBLOX7)) {
configureRATE(hz2rate(gpsState.gpsConfig->ubloxNavHz)); // default 10Hz
} else {
- configureRATE(200); // 5Hz
+ configureRATE(hz2rate(5)); // 5Hz
+ gpsConfigMutable()->ubloxNavHz = SETTING_GPS_UBLOX_NAV_HZ_DEFAULT;
}
ptWait(_ack_state == UBX_ACK_GOT_ACK || _ack_state == UBX_ACK_GOT_NAK);
if(_ack_state == UBX_ACK_GOT_NAK) { // Fallback to safe 5Hz in case of error
- configureRATE(200); // 5Hz
+ configureRATE(hz2rate(5)); // 5Hz
ptWait(_ack_state == UBX_ACK_GOT_ACK);
}
}
@@ -931,12 +893,12 @@ STATIC_PROTOTHREAD(gpsConfigure)
configureRATE(hz2rate(gpsState.gpsConfig->ubloxNavHz)); // default 10Hz
}
else {
- configureRATE(200); // 5Hz
+ configureRATE(hz2rate(5)); // 5Hz
}
ptWait(_ack_state == UBX_ACK_GOT_ACK || _ack_state == UBX_ACK_GOT_NAK);
if(_ack_state == UBX_ACK_GOT_NAK) { // Fallback to safe 5Hz in case of error
- configureRATE(200); // 5Hz
+ configureRATE(hz2rate(5)); // 5Hz
ptWait(_ack_state == UBX_ACK_GOT_ACK);
}
}
@@ -981,8 +943,18 @@ STATIC_PROTOTHREAD(gpsConfigure)
// Configure GNSS for M8N and later
if (gpsState.hwVersion >= UBX_HW_VERSION_UBLOX8) {
gpsSetProtocolTimeout(GPS_SHORT_TIMEOUT);
- configureGNSS();
+ if(gpsState.hwVersion >= UBX_HW_VERSION_UBLOX10 || (gpsState.swVersionMajor>=23 && gpsState.swVersionMinor >= 1)) {
+ configureGNSS10();
+ } else {
+ configureGNSS();
+ }
ptWaitTimeout((_ack_state == UBX_ACK_GOT_ACK || _ack_state == UBX_ACK_GOT_NAK), GPS_CFG_CMD_TIMEOUT_MS);
+
+ if(_ack_state == UBX_ACK_GOT_NAK) {
+ gpsConfigMutable()->ubloxUseGalileo = SETTING_GPS_UBLOX_USE_GALILEO_DEFAULT;
+ gpsConfigMutable()->ubloxUseBeidou = SETTING_GPS_UBLOX_USE_BEIDOU_DEFAULT;
+ gpsConfigMutable()->ubloxUseGlonass = SETTING_GPS_UBLOX_USE_GLONASS_DEFAULT;
+ }
}
ptEnd(0);
@@ -1044,21 +1016,29 @@ STATIC_PROTOTHREAD(gpsProtocolStateThread)
serialSetBaudRate(gpsState.gpsPort, baudRates[gpsToSerialBaudRate[gpsState.baudrateIndex]]);
}
- // Configure GPS module if enabled
- if (gpsState.gpsConfig->autoConfig) {
- // Reset protocol timeout
- gpsSetProtocolTimeout(MAX(GPS_TIMEOUT, ((GPS_VERSION_RETRY_TIMES + 3) * GPS_CFG_CMD_TIMEOUT_MS)));
+ // Reset protocol timeout
+ gpsSetProtocolTimeout(MAX(GPS_TIMEOUT, ((GPS_VERSION_RETRY_TIMES + 3) * GPS_CFG_CMD_TIMEOUT_MS)));
+
+ // Attempt to detect GPS hw version
+ gpsState.hwVersion = UBX_HW_VERSION_UNKNOWN;
+ gpsState.autoConfigStep = 0;
- // Attempt to detect GPS hw version
- gpsState.hwVersion = UBX_HW_VERSION_UNKNOWN;
- gpsState.autoConfigStep = 0;
+ do {
+ pollVersion();
+ gpsState.autoConfigStep++;
+ ptWaitTimeout((gpsState.hwVersion != UBX_HW_VERSION_UNKNOWN), GPS_CFG_CMD_TIMEOUT_MS);
+ } while (gpsState.autoConfigStep < GPS_VERSION_RETRY_TIMES && gpsState.hwVersion == UBX_HW_VERSION_UNKNOWN);
- do {
- pollVersion();
- gpsState.autoConfigStep++;
- ptWaitTimeout((gpsState.hwVersion != UBX_HW_VERSION_UNKNOWN), GPS_CFG_CMD_TIMEOUT_MS);
- } while(gpsState.autoConfigStep < GPS_VERSION_RETRY_TIMES && gpsState.hwVersion == UBX_HW_VERSION_UNKNOWN);
+ gpsState.autoConfigStep = 0;
+ ubx_capabilities.supported = ubx_capabilities.enabledGnss = ubx_capabilities.defaultGnss = 0;
+ do {
+ pollGnssCapabilities();
+ gpsState.autoConfigStep++;
+ ptWaitTimeout((ubx_capabilities.capMaxGnss != 0), GPS_CFG_CMD_TIMEOUT_MS);
+ } while (gpsState.autoConfigStep < GPS_VERSION_RETRY_TIMES && ubx_capabilities.capMaxGnss == 0);
+ // Configure GPS module if enabled
+ if (gpsState.gpsConfig->autoConfig) {
// Configure GPS
ptSpawn(gpsConfigure);
}
@@ -1070,6 +1050,21 @@ STATIC_PROTOTHREAD(gpsProtocolStateThread)
while (1) {
ptSemaphoreWait(semNewDataReady);
gpsProcessNewSolutionData();
+
+ if ((gpsState.gpsConfig->provider == GPS_UBLOX || gpsState.gpsConfig->provider == GPS_UBLOX7PLUS)) {
+ if ((millis() - gpsState.lastCapaPoolMs) > GPS_CAPA_INTERVAL) {
+ gpsState.lastCapaPoolMs = millis();
+
+ if (gpsState.hwVersion == UBX_HW_VERSION_UNKNOWN)
+ {
+ pollVersion();
+ ptWaitTimeout((_ack_state == UBX_ACK_GOT_ACK || _ack_state == UBX_ACK_GOT_NAK), GPS_CFG_CMD_TIMEOUT_MS);
+ }
+
+ pollGnssCapabilities();
+ ptWaitTimeout((_ack_state == UBX_ACK_GOT_ACK || _ack_state == UBX_ACK_GOT_NAK), GPS_CFG_CMD_TIMEOUT_MS);
+ }
+ }
}
ptEnd(0);
diff --git a/src/main/io/gps_ublox.h b/src/main/io/gps_ublox.h
new file mode 100644
index 00000000000..00b42eeb2b7
--- /dev/null
+++ b/src/main/io/gps_ublox.h
@@ -0,0 +1,429 @@
+/*
+ * This file is part of INAV
+ *
+ * Cleanflight is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * Cleanflight is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with Cleanflight. If not, see .
+ */
+
+#pragma once
+
+#include
+#include
+
+#include "common/time.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#define GPS_CFG_CMD_TIMEOUT_MS 500
+#define GPS_VERSION_RETRY_TIMES 3
+#define MAX_UBLOX_PAYLOAD_SIZE 256
+#define UBLOX_BUFFER_SIZE MAX_UBLOX_PAYLOAD_SIZE
+#define UBLOX_SBAS_MESSAGE_LENGTH 16
+#define GPS_CAPA_INTERVAL 5000
+
+#define UBX_DYNMODEL_PEDESTRIAN 3
+#define UBX_DYNMODEL_AIR_1G 6
+#define UBX_DYNMODEL_AIR_4G 8
+
+#define UBX_FIXMODE_2D_ONLY 1
+#define UBX_FIXMODE_3D_ONLY 2
+#define UBX_FIXMODE_AUTO 3
+
+#define UBX_VALID_GPS_DATE(valid) (valid & 1 << 0)
+#define UBX_VALID_GPS_TIME(valid) (valid & 1 << 1)
+#define UBX_VALID_GPS_DATE_TIME(valid) (UBX_VALID_GPS_DATE(valid) && UBX_VALID_GPS_TIME(valid))
+
+#define UBX_HW_VERSION_UNKNOWN 0
+#define UBX_HW_VERSION_UBLOX5 500
+#define UBX_HW_VERSION_UBLOX6 600
+#define UBX_HW_VERSION_UBLOX7 700
+#define UBX_HW_VERSION_UBLOX8 800
+#define UBX_HW_VERSION_UBLOX9 900
+#define UBX_HW_VERSION_UBLOX10 1000
+
+
+#define UBLOX_CFG_SIGNAL_SBAS_ENA 0x10310020 // U1
+#define UBLOX_CFG_SIGNAL_SBAS_L1CA_ENA 0x10310005 // U1
+
+#define UBLOX_CFG_SIGNAL_GAL_ENA 0x10310021 // U1
+#define UBLOX_CFG_SIGNAL_GAL_E1_ENA 0x10310007 // U1
+
+#define UBLOX_CFG_SIGNAL_BDS_ENA 0x10310022 // U1
+#define UBLOX_CFG_SIGNAL_BDS_B1_ENA 0x1031000d // U1
+#define UBLOX_CFG_SIGNAL_BDS_B1C_ENA 0x1031000f // U1 default off
+
+#define UBLOX_CFG_QZSS_ENA 0x10310024 // U1
+#define UBLOX_CFG_QZSS_L1CA_ENA 0x10310012 // U1
+#define UBLOX_CFG_QZSS_L1S_ENA 0x10310014 // U1
+
+#define UBLOX_CFG_GLO_ENA 0x10310025 // U1 default off - may conflict with other constelations
+#define UBLOX_CFG_GLO_L1_ENA 0x10310018 // U1 default off
+
+#define UBLOX_CFG_SBAS_PRNSCANMASK 0x50360006 // 0 = auto // X8
+#define UBLOX_SBAS_ALL 0x0000000000000000 //Enable search for all SBAS PRNs
+#define UBLOX_SBAS_PRN120 0x0000000000000001 //Enable search for SBAS PRN120
+#define UBLOX_SBAS_PRN121 0x0000000000000002 //Enable search for SBAS PRN121
+#define UBLOX_SBAS_PRN122 0x0000000000000004 //Enable search for SBAS PRN122
+#define UBLOX_SBAS_PRN123 0x0000000000000008 //Enable search for SBAS PRN123
+#define UBLOX_SBAS_PRN124 0x0000000000000010 //Enable search for SBAS PRN124
+#define UBLOX_SBAS_PRN125 0x0000000000000020 //Enable search for SBAS PRN125
+#define UBLOX_SBAS_PRN126 0x0000000000000040 //Enable search for SBAS PRN126
+#define UBLOX_SBAS_PRN127 0x0000000000000080 //Enable search for SBAS PRN127
+#define UBLOX_SBAS_PRN128 0x0000000000000100 //Enable search for SBAS PRN128
+#define UBLOX_SBAS_PRN129 0x0000000000000200 //Enable search for SBAS PRN129
+#define UBLOX_SBAS_PRN130 0x0000000000000400 //Enable search for SBAS PRN130
+#define UBLOX_SBAS_PRN131 0x0000000000000800 //Enable search for SBAS PRN131
+#define UBLOX_SBAS_PRN132 0x0000000000001000 //Enable search for SBAS PRN132
+#define UBLOX_SBAS_PRN133 0x0000000000002000 //Enable search for SBAS PRN133
+#define UBLOX_SBAS_PRN134 0x0000000000004000 //Enable search for SBAS PRN134
+#define UBLOX_SBAS_PRN135 0x0000000000008000 //Enable search for SBAS PRN135
+#define UBLOX_SBAS_PRN136 0x0000000000010000 //Enable search for SBAS PRN136
+#define UBLOX_SBAS_PRN137 0x0000000000020000 //Enable search for SBAS PRN137
+#define UBLOX_SBAS_PRN138 0x0000000000040000 //Enable search for SBAS PRN138
+#define UBLOX_SBAS_PRN139 0x0000000000080000 //Enable search for SBAS PRN139
+#define UBLOX_SBAS_PRN140 0x0000000000100000 //Enable search for SBAS PRN140
+#define UBLOX_SBAS_PRN141 0x0000000000200000 //Enable search for SBAS PRN141
+#define UBLOX_SBAS_PRN142 0x0000000000400000 //Enable search for SBAS PRN142
+#define UBLOX_SBAS_PRN143 0x0000000000800000 //Enable search for SBAS PRN143
+#define UBLOX_SBAS_PRN144 0x0000000001000000 //Enable search for SBAS PRN144
+#define UBLOX_SBAS_PRN145 0x0000000002000000 //Enable search for SBAS PRN145
+#define UBLOX_SBAS_PRN146 0x0000000004000000 //Enable search for SBAS PRN146
+#define UBLOX_SBAS_PRN147 0x0000000008000000 //Enable search for SBAS PRN147
+#define UBLOX_SBAS_PRN148 0x0000000010000000 //Enable search for SBAS PRN148
+#define UBLOX_SBAS_PRN149 0x0000000020000000 //Enable search for SBAS PRN149
+#define UBLOX_SBAS_PRN150 0x0000000040000000 //Enable search for SBAS PRN150
+#define UBLOX_SBAS_PRN151 0x0000000080000000 //Enable search for SBAS PRN151
+#define UBLOX_SBAS_PRN152 0x0000000100000000 //Enable search for SBAS PRN152
+#define UBLOX_SBAS_PRN153 0x0000000200000000 //Enable search for SBAS PRN153
+#define UBLOX_SBAS_PRN154 0x0000000400000000 //Enable search for SBAS PRN154
+#define UBLOX_SBAS_PRN155 0x0000000800000000 //Enable search for SBAS PRN155
+#define UBLOX_SBAS_PRN156 0x0000001000000000 //Enable search for SBAS PRN156
+#define UBLOX_SBAS_PRN157 0x0000002000000000 //Enable search for SBAS PRN157
+#define UBLOX_SBAS_PRN158 0x0000004000000000 //Enable search for SBAS PRN158
+
+// payload types
+typedef struct {
+ uint8_t mode;
+ uint8_t usage;
+ uint8_t maxSBAS;
+ uint8_t scanmode2;
+ uint32_t scanmode1;
+} ubx_sbas;
+
+typedef struct {
+ uint8_t msg_class;
+ uint8_t id;
+ uint8_t rate;
+} ubx_msg;
+
+typedef struct {
+ uint16_t meas;
+ uint16_t nav;
+ 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;
+
+typedef struct {
+ uint8_t version;
+ uint8_t layers;
+ uint8_t reserved;
+} __attribute__((packed)) ubx_config_data_header_v0_t;
+
+typedef struct {
+ uint8_t version;
+ uint8_t layers;
+ uint8_t transaction;
+ uint8_t reserved;
+} __attribute__((packed)) ubx_config_data_header_v1_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[MAX_GNSS_SIZE_BYTES]; // placeholder
+ ubx_sbas sbas;
+ ubx_msg msg;
+ ubx_rate rate;
+ ubx_gnss_msg_t gnss;
+} ubx_payload;
+
+// 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 {
+ uint32_t key;
+ uint8_t value;
+} __attribute__((packed)) ubx_config_data8_payload_t;
+
+
+#define MAX_CONFIG_SET_VAL_VALUES 32
+
+typedef struct {
+ ubx_header header;
+ ubx_config_data_header_v1_t configHeader;
+ union {
+ ubx_config_data8_payload_t payload[0];
+ uint8_t buffer[(MAX_CONFIG_SET_VAL_VALUES * sizeof(ubx_config_data8_payload_t)) + 2]; // 12 key/value pairs + 2 checksum bytes
+ } data;
+} __attribute__((packed)) ubx_config_data8_t;
+
+typedef struct {
+ ubx_header header;
+ ubx_payload payload;
+} __attribute__((packed)) ubx_message;
+
+typedef struct {
+ char swVersion[30]; // Zero-terminated Software Version String
+ char hwVersion[10]; // Zero-terminated Hardware Version String
+} ubx_mon_ver;
+
+typedef struct {
+ uint32_t time; // GPS msToW
+ int32_t longitude;
+ int32_t latitude;
+ int32_t altitude_ellipsoid;
+ int32_t altitude_msl;
+ uint32_t horizontal_accuracy;
+ uint32_t vertical_accuracy;
+} ubx_nav_posllh;
+
+typedef struct {
+ uint32_t time; // GPS msToW
+ uint8_t fix_type;
+ uint8_t fix_status;
+ uint8_t differential_status;
+ uint8_t res;
+ uint32_t time_to_first_fix;
+ uint32_t uptime; // milliseconds
+} ubx_nav_status;
+
+typedef struct {
+ uint32_t time;
+ int32_t time_nsec;
+ int16_t week;
+ uint8_t fix_type;
+ uint8_t fix_status;
+ int32_t ecef_x;
+ int32_t ecef_y;
+ int32_t ecef_z;
+ uint32_t position_accuracy_3d;
+ int32_t ecef_x_velocity;
+ int32_t ecef_y_velocity;
+ int32_t ecef_z_velocity;
+ uint32_t speed_accuracy;
+ uint16_t position_DOP;
+ uint8_t res;
+ uint8_t satellites;
+ uint32_t res2;
+} ubx_nav_solution;
+
+typedef struct {
+ uint32_t time; // GPS msToW
+ int32_t ned_north;
+ int32_t ned_east;
+ int32_t ned_down;
+ uint32_t speed_3d;
+ uint32_t speed_2d;
+ int32_t heading_2d;
+ uint32_t speed_accuracy;
+ uint32_t heading_accuracy;
+} ubx_nav_velned;
+
+typedef struct {
+ uint8_t chn; // Channel number, 255 for SVx not assigned to channel
+ uint8_t svid; // Satellite ID
+ uint8_t flags; // Bitmask
+ uint8_t quality; // Bitfield
+ uint8_t cno; // Carrier to Noise Ratio (Signal Strength) // dbHz, 0-55.
+ uint8_t elev; // Elevation in integer degrees
+ int16_t azim; // Azimuth in integer degrees
+ int32_t prRes; // Pseudo range residual in centimetres
+} ubx_nav_svinfo_channel;
+
+typedef struct {
+ uint32_t time; // GPS Millisecond time of week
+ uint8_t numCh; // Number of channels
+ uint8_t globalFlags; // Bitmask, Chip hardware generation 0:Antaris, 1:u-blox 5, 2:u-blox 6
+ uint16_t reserved2; // Reserved
+ ubx_nav_svinfo_channel channel[16]; // 16 satellites * 12 byte
+} ubx_nav_svinfo;
+
+typedef struct {
+ uint32_t time; // GPS msToW
+ uint32_t tAcc;
+ int32_t nano;
+ uint16_t year;
+ uint8_t month;
+ uint8_t day;
+ uint8_t hour;
+ uint8_t min;
+ uint8_t sec;
+ uint8_t valid;
+} ubx_nav_timeutc;
+
+typedef struct {
+ uint32_t time; // GPS msToW
+ uint16_t year;
+ uint8_t month;
+ uint8_t day;
+ uint8_t hour;
+ uint8_t min;
+ uint8_t sec;
+ uint8_t valid;
+ uint32_t tAcc;
+ int32_t nano;
+ uint8_t fix_type;
+ uint8_t fix_status;
+ uint8_t reserved1;
+ uint8_t satellites;
+ int32_t longitude;
+ int32_t latitude;
+ int32_t altitude_ellipsoid;
+ int32_t altitude_msl;
+ uint32_t horizontal_accuracy;
+ uint32_t vertical_accuracy;
+ int32_t ned_north;
+ int32_t ned_east;
+ int32_t ned_down;
+ int32_t speed_2d;
+ int32_t heading_2d;
+ uint32_t speed_accuracy;
+ uint32_t heading_accuracy;
+ uint16_t position_DOP;
+ uint16_t reserved2;
+ uint16_t reserved3;
+} ubx_nav_pvt;
+
+#define UBX_MON_GNSS_GPS_MASK (1 << 0)
+#define UBX_MON_GNSS_GLONASS_MASK (1 << 1)
+#define UBX_MON_GNSS_BEIDOU_MASK (1 << 2)
+#define UBX_MON_GNSS_GALILEO_MASK (1 << 3)
+
+typedef struct {
+ uint8_t version;
+ uint8_t supported; // bitfield for GNSS types: 0:GPS, 1:Glonass, 2:Beidou, 3:Galileo
+ uint8_t defaultGnss; // bitfield for GNSS types: 0:GPS, 1:Glonass, 2:Beidou, 3:Galileo
+ uint8_t enabled; // bitfield for GNSS types: 0:GPS, 1:Glonass, 2:Beidou, 3:Galileo
+ uint8_t maxConcurrent;
+ uint8_t reserverd1;
+ uint8_t reserverd2;
+ uint8_t reserverd3;
+} ubx_mon_gnss;
+
+typedef struct {
+ uint8_t msg_class;
+ uint8_t msg;
+} ubx_ack_ack;
+
+typedef enum {
+ UBX_ACK_WAITING = 0,
+ UBX_ACK_GOT_ACK = 1,
+ UBX_ACK_GOT_NAK = 2
+} ubx_ack_state_t;
+
+typedef enum {
+ PREAMBLE1 = 0xB5,
+ PREAMBLE2 = 0x62,
+ CLASS_NAV = 0x01,
+ CLASS_ACK = 0x05,
+ CLASS_CFG = 0x06,
+ CLASS_MON = 0x0A,
+ MSG_CLASS_UBX = 0x01,
+ MSG_CLASS_NMEA = 0xF0,
+ MSG_VER = 0x04,
+ MSG_ACK_NACK = 0x00,
+ MSG_ACK_ACK = 0x01,
+ MSG_NMEA_GGA = 0x0,
+ MSG_NMEA_GLL = 0x1,
+ MSG_NMEA_GSA = 0x2,
+ MSG_NMEA_GSV = 0x3,
+ MSG_NMEA_RMC = 0x4,
+ MSG_NMEA_VGS = 0x5,
+ MSG_POSLLH = 0x2,
+ MSG_STATUS = 0x3,
+ MSG_SOL = 0x6,
+ MSG_PVT = 0x7,
+ MSG_VELNED = 0x12,
+ MSG_TIMEUTC = 0x21,
+ MSG_SVINFO = 0x30,
+ MSG_NAV_SAT = 0x35,
+ MSG_NAV_SIG = 0x35,
+ MSG_CFG_PRT = 0x00,
+ MSG_CFG_RATE = 0x08,
+ MSG_CFG_SET_RATE = 0x01,
+ MSG_CFG_NAV_SETTINGS = 0x24,
+ MSG_CFG_SBAS = 0x16,
+ MSG_CFG_GNSS = 0x3e,
+ MSG_MON_GNSS = 0x28
+} ubx_protocol_bytes_t;
+
+typedef enum {
+ FIX_NONE = 0,
+ FIX_DEAD_RECKONING = 1,
+ FIX_2D = 2,
+ FIX_3D = 3,
+ FIX_GPS_DEAD_RECKONING = 4,
+ FIX_TIME = 5
+} ubs_nav_fix_type_t;
+
+typedef enum {
+ NAV_STATUS_FIX_VALID = 1
+} ubx_nav_status_bits_t;
+
+uint8_t gpsUbloxMaxGnss(void);
+timeMs_t gpsUbloxCapLastUpdate(void);
+
+bool gpsUbloxHasGalileo(void);
+bool gpsUbloxHasBeidou(void);
+bool gpsUbloxHasGlonass(void);
+
+bool gpsUbloxGalileoDefault(void);
+bool gpsUbloxBeidouDefault(void);
+bool gpsUbloxGlonassDefault(void);
+
+bool gpsUbloxGalileoEnabled(void);
+bool gpsUbloxBeidouEnabled(void);
+bool gpsUbloxGlonassEnabled(void);
+
+
+#ifdef __cplusplus
+}
+#endif
diff --git a/src/main/io/gps_ublox_utils.c b/src/main/io/gps_ublox_utils.c
new file mode 100644
index 00000000000..97c5bf5cd9a
--- /dev/null
+++ b/src/main/io/gps_ublox_utils.c
@@ -0,0 +1,62 @@
+/*
+ * This file is part of INAV
+ *
+ * Cleanflight is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * Cleanflight is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with Cleanflight. If not, see .
+ */
+
+
+#include
+
+#include "gps_ublox_utils.h"
+
+void ublox_update_checksum(uint8_t *data, uint8_t len, uint8_t *ck_a, uint8_t *ck_b)
+{
+ *ck_a = *ck_b = 0;
+ while (len--) {
+ *ck_a += *data;
+ *ck_b += *ck_a;
+ data++;
+ }
+}
+
+int ubloxCfgFillBytes(ubx_config_data8_t *cfg, ubx_config_data8_payload_t *kvPairs, uint8_t count)
+{
+ if (count > MAX_CONFIG_SET_VAL_VALUES)
+ count = MAX_CONFIG_SET_VAL_VALUES;
+
+ cfg->header.preamble1 = 0xb5;
+ cfg->header.preamble2 = 0x62;
+ cfg->header.msg_class = 0x06;
+ cfg->header.msg_id = 0x8A;
+ cfg->header.length = sizeof(ubx_config_data_header_v1_t) + ((sizeof(ubx_config_data8_payload_t) * count));
+ cfg->configHeader.layers = 0x1;
+ cfg->configHeader.transaction = 0;
+ cfg->configHeader.reserved = 0;
+ cfg->configHeader.version = 1;
+
+
+ for (int i = 0; i < count; ++i) {
+ cfg->data.payload[i].key = kvPairs[i].key; //htonl(kvPairs[i].key);
+ cfg->data.payload[i].value = kvPairs[i].value;
+ }
+
+ uint8_t *buf = (uint8_t *)cfg;
+ uint8_t ck_a, ck_b;
+ ublox_update_checksum(buf + 2, cfg->header.length + 4, &ck_a, &ck_b);
+ buf[cfg->header.length + 6] = ck_a;
+ buf[cfg->header.length + 7] = ck_b;
+
+ return count;
+}
+
diff --git a/src/main/io/gps_ublox_utils.h b/src/main/io/gps_ublox_utils.h
new file mode 100644
index 00000000000..e7ae0caac90
--- /dev/null
+++ b/src/main/io/gps_ublox_utils.h
@@ -0,0 +1,34 @@
+/*
+ * This file is part of INAV
+ *
+ * Cleanflight is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * Cleanflight is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with Cleanflight. If not, see .
+ */
+
+#pragma once
+
+#include
+
+#include "gps_ublox.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+int ubloxCfgFillBytes(ubx_config_data8_t *cfg, ubx_config_data8_payload_t *kvPairs, uint8_t count);
+
+void ublox_update_checksum(uint8_t *data, uint8_t len, uint8_t *ck_a, uint8_t *ck_b);
+
+#ifdef __cplusplus
+}
+#endif
diff --git a/src/test/unit/CMakeLists.txt b/src/test/unit/CMakeLists.txt
index abb9389fbb3..ebfd3b78d4f 100644
--- a/src/test/unit/CMakeLists.txt
+++ b/src/test/unit/CMakeLists.txt
@@ -37,6 +37,9 @@ set_property(SOURCE circular_queue_unittest.cc PROPERTY depends "common/circular
set_property(SOURCE osd_unittest.cc PROPERTY depends "io/osd_utils.c" "io/displayport_msp_osd.c" "common/typeconversion.c")
set_property(SOURCE osd_unittest.cc PROPERTY definitions OSD_UNIT_TEST USE_MSP_DISPLAYPORT DISABLE_MSP_BF_COMPAT)
+set_property(SOURCE gps_ublox_unittest.cc PROPERTY depends "io/gps_ublox_utils.c")
+set_property(SOURCE gps_ublox_unittest.cc PROPERTY definitions GPS_UBLOX_UNIT_TEST)
+
function(unit_test src)
get_filename_component(basename ${src} NAME)
string(REPLACE ".cc" "" name ${basename} )
diff --git a/src/test/unit/gps_ublox_unittest.cc b/src/test/unit/gps_ublox_unittest.cc
new file mode 100644
index 00000000000..bc20b340251
--- /dev/null
+++ b/src/test/unit/gps_ublox_unittest.cc
@@ -0,0 +1,90 @@
+/*
+ * This file is part of INAV.
+ *
+ * Cleanflight is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * Cleanflight is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with Cleanflight. If not, see .
+ */
+
+
+#include "gtest/gtest.h"
+#include "unittest_macros.h"
+
+#include
+#include
+#include
+
+#include "io/gps_ublox_utils.h"
+
+void dumpCfg(const ubx_config_data8_t *cfg, int valuesAdded)
+{
+ printf("%02x %02x %02x %02x %04x\n", cfg->header.preamble1, cfg->header.preamble2, cfg->header.msg_class, cfg->header.msg_id, cfg->header.length);
+
+ printf("%02x %02x %02x %02x\n", cfg->configHeader.version, cfg->configHeader.layers, cfg->configHeader.transaction, cfg->configHeader.reserved);
+
+ for(int i =0; i < valuesAdded; ++i) {
+ printf("%i: %08x %02x\n", i+1, cfg->data.payload[i].key, cfg->data.payload[i].value);
+ }
+
+ uint8_t *chksums = (uint8_t *)&cfg->data.payload[valuesAdded];
+
+ printf("%02x %02x\n", chksums[0], chksums[1]);
+}
+
+void dumpMemory(uint8_t *mem, int size)
+{
+ for(int i =0; i < size; ++i) {
+ printf("%02x ", mem[i]);
+ }
+ printf("\n");
+}
+
+TEST(GPSUbloxTest, TestUbloxCfgFillBytes)
+{
+ ubx_config_data8_t cfg = {};
+ ubx_config_data8_payload_t kvPairs[] = {
+ { 0x10310025, 0x1},
+ { 0x42, 0x69},
+ { 0x04, 0x20},
+ { 0x42, 0x69},
+ { 0x04, 0x20},
+ { 0x42, 0x69},
+ { 0x04, 0x20},
+ { 0x42, 0x69},
+ { 0x04, 0x20},
+ { 0x42, 0x69},
+ { 0x04, 0x20},
+ { 0x42, 0x69}
+ };
+
+ int valuesAdded = ubloxCfgFillBytes(&cfg, kvPairs, 12);
+
+ EXPECT_TRUE(valuesAdded == 12);
+
+ dumpCfg(&cfg, valuesAdded);
+
+ valuesAdded = ubloxCfgFillBytes(&cfg, kvPairs, 1);
+ EXPECT_TRUE(1 == valuesAdded);
+
+ // Set glonass enabled, from u-center 2
+ const uint8_t expected[] = {0xB5, 0x62, 0x06, 0x8A, 0x09, 0x00, 0x01, 0x01, 0x00, 0x00, 0x25, 0x00, 0x31, 0x10, 0x01, 0x02, 0xA7};
+ EXPECT_FALSE(memcmp((void *)expected, (void *)&cfg, 17));
+
+ printf("Expected:\n");
+ dumpMemory((uint8_t *)expected, 17);
+ printf("Actual:\n");
+ dumpMemory((uint8_t *)&cfg, 17);
+
+ // osdFormatCentiNumber(buf, 12345, 1, 2, 3, 7);
+ // std::cout << "'" << buf << "'" << std::endl;
+ //EXPECT_FALSE(strcmp(buf, " 123.45"));
+}
\ No newline at end of file