Mirror/inav
1
0
Fork 0
mirror of https://github.com/iNavFlight/inav.git synced 2025-07-23 08:15:26 +03:00
Code Issues Wiki Activity

GPS: UBLOX version auto-detection and PVT message support from @Sambas. Some code refactoring. NAZA GPS support.

Browse source
This commit is contained in:
Konstantin Sharlaimov (DigitalEntity) 2015-12-30 21:55:13 +10:00
parent e07248787a
commit 28672eb0c6
11 changed files with 276 additions and 205 deletions
Download patch file Download diff file Expand all files Collapse all files

36
src/main/drivers/gps.h
View file

@ -1,36 +0,0 @@
/*
* This file is part of Cleanflight.
*
* 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 <http://www.gnu.org/licenses/>.
*/
#pragma once
typedef struct gpsDataGeneric_s {
struct {
unsigned gpsOk : 1; // gps read successful
unsigned newData : 1; // new gps data available (lat/lon/alt)
unsigned fix3D : 1; // gps fix status
} flags;
int32_t latitude;
int32_t longitude;
uint8_t numSat;
uint16_t altitude;
uint16_t speed;
uint16_t ground_course;
uint16_t hdop;
} gpsDataGeneric_t;
typedef void (*gpsReadFuncPtr)(gpsDataGeneric_t * gpsMsg); // baro start operation

3
src/main/drivers/gps_i2cnav.c
View file

@ -22,7 +22,6 @@
#include "build_config.h" #include "build_config.h"
#include "gps.h"
#include "gps_i2cnav.h" #include "gps_i2cnav.h"
#include "gpio.h" #include "gpio.h"
@ -56,7 +55,7 @@ bool i2cnavGPSModuleDetect(void)
return false; return false;
} }
void i2cnavGPSModuleRead(gpsDataGeneric_t * gpsMsg) void i2cnavGPSModuleRead(gpsDataI2CNAV_t * gpsMsg)
{ {
bool ack; bool ack;
uint8_t i2cGpsStatus; uint8_t i2cGpsStatus;

17
src/main/drivers/gps_i2cnav.h
View file

@ -17,7 +17,20 @@
#pragma once #pragma once
#include "gps.h" typedef struct {
struct {
unsigned gpsOk : 1; // gps read successful
unsigned newData : 1; // new gps data available (lat/lon/alt)
unsigned fix3D : 1; // gps fix status
} flags;
int32_t latitude;
int32_t longitude;
uint8_t numSat;
uint16_t altitude;
uint16_t speed;
uint16_t ground_course;
uint16_t hdop;
} gpsDataI2CNAV_t;
bool i2cnavGPSModuleDetect(void); bool i2cnavGPSModuleDetect(void);
void i2cnavGPSModuleRead(gpsDataGeneric_t * gpsMsg); void i2cnavGPSModuleRead(gpsDataI2CNAV_t * gpsMsg);

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

@ -18,7 +18,6 @@
#include <stdbool.h> #include <stdbool.h>
#include <stdint.h> #include <stdint.h>
#include <ctype.h> #include <ctype.h>
#include <string.h>
#include <math.h> #include <math.h>
#include "platform.h" #include "platform.h"
@ -37,9 +36,6 @@
#include "drivers/sensor.h" #include "drivers/sensor.h"
#include "drivers/compass.h" #include "drivers/compass.h"
#include "drivers/gps.h"
#include "drivers/gps_i2cnav.h"
#include "sensors/sensors.h" #include "sensors/sensors.h"
#include "sensors/compass.h" #include "sensors/compass.h"
@ -48,9 +44,6 @@
#include "io/gps.h" #include "io/gps.h"
#include "io/gps_private.h" #include "io/gps_private.h"
#include "flight/gps_conversion.h"
#include "flight/pid.h"
#include "flight/hil.h"
#include "flight/navigation_rewrite.h" #include "flight/navigation_rewrite.h"
#include "config/config.h" #include "config/config.h"
@ -258,13 +251,15 @@ void gpsThread(void)
if ((millis() - gpsState.lastStateSwitchMs) >= GPS_INIT_DELAY) { if ((millis() - gpsState.lastStateSwitchMs) >= GPS_INIT_DELAY) {
// Switch to required serial port baud // Switch to required serial port baud
serialSetBaudRate(gpsState.gpsPort, baudRates[gpsToSerialBaudRate[gpsState.baudrateIndex]]); serialSetBaudRate(gpsState.gpsPort, baudRates[gpsToSerialBaudRate[gpsState.baudrateIndex]]);
gpsState.hwVersion = 0;
gpsState.autoConfigStep = 0; gpsState.autoConfigStep = 0;
gpsState.autoConfigPosition = 0; gpsState.autoConfigPosition = 0;
gpsState.lastMessageMs = millis(); gpsState.lastMessageMs = millis();
gpsSetState(GPS_CONFIGURE); gpsSetState(GPS_CHECK_VERSION);
} }
break; break;
case GPS_CHECK_VERSION:
case GPS_CONFIGURE: case GPS_CONFIGURE:
case GPS_RECEIVING_DATA: case GPS_RECEIVING_DATA:
gpsHandleProtocol(); gpsHandleProtocol();
@ -298,11 +293,12 @@ void gpsThread(void)
gpsResetSolution(); gpsResetSolution();
if (gpsProviders[gpsState.gpsConfig->provider].detect && gpsProviders[gpsState.gpsConfig->provider].detect()) { if (gpsProviders[gpsState.gpsConfig->provider].detect && gpsProviders[gpsState.gpsConfig->provider].detect()) {
gpsState.hwVersion = 0;
gpsState.autoConfigStep = 0; gpsState.autoConfigStep = 0;
gpsState.autoConfigPosition = 0; gpsState.autoConfigPosition = 0;
gpsState.lastMessageMs = millis(); gpsState.lastMessageMs = millis();
sensorsSet(SENSOR_GPS); sensorsSet(SENSOR_GPS);
gpsSetState(GPS_CONFIGURE); gpsSetState(GPS_CHECK_VERSION);
} }
else { else {
sensorsClear(SENSOR_GPS); sensorsClear(SENSOR_GPS);
@ -310,6 +306,7 @@ void gpsThread(void)
} }
break; break;
case GPS_CHECK_VERSION:
case GPS_CONFIGURE: case GPS_CONFIGURE:
case GPS_RECEIVING_DATA: case GPS_RECEIVING_DATA:
gpsHandleProtocol(); gpsHandleProtocol();

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

@ -31,6 +31,7 @@ typedef enum {
GPS_NMEA = 0, GPS_NMEA = 0,
GPS_UBLOX, GPS_UBLOX,
GPS_I2CNAV, GPS_I2CNAV,
GPS_NAZA,
GPS_PROVIDER_COUNT GPS_PROVIDER_COUNT
} gpsProvider_e; } gpsProvider_e;

14
src/main/io/gps_i2cnav.c
View file

@ -18,8 +18,6 @@
#include <stdbool.h> #include <stdbool.h>
#include <stdint.h> #include <stdint.h>
#include <ctype.h> #include <ctype.h>
#include <string.h>
#include <math.h>
#include "platform.h" #include "platform.h"
#include "build_config.h" #include "build_config.h"
@ -31,18 +29,9 @@
#include "drivers/system.h" #include "drivers/system.h"
#include "drivers/serial.h" #include "drivers/serial.h"
#include "drivers/serial_uart.h"
#include "drivers/gpio.h"
#include "drivers/light_led.h"
#include "drivers/sensor.h"
#include "drivers/gps.h"
#include "drivers/gps_i2cnav.h" #include "drivers/gps_i2cnav.h"
#include "sensors/sensors.h"
#include "io/serial.h" #include "io/serial.h"
#include "io/display.h"
#include "io/gps.h" #include "io/gps.h"
#include "io/gps_private.h" #include "io/gps_private.h"
@ -65,7 +54,7 @@ bool gpsDetectI2CNAV(void)
bool gpsPollI2CNAV(void) bool gpsPollI2CNAV(void)
{ {
gpsDataGeneric_t gpsMsg; gpsDataI2CNAV_t gpsMsg;
// Check for poll rate timeout // Check for poll rate timeout
if ((millis() - gpsState.lastMessageMs) < (1000 / GPS_I2C_POLL_RATE_HZ)) if ((millis() - gpsState.lastMessageMs) < (1000 / GPS_I2C_POLL_RATE_HZ))
@ -113,6 +102,7 @@ bool gpsHandleI2CNAV(void)
default: default:
return false; return false;
case GPS_CHECK_VERSION:
case GPS_CONFIGURE: case GPS_CONFIGURE:
gpsSetState(GPS_RECEIVING_DATA); gpsSetState(GPS_RECEIVING_DATA);
return false; return false;

33
src/main/io/gps_naza.c
View file

@ -32,24 +32,12 @@
#include "drivers/system.h" #include "drivers/system.h"
#include "drivers/serial.h" #include "drivers/serial.h"
#include "drivers/serial_uart.h" #include "drivers/serial_uart.h"
#include "drivers/gpio.h"
#include "drivers/light_led.h"
#include "drivers/sensor.h"
#include "drivers/gps.h"
#include "drivers/gps_i2cnav.h"
#include "sensors/sensors.h"
#include "io/serial.h" #include "io/serial.h"
#include "io/display.h"
#include "io/gps.h" #include "io/gps.h"
#include "io/gps_private.h" #include "io/gps_private.h"
#include "flight/gps_conversion.h" #include "flight/gps_conversion.h"
#include "flight/pid.h"
#include "flight/hil.h"
#include "flight/navigation_rewrite.h"
#include "config/config.h" #include "config/config.h"
#include "config/runtime_config.h" #include "config/runtime_config.h"
@ -263,9 +251,13 @@ static bool gpsNewFrameNAZA(uint8_t data)
_ck_b += (_ck_a += data); // checksum byte _ck_b += (_ck_a += data); // checksum byte
_payload_length = data; // payload length low byte _payload_length = data; // payload length low byte
if (_payload_length > NAZA_MAX_PAYLOAD_SIZE) { if (_payload_length > NAZA_MAX_PAYLOAD_SIZE) {
_skip_packet = true; // we can't receive the whole packet, just log the error and start searching for the next packet.
gpsStats.errors++;
_step = 0;
break;
} }
_payload_counter = 0; // prepare to receive payload // prepare to receive payload
_payload_counter = 0;
if (_payload_length == 0) { if (_payload_length == 0) {
_step = 6; _step = 6;
} }
@ -288,7 +280,6 @@ static bool gpsNewFrameNAZA(uint8_t data)
break; break;
case 6: case 6:
_step = 0; _step = 0;
if (_ck_b != data) { if (_ck_b != data) {
gpsStats.errors++; gpsStats.errors++;
break; // bad checksum break; // bad checksum
@ -314,13 +305,6 @@ static bool gpsInitialize(void)
return false; return false;
} }
static bool gpsConfigure(void)
{
// No autoconfig, switch straight to receiving data
gpsSetState(GPS_RECEIVING_DATA);
return false;
}
static bool gpsReceiveData(void) static bool gpsReceiveData(void)
{ {
bool hasNewData = false; bool hasNewData = false;
@ -353,8 +337,11 @@ bool gpsHandleNAZA(void)
case GPS_INITIALIZING: case GPS_INITIALIZING:
return gpsInitialize(); return gpsInitialize();
case GPS_CHECK_VERSION:
case GPS_CONFIGURE: case GPS_CONFIGURE:
return gpsConfigure(); // No autoconfig, switch straight to receiving data
gpsSetState(GPS_RECEIVING_DATA);
return false;
case GPS_RECEIVING_DATA: case GPS_RECEIVING_DATA:
return hasNewData; return hasNewData;

24
src/main/io/gps_nmea.c
View file

@ -32,24 +32,12 @@
#include "drivers/system.h" #include "drivers/system.h"
#include "drivers/serial.h" #include "drivers/serial.h"
#include "drivers/serial_uart.h" #include "drivers/serial_uart.h"
#include "drivers/gpio.h"
#include "drivers/light_led.h"
#include "drivers/sensor.h"
#include "drivers/gps.h"
#include "drivers/gps_i2cnav.h"
#include "sensors/sensors.h"
#include "io/serial.h" #include "io/serial.h"
#include "io/display.h"
#include "io/gps.h" #include "io/gps.h"
#include "io/gps_private.h" #include "io/gps_private.h"
#include "flight/gps_conversion.h" #include "flight/gps_conversion.h"
#include "flight/pid.h"
#include "flight/hil.h"
#include "flight/navigation_rewrite.h"
#include "config/config.h" #include "config/config.h"
#include "config/runtime_config.h" #include "config/runtime_config.h"
@ -281,13 +269,6 @@ static bool gpsInitialize(void)
return false; return false;
} }
static bool gpsConfigure(void)
{
// No autoconfig, switch straight to receiving data
gpsSetState(GPS_RECEIVING_DATA);
return false;
}
static bool gpsReceiveData(void) static bool gpsReceiveData(void)
{ {
bool hasNewData = false; bool hasNewData = false;
@ -320,8 +301,11 @@ bool gpsHandleNMEA(void)
case GPS_INITIALIZING: case GPS_INITIALIZING:
return gpsInitialize(); return gpsInitialize();
case GPS_CHECK_VERSION:
case GPS_CONFIGURE: case GPS_CONFIGURE:
return gpsConfigure(); // No autoconfig, switch straight to receiving data
gpsSetState(GPS_RECEIVING_DATA);
return false;
case GPS_RECEIVING_DATA: case GPS_RECEIVING_DATA:
return hasNewData; return hasNewData;

9
src/main/io/gps_private.h
View file

@ -23,9 +23,10 @@ typedef enum {
GPS_UNKNOWN, // 0 GPS_UNKNOWN, // 0
GPS_INITIALIZING, // 1 GPS_INITIALIZING, // 1
GPS_CHANGE_BAUD, // 2 GPS_CHANGE_BAUD, // 2
GPS_CONFIGURE, // 3 GPS_CHECK_VERSION, // 3
GPS_RECEIVING_DATA, // 4 GPS_CONFIGURE, // 4
GPS_LOST_COMMUNICATION, // 5 GPS_RECEIVING_DATA, // 5
GPS_LOST_COMMUNICATION, // 6
} gpsState_e; } gpsState_e;
typedef struct { typedef struct {
@ -33,6 +34,8 @@ typedef struct {
serialConfig_t * serialConfig; serialConfig_t * serialConfig;
serialPort_t * gpsPort; // Serial GPS only serialPort_t * gpsPort; // Serial GPS only
uint32_t hwVersion;
gpsState_e state; gpsState_e state;
gpsBaudRate_e baudrateIndex; gpsBaudRate_e baudrateIndex;
gpsBaudRate_e autoBaudrateIndex; // Driver internal use (for autoBaud) gpsBaudRate_e autoBaudrateIndex; // Driver internal use (for autoBaud)

327
src/main/io/gps_ublox.c
View file

@ -16,6 +16,7 @@
*/ */
#include <stdbool.h> #include <stdbool.h>
#include <stdlib.h>
#include <stdint.h> #include <stdint.h>
#include <ctype.h> #include <ctype.h>
#include <string.h> #include <string.h>
@ -32,30 +33,20 @@
#include "drivers/system.h" #include "drivers/system.h"
#include "drivers/serial.h" #include "drivers/serial.h"
#include "drivers/serial_uart.h" #include "drivers/serial_uart.h"
#include "drivers/gpio.h"
#include "drivers/light_led.h"
#include "drivers/sensor.h"
#include "drivers/gps.h"
#include "drivers/gps_i2cnav.h"
#include "sensors/sensors.h"
#include "io/serial.h" #include "io/serial.h"
#include "io/display.h"
#include "io/gps.h" #include "io/gps.h"
#include "io/gps_private.h" #include "io/gps_private.h"
#include "flight/gps_conversion.h" #include "flight/gps_conversion.h"
#include "flight/pid.h"
#include "flight/hil.h"
#include "flight/navigation_rewrite.h"
#include "config/config.h" #include "config/config.h"
#include "config/runtime_config.h" #include "config/runtime_config.h"
#if defined(GPS) && defined(GPS_PROTO_UBLOX) #if defined(GPS) && defined(GPS_PROTO_UBLOX)
#define GPS_PROTO_UBLOX_NEO7PLUS
static const char * baudInitData[GPS_BAUDRATE_COUNT] = { static const char * baudInitData[GPS_BAUDRATE_COUNT] = {
"$PUBX,41,1,0003,0001,115200,0*1E\r\n", // GPS_BAUDRATE_115200 "$PUBX,41,1,0003,0001,115200,0*1E\r\n", // GPS_BAUDRATE_115200
"$PUBX,41,1,0003,0001,57600,0*2D\r\n", // GPS_BAUDRATE_57600 "$PUBX,41,1,0003,0001,57600,0*2D\r\n", // GPS_BAUDRATE_57600
@ -64,6 +55,12 @@ static const char * baudInitData[GPS_BAUDRATE_COUNT] = {
"$PUBX,41,1,0003,0001,9600,0*16\r\n" // GPS_BAUDRATE_9600 "$PUBX,41,1,0003,0001,9600,0*16\r\n" // GPS_BAUDRATE_9600
}; };
#ifdef GPS_PROTO_UBLOX_NEO7PLUS
static const uint8_t ubloxVerPoll[] = {
0xB5, 0x62, 0x0A, 0x04, 0x00, 0x00, 0x0E, 0x34, // MON-VER - Poll version info
};
#endif
static const uint8_t ubloxInit6[] = { static const uint8_t ubloxInit6[] = {
0xB5, 0x62, 0x06, 0x24, 0x24, 0x00, 0xFF, 0xFF, 0x06, 0x02, 0x00, // CFG-NAV5 - Set engine settings 0xB5, 0x62, 0x06, 0x24, 0x24, 0x00, 0xFF, 0xFF, 0x06, 0x02, 0x00, // CFG-NAV5 - Set engine settings
@ -87,13 +84,39 @@ static const uint8_t ubloxInit6[] = {
0xB5, 0x62, 0x06, 0x01, 0x03, 0x00, 0x01, 0x02, 0x01, 0x0E, 0x47, // set POSLLH MSG rate 0xB5, 0x62, 0x06, 0x01, 0x03, 0x00, 0x01, 0x02, 0x01, 0x0E, 0x47, // set POSLLH MSG rate
0xB5, 0x62, 0x06, 0x01, 0x03, 0x00, 0x01, 0x03, 0x01, 0x0F, 0x49, // set STATUS MSG rate 0xB5, 0x62, 0x06, 0x01, 0x03, 0x00, 0x01, 0x03, 0x01, 0x0F, 0x49, // set STATUS MSG rate
0xB5, 0x62, 0x06, 0x01, 0x03, 0x00, 0x01, 0x06, 0x01, 0x12, 0x4F, // set SOL MSG rate 0xB5, 0x62, 0x06, 0x01, 0x03, 0x00, 0x01, 0x06, 0x01, 0x12, 0x4F, // set SOL MSG rate
//0xB5, 0x62, 0x06, 0x01, 0x03, 0x00, 0x01, 0x30, 0x01, 0x3C, 0xA3, // set SVINFO MSG rate (every cycle - high bandwidth)
0xB5, 0x62, 0x06, 0x01, 0x03, 0x00, 0x01, 0x30, 0x05, 0x40, 0xA7, // set SVINFO MSG rate (evey 5 cycles - low bandwidth) 0xB5, 0x62, 0x06, 0x01, 0x03, 0x00, 0x01, 0x30, 0x05, 0x40, 0xA7, // set SVINFO MSG rate (evey 5 cycles - low bandwidth)
0xB5, 0x62, 0x06, 0x01, 0x03, 0x00, 0x01, 0x12, 0x01, 0x1E, 0x67, // set VELNED MSG rate 0xB5, 0x62, 0x06, 0x01, 0x03, 0x00, 0x01, 0x12, 0x01, 0x1E, 0x67, // set VELNED MSG rate
0xB5, 0x62, 0x06, 0x08, 0x06, 0x00, 0xC8, 0x00, 0x01, 0x00, 0x01, 0x00, 0xDE, 0x6A, // set rate to 5Hz (measurement period: 200ms, navigation rate: 1 cycle) 0xB5, 0x62, 0x06, 0x08, 0x06, 0x00, 0xC8, 0x00, 0x01, 0x00, 0x01, 0x00, 0xDE, 0x6A, // set rate to 5Hz (measurement period: 200ms, navigation rate: 1 cycle)
}; };
#ifdef GPS_PROTO_UBLOX_NEO7PLUS
static const uint8_t ubloxInit[] = {
0xB5, 0x62, 0x06, 0x24, 0x24, 0x00, 0xFF, 0xFF, 0x06, 0x03, 0x00, // CFG-NAV5 - Set engine settings
0x00, 0x00, 0x00, 0x10, 0x27, 0x00, 0x00, 0x05, 0x00, 0xFA, 0x00, // Airborne <1G
0xFA, 0x00, 0x64, 0x00, 0x2C, 0x01, 0x00, 0x3C, 0x00, 0x00, 0x00,
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x52, 0xE8,
// DISABLE NMEA messages
0xB5, 0x62, 0x06, 0x01, 0x03, 0x00, 0xF0, 0x00, 0x00, 0xFA, 0x0F, // GGA: Global positioning system fix data
0xB5, 0x62, 0x06, 0x01, 0x03, 0x00, 0xF0, 0x01, 0x00, 0xFB, 0x11, // GLL: Latitude and longitude, with time of position fix and status
0xB5, 0x62, 0x06, 0x01, 0x03, 0x00, 0xF0, 0x02, 0x00, 0xFC, 0x13, // GSA: GNSS DOP and Active Satellites
0xB5, 0x62, 0x06, 0x01, 0x03, 0x00, 0xF0, 0x03, 0x00, 0xFD, 0x15, // GSV: GNSS Satellites in View
0xB5, 0x62, 0x06, 0x01, 0x03, 0x00, 0xF0, 0x04, 0x00, 0xFE, 0x17, // RMC: Recommended Minimum data
0xB5, 0x62, 0x06, 0x01, 0x03, 0x00, 0xF0, 0x05, 0x00, 0xFF, 0x19, // VGS: Course over ground and Ground speed
// Enable UBLOX messages
0xB5, 0x62, 0x06, 0x01, 0x08, 0x00, 0x01, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x12, 0xB9, // disable POSLLH
0xB5, 0x62, 0x06, 0x01, 0x08, 0x00, 0x01, 0x03, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x13, 0xC0, // disable STATUS
0xB5, 0x62, 0x06, 0x01, 0x08, 0x00, 0x01, 0x06, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x16, 0xD5, // disable SOL
0xB5, 0x62, 0x06, 0x01, 0x08, 0x00, 0x01, 0x30, 0x00, 0x0A, 0x00, 0x00, 0x00, 0x00, 0x4A, 0x2D, // enable SVINFO 10 cycle
0xB5, 0x62, 0x06, 0x01, 0x08, 0x00, 0x01, 0x12, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x22, 0x29, // disable VELNED
0xB5, 0x62, 0x06, 0x01, 0x08, 0x00, 0x01, 0x07, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00, 0x18, 0xE1, // enable PVT 1 cycle
0xB5, 0x62, 0x06, 0x08, 0x06, 0x00, 0x64, 0x00, 0x01, 0x00, 0x01, 0x00, 0x7A, 0x12, // set rate to 10Hz (measurement period: 100ms, navigation rate: 1 cycle)
};
#endif
// UBlox 6 Protocol documentation - GPS.G6-SW-10018-F // UBlox 6 Protocol documentation - GPS.G6-SW-10018-F
// SBAS Configuration Settings Desciption, Page 4/210 // SBAS Configuration Settings Desciption, Page 4/210
// 31.21 CFG-SBAS (0x06 0x16), Page 142/210 // 31.21 CFG-SBAS (0x06 0x16), Page 142/210
@ -124,6 +147,11 @@ typedef struct {
uint16_t length; uint16_t length;
} ubx_header; } ubx_header;
typedef struct {
char swVersion[30]; // Zero-terminated Software Version String
char hwVersion[10]; // Zero-terminated Hardware Version String
} ubx_mon_ver;
typedef struct { typedef struct {
uint32_t time; // GPS msToW uint32_t time; // GPS msToW
int32_t longitude; int32_t longitude;
@ -195,17 +223,53 @@ 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 {
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;
enum { enum {
PREAMBLE1 = 0xb5, PREAMBLE1 = 0xb5,
PREAMBLE2 = 0x62, PREAMBLE2 = 0x62,
CLASS_NAV = 0x01, CLASS_NAV = 0x01,
CLASS_ACK = 0x05, CLASS_ACK = 0x05,
CLASS_CFG = 0x06, CLASS_CFG = 0x06,
CLASS_MON = 0x0A,
MSG_VER = 0x04,
MSG_ACK_NACK = 0x00, MSG_ACK_NACK = 0x00,
MSG_ACK_ACK = 0x01, MSG_ACK_ACK = 0x01,
MSG_POSLLH = 0x2, MSG_POSLLH = 0x2,
MSG_STATUS = 0x3, MSG_STATUS = 0x3,
MSG_SOL = 0x6, MSG_SOL = 0x6,
MSG_PVT = 0x7,
MSG_VELNED = 0x12, MSG_VELNED = 0x12,
MSG_SVINFO = 0x30, MSG_SVINFO = 0x30,
MSG_CFG_PRT = 0x00, MSG_CFG_PRT = 0x00,
@ -264,14 +328,15 @@ static bool _new_speed;
#define MAX_UBLOX_PAYLOAD_SIZE 344 #define MAX_UBLOX_PAYLOAD_SIZE 344
#define UBLOX_BUFFER_SIZE MAX_UBLOX_PAYLOAD_SIZE #define UBLOX_BUFFER_SIZE MAX_UBLOX_PAYLOAD_SIZE
// Receive buffer // Receive buffer
static union { static union {
ubx_nav_posllh posllh; ubx_nav_posllh posllh;
ubx_nav_status status; ubx_nav_status status;
ubx_nav_solution solution; ubx_nav_solution solution;
ubx_nav_velned velned; ubx_nav_velned velned;
ubx_nav_pvt pvt;
ubx_nav_svinfo svinfo; ubx_nav_svinfo svinfo;
ubx_mon_ver ver;
uint8_t bytes[UBLOX_BUFFER_SIZE]; uint8_t bytes[UBLOX_BUFFER_SIZE];
} _buffer; } _buffer;
@ -320,6 +385,32 @@ static bool gpsParceFrameUBLOX(void)
gpsSol.flags.validVelD = 1; gpsSol.flags.validVelD = 1;
_new_speed = true; _new_speed = true;
break; break;
#ifdef GPS_PROTO_UBLOX_NEO7PLUS
case MSG_PVT:
next_fix = (_buffer.pvt.fix_status & NAV_STATUS_FIX_VALID) && (_buffer.pvt.fix_type == FIX_3D);
gpsSol.flags.fix3D = next_fix ? 1 : 0;
gpsSol.llh.lon = _buffer.pvt.longitude;
gpsSol.llh.lat = _buffer.pvt.latitude;
gpsSol.llh.alt = _buffer.pvt.altitude_msl / 10; //alt in cm
gpsSol.velNED[X]=_buffer.pvt.ned_north / 10; // to cm/s
gpsSol.velNED[Y]=_buffer.pvt.ned_east / 10; // to cm/s
gpsSol.velNED[Z]=_buffer.pvt.ned_down / 10; // to cm/s
gpsSol.groundSpeed = _buffer.pvt.speed_2d / 10; // to cm/s
gpsSol.groundCourse = (uint16_t) (_buffer.pvt.heading_2d / 10000); // Heading 2D deg * 100000 rescaled to deg * 10
gpsSol.numSat = _buffer.pvt.satellites;
gpsSol.hdop = _buffer.pvt.position_DOP;
gpsSol.flags.validVelNE = 1;
gpsSol.flags.validVelD = 1;
_new_position = true;
_new_speed = true;
break;
case MSG_VER:
if(_class == CLASS_MON) {
//uint32_t swver = _buffer.ver.swVersion;
gpsState.hwVersion = atoi(_buffer.ver.hwVersion);
}
break;
#endif
case MSG_SVINFO: case MSG_SVINFO:
gpsSol.numCh = _buffer.svinfo.numCh; gpsSol.numCh = _buffer.svinfo.numCh;
if (gpsSol.numCh > 16) if (gpsSol.numCh > 16)
@ -342,6 +433,7 @@ static bool gpsParceFrameUBLOX(void)
_new_speed = _new_position = false; _new_speed = _new_position = false;
return true; return true;
} }
return false; return false;
} }
@ -350,89 +442,85 @@ static bool gpsNewFrameUBLOX(uint8_t data)
bool parsed = false; bool parsed = false;
switch (_step) { switch (_step) {
case 0: // Sync char 1 (0xB5) case 0: // Sync char 1 (0xB5)
if (PREAMBLE1 == data) { if (PREAMBLE1 == data) {
_skip_packet = false; _skip_packet = false;
_step++;
}
break;
case 1: // Sync char 2 (0x62)
if (PREAMBLE2 != data) {
_step = 0;
break;
}
_step++; _step++;
}
break;
case 1: // Sync char 2 (0x62)
if (PREAMBLE2 != data) {
_step = 0;
break; break;
} case 2: // Class
_step++;
break;
case 2: // Class
_step++;
_class = data;
_ck_b = _ck_a = data; // reset the checksum accumulators
break;
case 3: // Id
_step++;
_ck_b += (_ck_a += data); // checksum byte
_msg_id = data;
break;
case 4: // Payload length (part 1)
_step++;
_ck_b += (_ck_a += data); // checksum byte
_payload_length = data; // payload length low byte
break;
case 5: // Payload length (part 2)
_step++;
_ck_b += (_ck_a += data); // checksum byte
_payload_length |= (uint16_t)(data << 8);
if (_payload_length > MAX_UBLOX_PAYLOAD_SIZE ) {
// we can't receive the whole packet, just log the error and start searching for the next packet.
gpsStats.errors++;
_step = 0;
break;
}
if (_payload_length > UBLOX_BUFFER_SIZE) {
_skip_packet = true;
}
// prepare to receive payload
_payload_counter = 0;
if (_payload_length == 0) {
_step = 7;
}
break;
case 6:
_ck_b += (_ck_a += data); // checksum byte
if (_payload_counter < UBLOX_BUFFER_SIZE) {
_buffer.bytes[_payload_counter] = data;
}
// NOTE: check counter BEFORE increasing so that a payload_size of 65535 is correctly handled. This can happen if garbage data is received.
if (_payload_counter == _payload_length - 1) {
_step++; _step++;
} _class = data;
_payload_counter++; _ck_b = _ck_a = data; // reset the checksum accumulators
break;
case 7:
_step++;
if (_ck_a != data) {
_skip_packet = true; // bad checksum
gpsStats.errors++;
}
break;
case 8:
_step = 0;
if (_ck_b != data) {
gpsStats.errors++;
break; // bad checksum
}
gpsStats.packetCount++;
if (_skip_packet) {
break; break;
} case 3: // Id
_step++;
_ck_b += (_ck_a += data); // checksum byte
_msg_id = data;
break;
case 4: // Payload length (part 1)
_step++;
_ck_b += (_ck_a += data); // checksum byte
_payload_length = data; // payload length low byte
break;
case 5: // Payload length (part 2)
_step++;
_ck_b += (_ck_a += data); // checksum byte
_payload_length |= (uint16_t)(data << 8);
if (_payload_length > MAX_UBLOX_PAYLOAD_SIZE ) {
// we can't receive the whole packet, just log the error and start searching for the next packet.
gpsStats.errors++;
_step = 0;
break;
}
// prepare to receive payload
_payload_counter = 0;
if (_payload_length == 0) {
_step = 7;
}
break;
case 6:
_ck_b += (_ck_a += data); // checksum byte
if (_payload_counter < MAX_UBLOX_PAYLOAD_SIZE) {
_buffer.bytes[_payload_counter] = data;
}
// NOTE: check counter BEFORE increasing so that a payload_size of 65535 is correctly handled. This can happen if garbage data is received.
if (_payload_counter == _payload_length - 1) {
_step++;
}
_payload_counter++;
break;
case 7:
_step++;
if (_ck_a != data) {
_skip_packet = true; // bad checksum
gpsStats.errors++;
}
break;
case 8:
_step = 0;
if (gpsParceFrameUBLOX()) { if (_ck_b != data) {
parsed = true; gpsStats.errors++;
} break; // bad checksum
}
gpsStats.packetCount++;
if (_skip_packet) {
break;
}
if (gpsParceFrameUBLOX()) {
parsed = true;
}
} }
return parsed; return parsed;
@ -453,14 +541,30 @@ static bool gpsConfigure(void)
{ {
switch (gpsState.autoConfigStep) { switch (gpsState.autoConfigStep) {
case 0: // Config case 0: // Config
if (gpsState.autoConfigPosition < sizeof(ubloxInit6)) { #ifdef GPS_PROTO_UBLOX_NEO7PLUS
serialWrite(gpsState.gpsPort, ubloxInit6[gpsState.autoConfigPosition]); if (gpsState.hwVersion < 70000) {
gpsState.autoConfigPosition++; #endif
if (gpsState.autoConfigPosition < sizeof(ubloxInit6)) {
serialWrite(gpsState.gpsPort, ubloxInit6[gpsState.autoConfigPosition]);
gpsState.autoConfigPosition++;
}
else {
gpsState.autoConfigPosition = 0;
gpsState.autoConfigStep++;
}
#ifdef GPS_PROTO_UBLOX_NEO7PLUS
} }
else { else {
gpsState.autoConfigPosition = 0; if (gpsState.autoConfigPosition < sizeof(ubloxInit)) {
gpsState.autoConfigStep++; serialWrite(gpsState.gpsPort, ubloxInit[gpsState.autoConfigPosition]);
gpsState.autoConfigPosition++;
}
else {
gpsState.autoConfigPosition = 0;
gpsState.autoConfigStep++;
}
} }
#endif
break; break;
case 1: // SBAS case 1: // SBAS
@ -483,6 +587,32 @@ static bool gpsConfigure(void)
return false; return false;
} }
static bool gpsCheckVersion(void)
{
#ifdef GPS_PROTO_UBLOX_NEO7PLUS
if (gpsState.autoConfigStep == 0) {
if (gpsState.autoConfigPosition < sizeof(ubloxVerPoll)) {
serialWrite(gpsState.gpsPort, ubloxVerPoll[gpsState.autoConfigPosition]);
gpsState.autoConfigPosition++;
}
else {
gpsState.autoConfigStep++;
}
}
else {
// Wait until version found
if (gpsState.hwVersion != 0) {
gpsState.autoConfigStep = 0;
gpsState.autoConfigPosition = 0;
gpsSetState(GPS_CONFIGURE);
}
}
#else
gpsSetState(GPS_CONFIGURE);
#endif
return false;
}
static bool gpsReceiveData(void) static bool gpsReceiveData(void)
{ {
bool hasNewData = false; bool hasNewData = false;
@ -512,6 +642,9 @@ bool gpsHandleUBLOX(void)
case GPS_INITIALIZING: case GPS_INITIALIZING:
return gpsInitialize(); return gpsInitialize();
case GPS_CHECK_VERSION:
return gpsCheckVersion();
case GPS_CONFIGURE: case GPS_CONFIGURE:
return gpsConfigure(); return gpsConfigure();

2
src/main/io/serial_cli.c
View file

@ -322,7 +322,7 @@ static const char * const lookupTableAlignment[] = {
#ifdef GPS #ifdef GPS
static const char * const lookupTableGPSProvider[] = { static const char * const lookupTableGPSProvider[] = {
"NMEA", "UBLOX", "I2CNAV" "NMEA", "UBLOX", "I2C-NAV", "NAZA"
}; };
static const char * const lookupTableGPSSBASMode[] = { static const char * const lookupTableGPSSBASMode[] = {