void gpsThread(void)
{
switch (gpsData.state) {
case GPS_UNKNOWN:
break;
case GPS_INITIALIZING:
case GPS_INITDONE:
gpsInitHardware();
break;
case GPS_LOSTCOMMS:
gpsData.errors++;
// try another rate
gpsData.baudrateIndex++;
gpsData.baudrateIndex %= GPS_INIT_ENTRIES;
gpsData.lastMessage = millis();
// TODO - move some / all of these into gpsData
GPS_numSat = 0;
f.GPS_FIX = 0;
gpsSetState(GPS_INITIALIZING);
break;
case GPS_RECEIVINGDATA:
// check for no data/gps timeout/cable disconnection etc
if (millis() - gpsData.lastMessage > GPS_TIMEOUT) {
// remove GPS from capability
sensorsClear(SENSOR_GPS);
gpsSetState(GPS_LOSTCOMMS);
}
break;
}
}
static void gpsInitHardware(void)
{
switch (mcfg.gps_type) {
case GPS_NMEA:
// nothing to do, just set baud rate and try receiving some stuff and see if it parses
serialSetBaudRate(core.gpsport, gpsInitData[gpsData.baudrateIndex].baudrate);
gpsSetState(GPS_RECEIVINGDATA);
break;
case GPS_UBLOX:
// 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
if (!isSerialTransmitBufferEmpty(core.gpsport))
break;
if (gpsData.state == GPS_INITIALIZING) {
uint32_t m = millis();
if (m - gpsData.state_ts < GPS_BAUD_DELAY)
return;
if (gpsData.state_position < GPS_INIT_ENTRIES) {
// try different speed to INIT
serialSetBaudRate(core.gpsport, gpsInitData[gpsData.state_position].baudrate);
// but print our FIXED init string for the baudrate we want to be at
serialPrint(core.gpsport, gpsInitData[gpsData.baudrateIndex].ubx);
gpsData.state_position++;
gpsData.state_ts = m;
} else {
// we're now (hopefully) at the correct rate, next state will switch to it
gpsSetState(GPS_INITDONE);
}
} else {
// GPS_INITDONE, set our real baud rate and push some ublox config strings
if (gpsData.state_position == 0)
serialSetBaudRate(core.gpsport, gpsInitData[gpsData.baudrateIndex].baudrate);
if (gpsData.state_position < sizeof(ubloxInit)) {
serialWrite(core.gpsport, ubloxInit[gpsData.state_position]); // send ubx init binary
gpsData.state_position++;
} else {
// ublox should be init'd, time to try receiving some junk
gpsSetState(GPS_RECEIVINGDATA);
}
}
break;
case GPS_MTK_NMEA:
case GPS_MTK_BINARY:
// TODO. need to find my old piece of shit MTK GPS.
break;
}
// clear error counter
gpsData.errors = 0;
}
void gpsInit(serialConfig_t *initialSerialConfig, gpsConfig_t *initialGpsConfig)
{
gpsState.serialConfig = initialSerialConfig;
gpsState.gpsConfig = initialGpsConfig;
gpsState.baudrateIndex = 0;
gpsStats.errors = 0;
gpsStats.timeouts = 0;
gpsResetSolution();
// init gpsData structure. if we're not actually enabled, don't bother doing anything else
gpsState.autoConfigStep = 0;
gpsState.lastMessageMs = millis();
gpsSetState(GPS_UNKNOWN);
if (gpsProviders[gpsState.gpsConfig->provider].type == GPS_TYPE_BUS) {
gpsSetState(GPS_INITIALIZING);
return;
}
if (gpsProviders[gpsState.gpsConfig->provider].type == GPS_TYPE_SERIAL) {
serialPortConfig_t * gpsPortConfig = findSerialPortConfig(FUNCTION_GPS);
if (!gpsPortConfig) {
featureClear(FEATURE_GPS);
}
else {
while (gpsToSerialBaudRate[gpsState.baudrateIndex] != gpsPortConfig->gps_baudrateIndex) {
gpsState.baudrateIndex++;
if (gpsState.baudrateIndex >= GPS_BAUDRATE_COUNT) {
gpsState.baudrateIndex = 0;
break;
}
}
portMode_t mode = gpsProviders[gpsState.gpsConfig->provider].portMode;
// no callback - buffer will be consumed in gpsThread()
gpsState.gpsPort = openSerialPort(gpsPortConfig->identifier, FUNCTION_GPS, NULL, baudRates[gpsToSerialBaudRate[gpsState.baudrateIndex]], mode, SERIAL_NOT_INVERTED);
if (!gpsState.gpsPort) {
featureClear(FEATURE_GPS);
}
else {
gpsSetState(GPS_INITIALIZING);
return;
}
}
}
}
void gpsInit(serialConfig_t *initialSerialConfig, gpsConfig_t *initialGpsConfig)
{
serialConfig = initialSerialConfig;
gpsData.baudrateIndex = 0;
gpsData.errors = 0;
gpsData.timeouts = 0;
memset(gpsPacketLog, 0x00, sizeof(gpsPacketLog));
gpsConfig = initialGpsConfig;
// init gpsData structure. if we're not actually enabled, don't bother doing anything else
gpsSetState(GPS_UNKNOWN);
gpsData.lastMessage = millis();
serialPortConfig_t *gpsPortConfig = findSerialPortConfig(FUNCTION_GPS);
if (!gpsPortConfig) {
featureClear(FEATURE_GPS);
return;
}
while (gpsInitData[gpsData.baudrateIndex].baudrateIndex != gpsPortConfig->gps_baudrateIndex) {
gpsData.baudrateIndex++;
if (gpsData.baudrateIndex >= GPS_INIT_DATA_ENTRY_COUNT) {
gpsData.baudrateIndex = DEFAULT_BAUD_RATE_INDEX;
break;
}
}
portMode_t mode = MODE_RXTX;
// only RX is needed for NMEA-style GPS
#if !defined(COLIBRI_RACE) || !defined(LUX_RACE)
if (gpsConfig->provider == GPS_NMEA)
mode &= ~MODE_TX;
#endif
// no callback - buffer will be consumed in gpsThread()
gpsPort = openSerialPort(gpsPortConfig->identifier, FUNCTION_GPS, NULL, gpsInitData[gpsData.baudrateIndex].baudrateIndex, mode, SERIAL_NOT_INVERTED);
if (!gpsPort) {
featureClear(FEATURE_GPS);
return;
}
// signal GPS "thread" to initialize when it gets to it
gpsSetState(GPS_INITIALIZING);
}
void gpsInitNmea(void)
{
#if defined(COLIBRI_RACE) || defined(LUX_RACE)
uint32_t now;
#endif
switch(gpsData.state) {
case GPS_INITIALIZING:
#if defined(COLIBRI_RACE) || defined(LUX_RACE)
now = millis();
if (now - gpsData.state_ts < 1000)
return;
gpsData.state_ts = now;
if (gpsData.state_position < 1) {
serialSetBaudRate(gpsPort, 4800);
gpsData.state_position++;
} else if (gpsData.state_position < 2) {
// print our FIXED init string for the baudrate we want to be at
serialPrint(gpsPort, "$PSRF100,1,115200,8,1,0*05\r\n");
gpsData.state_position++;
} else {
// we're now (hopefully) at the correct rate, next state will switch to it
gpsSetState(GPS_CHANGE_BAUD);
}
break;
#endif
case GPS_CHANGE_BAUD:
#if defined(COLIBRI_RACE) || defined(LUX_RACE)
now = millis();
if (now - gpsData.state_ts < 1000)
return;
gpsData.state_ts = now;
if (gpsData.state_position < 1) {
serialSetBaudRate(gpsPort, baudRates[gpsInitData[gpsData.baudrateIndex].baudrateIndex]);
gpsData.state_position++;
} else if (gpsData.state_position < 2) {
serialPrint(gpsPort, "$PSRF103,00,6,00,0*23\r\n");
gpsData.state_position++;
} else {
#else
serialSetBaudRate(gpsPort, baudRates[gpsInitData[gpsData.baudrateIndex].baudrateIndex]);
#endif
gpsSetState(GPS_RECEIVING_DATA);
#if defined(COLIBRI_RACE) || defined(LUX_RACE)
}
#endif
break;
}
}
bool gpsHandleNMEA(void)
{
// Receive data
bool hasNewData = gpsReceiveData();
// Process state
switch(gpsState.state) {
default:
return false;
case GPS_INITIALIZING:
return gpsInitialize();
case GPS_CHANGE_BAUD:
return gpsChangeBaud();
case GPS_CHECK_VERSION:
case GPS_CONFIGURE:
// No autoconfig, switch straight to receiving data
gpsSetState(GPS_RECEIVING_DATA);
return false;
case GPS_RECEIVING_DATA:
return hasNewData;
}
}
static void gpsFakeGPSUpdate(void)
{
if (millis() - gpsState.lastMessageMs > 100) {
gpsSol.fixType = GPS_FIX_3D;
gpsSol.numSat = 6;
gpsSol.llh.lat = 509102311;
gpsSol.llh.lon = -15349744;
gpsSol.llh.alt = 0;
gpsSol.groundSpeed = 0;
gpsSol.groundCourse = 0;
gpsSol.velNED[X] = 0;
gpsSol.velNED[Y] = 0;
gpsSol.velNED[Z] = 0;
gpsSol.flags.validVelNE = 1;
gpsSol.flags.validVelD = 1;
gpsSol.flags.validEPE = 1;
gpsSol.eph = 100;
gpsSol.epv = 100;
ENABLE_STATE(GPS_FIX);
sensorsSet(SENSOR_GPS);
onNewGPSData();
gpsState.lastLastMessageMs = gpsState.lastMessageMs;
gpsState.lastMessageMs = millis();
gpsSetState(GPS_RECEIVING_DATA);
}
}
void gpsInitNmea(void)
{
switch(gpsData.state) {
case GPS_INITIALIZING:
case GPS_CHANGE_BAUD:
serialSetBaudRate(gpsPort, baudRates[gpsInitData[gpsData.baudrateIndex].baudrateIndex]);
gpsSetState(GPS_RECEIVING_DATA);
break;
}
}
void gpsThread(void)
{
// read out available GPS bytes
if (gpsPort) {
while (serialRxBytesWaiting(gpsPort))
gpsNewData(serialRead(gpsPort));
}
switch (gpsData.state) {
case GPS_UNKNOWN:
break;
case GPS_INITIALIZING:
case GPS_CHANGE_BAUD:
case GPS_CONFIGURE:
gpsInitHardware();
break;
case GPS_LOST_COMMUNICATION:
gpsData.timeouts++;
if (gpsConfig()->autoBaud) {
// try another rate
gpsData.baudrateIndex++;
gpsData.baudrateIndex %= GPS_INIT_ENTRIES;
}
gpsData.lastMessage = millis();
// TODO - move some / all of these into gpsData
GPS_numSat = 0;
DISABLE_STATE(GPS_FIX);
gpsSetState(GPS_INITIALIZING);
break;
case GPS_RECEIVING_DATA:
// check for no data/gps timeout/cable disconnection etc
if (millis() - gpsData.lastMessage > GPS_TIMEOUT) {
// remove GPS from capability
sensorsClear(SENSOR_GPS);
gpsSetState(GPS_LOST_COMMUNICATION);
}
break;
}
}
void gpsInit(uint8_t baudrate)
{
portMode_t mode = MODE_RXTX;
// init gpsData structure. if we're not actually enabled, don't bother doing anything else
gpsSetState(GPS_UNKNOWN);
if (!feature(FEATURE_GPS))
return;
gpsData.baudrateIndex = baudrate;
gpsData.lastMessage = millis();
gpsData.errors = 0;
// only RX is needed for NMEA-style GPS
if (mcfg.gps_type == GPS_NMEA)
mode = MODE_RX;
gpsSetPIDs();
core.gpsport = uartOpen(USART2, gpsNewData, gpsInitData[baudrate].baudrate, mode);
// signal GPS "thread" to initialize when it gets to it
gpsSetState(GPS_INITIALIZING);
}
// Finish baud rate change sequence - wait for TX buffer to empty and switch to the desired port speed
void gpsFinalizeChangeBaud(void)
{
if ((gpsProviders[gpsState.gpsConfig->provider].type == GPS_TYPE_SERIAL) && (gpsState.gpsPort != NULL)) {
// Wait for GPS_INIT_DELAY before switching to required baud rate
if ((millis() - gpsState.lastStateSwitchMs) >= GPS_BAUD_CHANGE_DELAY && isSerialTransmitBufferEmpty(gpsState.gpsPort)) {
// Switch to required serial port baud
serialSetBaudRate(gpsState.gpsPort, baudRates[gpsToSerialBaudRate[gpsState.baudrateIndex]]);
gpsState.lastMessageMs = millis();
gpsSetState(GPS_CHECK_VERSION);
}
}
}
// When using PWM input GPS usage reduces number of available channels by 2 - see pwm_common.c/pwmInit()
void gpsInit(serialConfig_t *initialSerialConfig, gpsConfig_t *initialGpsConfig)
{
serialConfig = initialSerialConfig;
gpsData.baudrateIndex = 0;
while (gpsInitData[gpsData.baudrateIndex].baudrate != serialConfig->gps_baudrate) {
gpsData.baudrateIndex++;
if (gpsData.baudrateIndex >= GPS_INIT_DATA_ENTRY_COUNT) {
gpsData.baudrateIndex = DEFAULT_BAUD_RATE_INDEX;
break;
}
}
gpsConfig = initialGpsConfig;
// init gpsData structure. if we're not actually enabled, don't bother doing anything else
gpsSetState(GPS_UNKNOWN);
gpsData.lastMessage = millis();
gpsData.errors = 0;
portMode_t mode = MODE_RXTX;
// only RX is needed for NMEA-style GPS
if (gpsConfig->provider == GPS_NMEA)
mode = MODE_RX;
// no callback - buffer will be consumed in gpsThread()
gpsPort = openSerialPort(FUNCTION_GPS, NULL, gpsInitData[gpsData.baudrateIndex].baudrate, mode, SERIAL_NOT_INVERTED);
if (!gpsPort) {
featureClear(FEATURE_GPS);
return;
}
// signal GPS "thread" to initialize when it gets to it
gpsSetState(GPS_INITIALIZING);
}
bool gpsHandleI2CNAV(void)
{
// Process state
switch(gpsState.state) {
default:
return false;
case GPS_CHANGE_BAUD:
case GPS_CHECK_VERSION:
case GPS_CONFIGURE:
gpsSetState(GPS_RECEIVING_DATA);
return false;
case GPS_RECEIVING_DATA:
return gpsPollI2CNAV();
}
}
void gpsInitUblox(void)
{
uint32_t now;
// UBX will run at the serial port's baudrate, it shouldn't be "autodetected". So here we force it to that rate
// Wait until GPS transmit buffer is empty
if (!isSerialTransmitBufferEmpty(gpsPort))
return;
switch (gpsData.state) {
case GPS_INITIALIZING:
now = millis();
if (now - gpsData.state_ts < GPS_BAUDRATE_CHANGE_DELAY)
return;
if (gpsData.state_position < GPS_INIT_ENTRIES) {
// try different speed to INIT
baudRate_e newBaudRateIndex = gpsInitData[gpsData.state_position].baudrateIndex;
gpsData.state_ts = now;
if (lookupBaudRateIndex(serialGetBaudRate(gpsPort)) != newBaudRateIndex) {
// change the rate if needed and wait a little
serialSetBaudRate(gpsPort, baudRates[newBaudRateIndex]);
return;
}
// print our FIXED init string for the baudrate we want to be at
serialPrint(gpsPort, gpsInitData[gpsData.baudrateIndex].ubx);
gpsData.state_position++;
} else {
// we're now (hopefully) at the correct rate, next state will switch to it
gpsSetState(GPS_CHANGE_BAUD);
}
break;
case GPS_CHANGE_BAUD:
serialSetBaudRate(gpsPort, baudRates[gpsInitData[gpsData.baudrateIndex].baudrateIndex]);
gpsSetState(GPS_CONFIGURE);
break;
case GPS_CONFIGURE:
// Either use specific config file for GPS or let dynamically upload config
if( gpsConfig()->autoConfig == GPS_AUTOCONFIG_OFF ) {
gpsSetState(GPS_RECEIVING_DATA);
break;
}
if (gpsData.messageState == GPS_MESSAGE_STATE_IDLE) {
gpsData.messageState++;
}
if (gpsData.messageState == GPS_MESSAGE_STATE_INIT) {
if (gpsData.state_position < sizeof(ubloxInit)) {
serialWrite(gpsPort, ubloxInit[gpsData.state_position]);
gpsData.state_position++;
} else {
gpsData.state_position = 0;
gpsData.messageState++;
}
}
if (gpsData.messageState == GPS_MESSAGE_STATE_SBAS) {
if (gpsData.state_position < UBLOX_SBAS_MESSAGE_LENGTH) {
serialWrite(gpsPort, ubloxSbas[gpsConfig()->sbasMode].message[gpsData.state_position]);
gpsData.state_position++;
} else {
gpsData.messageState++;
}
}
if (gpsData.messageState >= GPS_MESSAGE_STATE_ENTRY_COUNT) {
// ublox should be initialised, try receiving
gpsSetState(GPS_RECEIVING_DATA);
}
break;
}
}
static bool gpsInitialize(void)
{
gpsSetState(GPS_CHANGE_BAUD);
return false;
}
void gpsThread(void)
{
/* Extra delay for at least 2 seconds after booting to give GPS time to initialise */
if (!isMPUSoftReset() && (millis() < GPS_BOOT_DELAY)) {
sensorsClear(SENSOR_GPS);
DISABLE_STATE(GPS_FIX);
return;
}
#ifdef USE_FAKE_GPS
gpsFakeGPSUpdate();
#else
// Serial-based GPS
if ((gpsProviders[gpsState.gpsConfig->provider].type == GPS_TYPE_SERIAL) && (gpsState.gpsPort != NULL)) {
switch (gpsState.state) {
default:
case GPS_INITIALIZING:
if ((millis() - gpsState.lastStateSwitchMs) >= GPS_INIT_DELAY) {
// Reset internals
DISABLE_STATE(GPS_FIX);
gpsSol.fixType = GPS_NO_FIX;
gpsState.hwVersion = 0;
gpsState.autoConfigStep = 0;
gpsState.autoConfigPosition = 0;
gpsState.autoBaudrateIndex = 0;
// Reset solution
gpsResetSolution();
// Call protocol handler - switch to next state is done there
gpsHandleProtocol();
}
break;
case GPS_CHANGE_BAUD:
// Call protocol handler - switch to next state is done there
gpsHandleProtocol();
break;
case GPS_CHECK_VERSION:
case GPS_CONFIGURE:
case GPS_RECEIVING_DATA:
gpsHandleProtocol();
if ((millis() - gpsState.lastMessageMs) > GPS_TIMEOUT) {
// Check for GPS timeout
sensorsClear(SENSOR_GPS);
DISABLE_STATE(GPS_FIX);
gpsSol.fixType = GPS_NO_FIX;
gpsSetState(GPS_LOST_COMMUNICATION);
}
break;
case GPS_LOST_COMMUNICATION:
gpsStats.timeouts++;
// Handle autobaud - switch to next port baud rate
if (gpsState.gpsConfig->autoBaud != GPS_AUTOBAUD_OFF) {
gpsState.baudrateIndex++;
gpsState.baudrateIndex %= GPS_BAUDRATE_COUNT;
}
gpsSetState(GPS_INITIALIZING);
break;
}
}
// Driver-based GPS (I2C)
else if (gpsProviders[gpsState.gpsConfig->provider].type == GPS_TYPE_BUS) {
switch (gpsState.state) {
default:
case GPS_INITIALIZING:
// Detect GPS unit
if ((millis() - gpsState.lastStateSwitchMs) >= GPS_BUS_INIT_DELAY) {
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_CHANGE_BAUD);
}
else {
sensorsClear(SENSOR_GPS);
}
}
break;
case GPS_CHANGE_BAUD:
case GPS_CHECK_VERSION:
case GPS_CONFIGURE:
case GPS_RECEIVING_DATA:
gpsHandleProtocol();
if (millis() - gpsState.lastMessageMs > GPS_TIMEOUT) {
// remove GPS from capability
gpsSetState(GPS_LOST_COMMUNICATION);
}
break;
case GPS_LOST_COMMUNICATION:
// No valid data from GPS unit, cause re-init and re-detection
gpsStats.timeouts++;
DISABLE_STATE(GPS_FIX);
gpsSol.fixType = GPS_NO_FIX;
gpsSetState(GPS_INITIALIZING);
break;
}
}
else {
// GPS_TYPE_NA
}
#endif
}
