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 }