mirror of
https://github.com/iNavFlight/inav.git
synced 2025-07-23 08:15:26 +03:00
GPS: UBLOX version auto-detection and PVT message support from @Sambas. Some code refactoring. NAZA GPS support.
This commit is contained in:
Konstantin Sharlaimov (DigitalEntity)
parent
e07248787a
commit
28672eb0c6
11 changed files with 276 additions and 205 deletions
|
|
@ -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
|
||||
|
||||
|
|
@ -22,7 +22,6 @@
|
|||
|
||||
#include "build_config.h"
|
||||
|
||||
#include "gps.h"
|
||||
#include "gps_i2cnav.h"
|
||||
|
||||
#include "gpio.h"
|
||||
|
|
@ -56,7 +55,7 @@ bool i2cnavGPSModuleDetect(void)
|
|||
return false;
|
||||
}
|
||||
|
||||
void i2cnavGPSModuleRead(gpsDataGeneric_t * gpsMsg)
|
||||
void i2cnavGPSModuleRead(gpsDataI2CNAV_t * gpsMsg)
|
||||
{
|
||||
bool ack;
|
||||
uint8_t i2cGpsStatus;
|
||||
|
|
|
|||
|
|
@ -17,7 +17,20 @@
|
|||
|
||||
#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);
|
||||
void i2cnavGPSModuleRead(gpsDataGeneric_t * gpsMsg);
|
||||
void i2cnavGPSModuleRead(gpsDataI2CNAV_t * gpsMsg);
|
||||
|
|
|
|||
|
|
@ -18,7 +18,6 @@
|
|||
#include <stdbool.h>
|
||||
#include <stdint.h>
|
||||
#include <ctype.h>
|
||||
#include <string.h>
|
||||
#include <math.h>
|
||||
|
||||
#include "platform.h"
|
||||
|
|
@ -37,9 +36,6 @@
|
|||
#include "drivers/sensor.h"
|
||||
#include "drivers/compass.h"
|
||||
|
||||
#include "drivers/gps.h"
|
||||
#include "drivers/gps_i2cnav.h"
|
||||
|
||||
#include "sensors/sensors.h"
|
||||
#include "sensors/compass.h"
|
||||
|
||||
|
|
@ -48,9 +44,6 @@
|
|||
#include "io/gps.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 "config/config.h"
|
||||
|
|
@ -258,13 +251,15 @@ void gpsThread(void)
|
|||
if ((millis() - gpsState.lastStateSwitchMs) >= GPS_INIT_DELAY) {
|
||||
// Switch to required serial port baud
|
||||
serialSetBaudRate(gpsState.gpsPort, baudRates[gpsToSerialBaudRate[gpsState.baudrateIndex]]);
|
||||
gpsState.hwVersion = 0;
|
||||
gpsState.autoConfigStep = 0;
|
||||
gpsState.autoConfigPosition = 0;
|
||||
gpsState.lastMessageMs = millis();
|
||||
gpsSetState(GPS_CONFIGURE);
|
||||
gpsSetState(GPS_CHECK_VERSION);
|
||||
}
|
||||
break;
|
||||
|
||||
case GPS_CHECK_VERSION:
|
||||
case GPS_CONFIGURE:
|
||||
case GPS_RECEIVING_DATA:
|
||||
gpsHandleProtocol();
|
||||
|
|
@ -298,11 +293,12 @@ void gpsThread(void)
|
|||
gpsResetSolution();
|
||||
|
||||
if (gpsProviders[gpsState.gpsConfig->provider].detect && gpsProviders[gpsState.gpsConfig->provider].detect()) {
|
||||
gpsState.hwVersion = 0;
|
||||
gpsState.autoConfigStep = 0;
|
||||
gpsState.autoConfigPosition = 0;
|
||||
gpsState.lastMessageMs = millis();
|
||||
sensorsSet(SENSOR_GPS);
|
||||
gpsSetState(GPS_CONFIGURE);
|
||||
gpsSetState(GPS_CHECK_VERSION);
|
||||
}
|
||||
else {
|
||||
sensorsClear(SENSOR_GPS);
|
||||
|
|
@ -310,6 +306,7 @@ void gpsThread(void)
|
|||
}
|
||||
break;
|
||||
|
||||
case GPS_CHECK_VERSION:
|
||||
case GPS_CONFIGURE:
|
||||
case GPS_RECEIVING_DATA:
|
||||
gpsHandleProtocol();
|
||||
|
|
|
|||
|
|
@ -31,6 +31,7 @@ typedef enum {
|
|||
GPS_NMEA = 0,
|
||||
GPS_UBLOX,
|
||||
GPS_I2CNAV,
|
||||
GPS_NAZA,
|
||||
GPS_PROVIDER_COUNT
|
||||
} gpsProvider_e;
|
||||
|
||||
|
|
|
|||
|
|
@ -18,8 +18,6 @@
|
|||
#include <stdbool.h>
|
||||
#include <stdint.h>
|
||||
#include <ctype.h>
|
||||
#include <string.h>
|
||||
#include <math.h>
|
||||
|
||||
#include "platform.h"
|
||||
#include "build_config.h"
|
||||
|
|
@ -31,18 +29,9 @@
|
|||
|
||||
#include "drivers/system.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 "sensors/sensors.h"
|
||||
|
||||
#include "io/serial.h"
|
||||
#include "io/display.h"
|
||||
#include "io/gps.h"
|
||||
#include "io/gps_private.h"
|
||||
|
||||
|
|
@ -65,7 +54,7 @@ bool gpsDetectI2CNAV(void)
|
|||
|
||||
bool gpsPollI2CNAV(void)
|
||||
{
|
||||
gpsDataGeneric_t gpsMsg;
|
||||
gpsDataI2CNAV_t gpsMsg;
|
||||
|
||||
// Check for poll rate timeout
|
||||
if ((millis() - gpsState.lastMessageMs) < (1000 / GPS_I2C_POLL_RATE_HZ))
|
||||
|
|
@ -113,6 +102,7 @@ bool gpsHandleI2CNAV(void)
|
|||
default:
|
||||
return false;
|
||||
|
||||
case GPS_CHECK_VERSION:
|
||||
case GPS_CONFIGURE:
|
||||
gpsSetState(GPS_RECEIVING_DATA);
|
||||
return false;
|
||||
|
|
|
|||
|
|
@ -32,24 +32,12 @@
|
|||
#include "drivers/system.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 "sensors/sensors.h"
|
||||
|
||||
#include "io/serial.h"
|
||||
#include "io/display.h"
|
||||
#include "io/gps.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 "config/config.h"
|
||||
#include "config/runtime_config.h"
|
||||
|
|
@ -263,9 +251,13 @@ static bool gpsNewFrameNAZA(uint8_t data)
|
|||
_ck_b += (_ck_a += data); // checksum byte
|
||||
_payload_length = data; // payload length low byte
|
||||
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) {
|
||||
_step = 6;
|
||||
}
|
||||
|
|
@ -288,7 +280,6 @@ static bool gpsNewFrameNAZA(uint8_t data)
|
|||
break;
|
||||
case 6:
|
||||
_step = 0;
|
||||
|
||||
if (_ck_b != data) {
|
||||
gpsStats.errors++;
|
||||
break; // bad checksum
|
||||
|
|
@ -314,13 +305,6 @@ static bool gpsInitialize(void)
|
|||
return false;
|
||||
}
|
||||
|
||||
static bool gpsConfigure(void)
|
||||
{
|
||||
// No autoconfig, switch straight to receiving data
|
||||
gpsSetState(GPS_RECEIVING_DATA);
|
||||
return false;
|
||||
}
|
||||
|
||||
static bool gpsReceiveData(void)
|
||||
{
|
||||
bool hasNewData = false;
|
||||
|
|
@ -353,8 +337,11 @@ bool gpsHandleNAZA(void)
|
|||
case GPS_INITIALIZING:
|
||||
return gpsInitialize();
|
||||
|
||||
case GPS_CHECK_VERSION:
|
||||
case GPS_CONFIGURE:
|
||||
return gpsConfigure();
|
||||
// No autoconfig, switch straight to receiving data
|
||||
gpsSetState(GPS_RECEIVING_DATA);
|
||||
return false;
|
||||
|
||||
case GPS_RECEIVING_DATA:
|
||||
return hasNewData;
|
||||
|
|
|
|||
|
|
@ -32,24 +32,12 @@
|
|||
#include "drivers/system.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 "sensors/sensors.h"
|
||||
|
||||
#include "io/serial.h"
|
||||
#include "io/display.h"
|
||||
#include "io/gps.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 "config/config.h"
|
||||
#include "config/runtime_config.h"
|
||||
|
|
@ -281,13 +269,6 @@ static bool gpsInitialize(void)
|
|||
return false;
|
||||
}
|
||||
|
||||
static bool gpsConfigure(void)
|
||||
{
|
||||
// No autoconfig, switch straight to receiving data
|
||||
gpsSetState(GPS_RECEIVING_DATA);
|
||||
return false;
|
||||
}
|
||||
|
||||
static bool gpsReceiveData(void)
|
||||
{
|
||||
bool hasNewData = false;
|
||||
|
|
@ -320,8 +301,11 @@ bool gpsHandleNMEA(void)
|
|||
case GPS_INITIALIZING:
|
||||
return gpsInitialize();
|
||||
|
||||
case GPS_CHECK_VERSION:
|
||||
case GPS_CONFIGURE:
|
||||
return gpsConfigure();
|
||||
// No autoconfig, switch straight to receiving data
|
||||
gpsSetState(GPS_RECEIVING_DATA);
|
||||
return false;
|
||||
|
||||
case GPS_RECEIVING_DATA:
|
||||
return hasNewData;
|
||||
|
|
|
|||
|
|
@ -23,9 +23,10 @@ typedef enum {
|
|||
GPS_UNKNOWN, // 0
|
||||
GPS_INITIALIZING, // 1
|
||||
GPS_CHANGE_BAUD, // 2
|
||||
GPS_CONFIGURE, // 3
|
||||
GPS_RECEIVING_DATA, // 4
|
||||
GPS_LOST_COMMUNICATION, // 5
|
||||
GPS_CHECK_VERSION, // 3
|
||||
GPS_CONFIGURE, // 4
|
||||
GPS_RECEIVING_DATA, // 5
|
||||
GPS_LOST_COMMUNICATION, // 6
|
||||
} gpsState_e;
|
||||
|
||||
typedef struct {
|
||||
|
|
@ -33,6 +34,8 @@ typedef struct {
|
|||
serialConfig_t * serialConfig;
|
||||
serialPort_t * gpsPort; // Serial GPS only
|
||||
|
||||
uint32_t hwVersion;
|
||||
|
||||
gpsState_e state;
|
||||
gpsBaudRate_e baudrateIndex;
|
||||
gpsBaudRate_e autoBaudrateIndex; // Driver internal use (for autoBaud)
|
||||
|
|
|
|||
|
|
@ -16,6 +16,7 @@
|
|||
*/
|
||||
|
||||
#include <stdbool.h>
|
||||
#include <stdlib.h>
|
||||
#include <stdint.h>
|
||||
#include <ctype.h>
|
||||
#include <string.h>
|
||||
|
|
@ -32,30 +33,20 @@
|
|||
#include "drivers/system.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 "sensors/sensors.h"
|
||||
|
||||
#include "io/serial.h"
|
||||
#include "io/display.h"
|
||||
#include "io/gps.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 "config/config.h"
|
||||
#include "config/runtime_config.h"
|
||||
|
||||
#if defined(GPS) && defined(GPS_PROTO_UBLOX)
|
||||
|
||||
#define GPS_PROTO_UBLOX_NEO7PLUS
|
||||
|
||||
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,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
|
||||
};
|
||||
|
||||
#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[] = {
|
||||
|
||||
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, 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, 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, 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)
|
||||
};
|
||||
|
||||
#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
|
||||
// SBAS Configuration Settings Desciption, Page 4/210
|
||||
// 31.21 CFG-SBAS (0x06 0x16), Page 142/210
|
||||
|
|
@ -124,6 +147,11 @@ typedef struct {
|
|||
uint16_t length;
|
||||
} 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 {
|
||||
uint32_t time; // GPS msToW
|
||||
int32_t longitude;
|
||||
|
|
@ -195,17 +223,53 @@ typedef struct {
|
|||
ubx_nav_svinfo_channel channel[16]; // 16 satellites * 12 byte
|
||||
} 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 {
|
||||
PREAMBLE1 = 0xb5,
|
||||
PREAMBLE2 = 0x62,
|
||||
CLASS_NAV = 0x01,
|
||||
CLASS_ACK = 0x05,
|
||||
CLASS_CFG = 0x06,
|
||||
CLASS_MON = 0x0A,
|
||||
MSG_VER = 0x04,
|
||||
MSG_ACK_NACK = 0x00,
|
||||
MSG_ACK_ACK = 0x01,
|
||||
MSG_POSLLH = 0x2,
|
||||
MSG_STATUS = 0x3,
|
||||
MSG_SOL = 0x6,
|
||||
MSG_PVT = 0x7,
|
||||
MSG_VELNED = 0x12,
|
||||
MSG_SVINFO = 0x30,
|
||||
MSG_CFG_PRT = 0x00,
|
||||
|
|
@ -264,14 +328,15 @@ static bool _new_speed;
|
|||
#define MAX_UBLOX_PAYLOAD_SIZE 344
|
||||
#define UBLOX_BUFFER_SIZE MAX_UBLOX_PAYLOAD_SIZE
|
||||
|
||||
|
||||
// Receive buffer
|
||||
static union {
|
||||
ubx_nav_posllh posllh;
|
||||
ubx_nav_status status;
|
||||
ubx_nav_solution solution;
|
||||
ubx_nav_velned velned;
|
||||
ubx_nav_pvt pvt;
|
||||
ubx_nav_svinfo svinfo;
|
||||
ubx_mon_ver ver;
|
||||
uint8_t bytes[UBLOX_BUFFER_SIZE];
|
||||
} _buffer;
|
||||
|
||||
|
|
@ -320,6 +385,32 @@ static bool gpsParceFrameUBLOX(void)
|
|||
gpsSol.flags.validVelD = 1;
|
||||
_new_speed = true;
|
||||
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:
|
||||
gpsSol.numCh = _buffer.svinfo.numCh;
|
||||
if (gpsSol.numCh > 16)
|
||||
|
|
@ -342,6 +433,7 @@ static bool gpsParceFrameUBLOX(void)
|
|||
_new_speed = _new_position = false;
|
||||
return true;
|
||||
}
|
||||
|
||||
return false;
|
||||
}
|
||||
|
||||
|
|
@ -388,10 +480,6 @@ static bool gpsNewFrameUBLOX(uint8_t data)
|
|||
_step = 0;
|
||||
break;
|
||||
}
|
||||
if (_payload_length > UBLOX_BUFFER_SIZE) {
|
||||
_skip_packet = true;
|
||||
}
|
||||
|
||||
// prepare to receive payload
|
||||
_payload_counter = 0;
|
||||
if (_payload_length == 0) {
|
||||
|
|
@ -400,7 +488,7 @@ static bool gpsNewFrameUBLOX(uint8_t data)
|
|||
break;
|
||||
case 6:
|
||||
_ck_b += (_ck_a += data); // checksum byte
|
||||
if (_payload_counter < UBLOX_BUFFER_SIZE) {
|
||||
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.
|
||||
|
|
@ -453,6 +541,9 @@ static bool gpsConfigure(void)
|
|||
{
|
||||
switch (gpsState.autoConfigStep) {
|
||||
case 0: // Config
|
||||
#ifdef GPS_PROTO_UBLOX_NEO7PLUS
|
||||
if (gpsState.hwVersion < 70000) {
|
||||
#endif
|
||||
if (gpsState.autoConfigPosition < sizeof(ubloxInit6)) {
|
||||
serialWrite(gpsState.gpsPort, ubloxInit6[gpsState.autoConfigPosition]);
|
||||
gpsState.autoConfigPosition++;
|
||||
|
|
@ -461,6 +552,19 @@ static bool gpsConfigure(void)
|
|||
gpsState.autoConfigPosition = 0;
|
||||
gpsState.autoConfigStep++;
|
||||
}
|
||||
#ifdef GPS_PROTO_UBLOX_NEO7PLUS
|
||||
}
|
||||
else {
|
||||
if (gpsState.autoConfigPosition < sizeof(ubloxInit)) {
|
||||
serialWrite(gpsState.gpsPort, ubloxInit[gpsState.autoConfigPosition]);
|
||||
gpsState.autoConfigPosition++;
|
||||
}
|
||||
else {
|
||||
gpsState.autoConfigPosition = 0;
|
||||
gpsState.autoConfigStep++;
|
||||
}
|
||||
}
|
||||
#endif
|
||||
break;
|
||||
|
||||
case 1: // SBAS
|
||||
|
|
@ -483,6 +587,32 @@ static bool gpsConfigure(void)
|
|||
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)
|
||||
{
|
||||
bool hasNewData = false;
|
||||
|
|
@ -512,6 +642,9 @@ bool gpsHandleUBLOX(void)
|
|||
case GPS_INITIALIZING:
|
||||
return gpsInitialize();
|
||||
|
||||
case GPS_CHECK_VERSION:
|
||||
return gpsCheckVersion();
|
||||
|
||||
case GPS_CONFIGURE:
|
||||
return gpsConfigure();
|
||||
|
||||
|
|
|
|||
|
|
@ -322,7 +322,7 @@ static const char * const lookupTableAlignment[] = {
|
|||
|
||||
#ifdef GPS
|
||||
static const char * const lookupTableGPSProvider[] = {
|
||||
"NMEA", "UBLOX", "I2CNAV"
|
||||
"NMEA", "UBLOX", "I2C-NAV", "NAZA"
|
||||
};
|
||||
|
||||
static const char * const lookupTableGPSSBASMode[] = {
|
||||
|
|
|
|||
Loading…
Add table
Add a link
Reference in a new issue