rusefi
/
atbetaflight
mirror of https://github.com/rusefi/atbetaflight.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Separate UBLOX GPS SBAS configuration from GPS init. 

Extracted NMEA and UBLOX code from the GPS hardware init method into
separate methods.  Introduced a state for changing baud rate rather than
re-using state_position.
This commit is contained in:
Dominic Clifton 2014-06-03 13:09:45 +01:00
parent 0dc476a999
commit 09a1739ce6
1 changed files with 131 additions and 52 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

137
src/main/io/gps.c
View File

@ -105,18 +105,55 @@ static const uint8_t ubloxInit[] = {
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, 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, 0x16, 0x08, 0x00, 0x03, 0x07, 0x03, 0x00, 0x00, 0x00, 0x00, 0x00, 0x31, 0xE5, // set SBAS to auto-detect
0xB5, 0x62, 0x06, 0x08, 0x06, 0x00, 0xC8, 0x00, 0x01, 0x00, 0x01, 0x00, 0xDE, 0x6A, // set rate to 5Hz 0xB5, 0x62, 0x06, 0x08, 0x06, 0x00, 0xC8, 0x00, 0x01, 0x00, 0x01, 0x00, 0xDE, 0x6A, // set rate to 5Hz
}; };
// 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
// A.10 SBAS Configuration (UBX-CFG-SBAS), Page 198/210 - GPS.G6-SW-10018-F
typedef enum {
SBAS_AUTO = 0,
SBAS_EGNOS,
SBAS_WAAS,
SBAS_MSAS,
SBAS_GAGAN
} sbasMode_e;
#define UBLOX_SBAS_MESSAGE_LENGTH 16
typedef struct ubloxSbas_s {
sbasMode_e mode;
uint8_t message[UBLOX_SBAS_MESSAGE_LENGTH];
} ubloxSbas_t;
static const ubloxSbas_t ubloxSbas[] = {
{ SBAS_AUTO, { 0xB5, 0x62, 0x06, 0x16, 0x08, 0x00, 0x03, 0x07, 0x03, 0x00, 0x00, 0x00, 0x00, 0x00, 0x31, 0xE5}},
{ SBAS_EGNOS, { 0xB5, 0x62, 0x06, 0x16, 0x08, 0x00, 0x03, 0x07, 0x03, 0x00, 0x51, 0x08, 0x00, 0x00, 0x8A, 0x41}},
{ SBAS_WAAS, { 0xB5, 0x62, 0x06, 0x16, 0x08, 0x00, 0x03, 0x07, 0x03, 0x00, 0x04, 0xE0, 0x04, 0x00, 0x19, 0x9D}},
{ SBAS_MSAS, { 0xB5, 0x62, 0x06, 0x16, 0x08, 0x00, 0x03, 0x07, 0x03, 0x00, 0x00, 0x02, 0x02, 0x00, 0x35, 0xEF}},
{ SBAS_GAGAN, { 0xB5, 0x62, 0x06, 0x16, 0x08, 0x00, 0x03, 0x07, 0x03, 0x00, 0x80, 0x01, 0x00, 0x00, 0xB2, 0xE8}}
};
enum { enum {
GPS_UNKNOWN, GPS_UNKNOWN,
GPS_INITIALIZING, GPS_INITIALIZING,
GPS_INITDONE, GPS_CHANGE_BAUD,
GPS_RECEIVINGDATA, GPS_CONFIGURE,
GPS_LOSTCOMMS, GPS_RECEIVING_DATA,
GPS_LOST_COMMUNICATION,
}; };
typedef enum {
GPS_MESSAGE_STATE_IDLE = 0,
GPS_MESSAGE_STATE_INIT,
GPS_MESSAGE_STATE_SBAS,
GPS_MESSAGE_STATE_MAX = GPS_MESSAGE_STATE_SBAS
} gpsMessageState_e;
#define GPS_MESSAGE_STATE_ENTRY_COUNT (GPS_MESSAGE_STATE_MAX + 1)
typedef struct gpsData_t { typedef struct gpsData_t {
uint8_t state; // GPS thread state. Used for detecting cable disconnects and configuring attached devices uint8_t state; // GPS thread state. Used for detecting cable disconnects and configuring attached devices
uint8_t baudrateIndex; // index into auto-detecting or current baudrate uint8_t baudrateIndex; // index into auto-detecting or current baudrate
@ -126,7 +163,7 @@ typedef struct gpsData_t {
uint32_t state_position; // incremental variable for loops uint32_t state_position; // incremental variable for loops
uint32_t state_ts; // timestamp for last state_position increment uint32_t state_ts; // timestamp for last state_position increment
gpsMessageState_e messageState;
} gpsData_t; } gpsData_t;
static gpsData_t gpsData; static gpsData_t gpsData;
@ -142,6 +179,7 @@ static void gpsSetState(uint8_t state)
gpsData.state = state; gpsData.state = state;
gpsData.state_position = 0; gpsData.state_position = 0;
gpsData.state_ts = millis(); gpsData.state_ts = millis();
gpsData.messageState = GPS_MESSAGE_STATE_IDLE;
} }
void gpsUseProfile(gpsProfile_t *gpsProfileToUse) void gpsUseProfile(gpsProfile_t *gpsProfileToUse)
@ -190,25 +228,31 @@ void gpsInit(serialConfig_t *initialSerialConfig, uint8_t initialGpsProvider, gp
gpsSetState(GPS_INITIALIZING); gpsSetState(GPS_INITIALIZING);
} }
void gpsInitHardware(void) void gpsInitNmea(void)
{ {
switch (gpsProvider) { switch(gpsData.state) {
case GPS_NMEA: case GPS_INITIALIZING:
// nothing to do, just set baud rate and try receiving some stuff and see if it parses case GPS_CHANGE_BAUD:
serialSetBaudRate(gpsPort, gpsInitData[gpsData.baudrateIndex].baudrate); serialSetBaudRate(gpsPort, gpsInitData[gpsData.baudrateIndex].baudrate);
gpsSetState(GPS_RECEIVINGDATA); gpsSetState(GPS_RECEIVING_DATA);
break; break;
}
}
case GPS_UBLOX: void gpsInitUblox(void)
{
uint32_t now;
// UBX will run at mcfg.gps_baudrate, it shouldn't be "autodetected". So here we force it to that rate // UBX will run at mcfg.gps_baudrate, it shouldn't be "autodetected". So here we force it to that rate
// Wait until GPS transmit buffer is empty // Wait until GPS transmit buffer is empty
if (!isSerialTransmitBufferEmpty(gpsPort)) if (!isSerialTransmitBufferEmpty(gpsPort))
break; return;
if (gpsData.state == GPS_INITIALIZING) {
uint32_t m = millis(); switch (gpsData.state) {
if (m - gpsData.state_ts < GPS_BAUDRATE_CHATE_DELAY) case GPS_INITIALIZING:
now = millis();
if (now - gpsData.state_ts < GPS_BAUDRATE_CHATE_DELAY)
return; return;
if (gpsData.state_position < GPS_INIT_ENTRIES) { if (gpsData.state_position < GPS_INIT_ENTRIES) {
@ -218,25 +262,59 @@ void gpsInitHardware(void)
serialPrint(gpsPort, gpsInitData[gpsData.baudrateIndex].ubx); serialPrint(gpsPort, gpsInitData[gpsData.baudrateIndex].ubx);
gpsData.state_position++; gpsData.state_position++;
gpsData.state_ts = m; gpsData.state_ts = now;
} else { } else {
// we're now (hopefully) at the correct rate, next state will switch to it // we're now (hopefully) at the correct rate, next state will switch to it
gpsSetState(GPS_INITDONE); gpsSetState(GPS_CHANGE_BAUD);
} }
} else { break;
// GPS_INITDONE, set our real baud rate and push some ublox config strings case GPS_CHANGE_BAUD:
if (gpsData.state_position == 0)
serialSetBaudRate(gpsPort, gpsInitData[gpsData.baudrateIndex].baudrate); serialSetBaudRate(gpsPort, gpsInitData[gpsData.baudrateIndex].baudrate);
gpsSetState(GPS_CONFIGURE);
break;
case GPS_CONFIGURE:
if (gpsData.messageState == GPS_MESSAGE_STATE_IDLE) {
gpsData.messageState++;
}
if (gpsData.messageState == GPS_MESSAGE_STATE_INIT) {
if (gpsData.state_position < sizeof(ubloxInit)) { if (gpsData.state_position < sizeof(ubloxInit)) {
serialWrite(gpsPort, ubloxInit[gpsData.state_position]); // send ubx init binary serialWrite(gpsPort, ubloxInit[gpsData.state_position]);
gpsData.state_position++; gpsData.state_position++;
} else { } else {
// ublox should be init'd, time to try receiving some junk gpsData.state_position = 0;
gpsSetState(GPS_RECEIVINGDATA); gpsData.messageState++;
} }
} }
if (gpsData.messageState == GPS_MESSAGE_STATE_SBAS) {
uint8_t sbasIndex = SBAS_AUTO; // TODO allow configuration
if (gpsData.state_position < UBLOX_SBAS_MESSAGE_LENGTH) {
serialWrite(gpsPort, ubloxSbas[sbasIndex].message[gpsData.state_position]);
gpsData.state_position++;
} else {
gpsData.messageState++;
}
}
if (gpsData.messageState >= GPS_MESSAGE_STATE_ENTRY_COUNT) {
gpsSetState(GPS_RECEIVING_DATA);
// ublox should be init'd, time to try receiving
}
break;
}
}
void gpsInitHardware(void)
{
switch (gpsProvider) {
case GPS_NMEA:
gpsInitNmea();
break;
case GPS_UBLOX:
gpsInitUblox();
break; break;
} }
@ -257,11 +335,12 @@ void gpsThread(void)
break; break;
case GPS_INITIALIZING: case GPS_INITIALIZING:
case GPS_INITDONE: case GPS_CHANGE_BAUD:
case GPS_CONFIGURE:
gpsInitHardware(); gpsInitHardware();
break; break;
case GPS_LOSTCOMMS: case GPS_LOST_COMMUNICATION:
gpsData.errors++; gpsData.errors++;
// try another rate // try another rate
gpsData.baudrateIndex++; gpsData.baudrateIndex++;
@ -273,12 +352,12 @@ void gpsThread(void)
gpsSetState(GPS_INITIALIZING); gpsSetState(GPS_INITIALIZING);
break; break;
case GPS_RECEIVINGDATA: case GPS_RECEIVING_DATA:
// check for no data/gps timeout/cable disconnection etc // check for no data/gps timeout/cable disconnection etc
if (millis() - gpsData.lastMessage > GPS_TIMEOUT) { if (millis() - gpsData.lastMessage > GPS_TIMEOUT) {
// remove GPS from capability // remove GPS from capability
sensorsClear(SENSOR_GPS); sensorsClear(SENSOR_GPS);
gpsSetState(GPS_LOSTCOMMS); gpsSetState(GPS_LOST_COMMUNICATION);
} }
break; break;
} }
@ -612,7 +691,7 @@ void gpsUsePIDs(pidProfile_t *pidProfile)
gpsEnablePassthroughResult_e gpsEnablePassthrough(void) gpsEnablePassthroughResult_e gpsEnablePassthrough(void)
{ {
if (gpsData.state != GPS_RECEIVINGDATA) if (gpsData.state != GPS_RECEIVING_DATA)
return GPS_PASSTHROUGH_NO_GPS; return GPS_PASSTHROUGH_NO_GPS;
serialPort_t *gpsPassthroughPort = findOpenSerialPort(FUNCTION_GPS_PASSTHROUGH); serialPort_t *gpsPassthroughPort = findOpenSerialPort(FUNCTION_GPS_PASSTHROUGH);