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 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; } }
gpsEnablePassthroughResult_e gpsEnablePassthrough(void) { if (gpsData.state != GPS_RECEIVING_DATA) return GPS_PASSTHROUGH_NO_GPS; serialPort_t *gpsPassthroughPort = findOpenSerialPort(FUNCTION_GPS_PASSTHROUGH); if (gpsPassthroughPort) { waitForSerialPortToFinishTransmitting(gpsPassthroughPort); serialSetBaudRate(gpsPassthroughPort, serialConfig->gps_passthrough_baudrate); } else { gpsPassthroughPort = openSerialPort(FUNCTION_GPS_PASSTHROUGH, NULL, serialConfig->gps_passthrough_baudrate, MODE_RXTX, SERIAL_NOT_INVERTED); if (!gpsPassthroughPort) { return GPS_PASSTHROUGH_NO_SERIAL_PORT; } } LED0_OFF; LED1_OFF; while(1) { if (serialTotalBytesWaiting(gpsPort)) { LED0_ON; serialWrite(gpsPassthroughPort, serialRead(gpsPort)); LED0_OFF; } if (serialTotalBytesWaiting(gpsPassthroughPort)) { LED1_ON; serialWrite(gpsPort, serialRead(gpsPassthroughPort)); LED1_OFF; } } return GPS_PASSTHROUGH_ENABLED; }
void freeFrSkyTelemetryPort(void) { // FIXME only need to reset the port if the port is shared serialSetMode(frskyPort, previousPortMode); serialSetBaudRate(frskyPort, previousBaudRate); endSerialPortFunction(frskyPort, FUNCTION_TELEMETRY); }
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; } }
// 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); } } }
void configureFrSkyTelemetryPort(void) { frskyPort = findOpenSerialPort(FUNCTION_TELEMETRY); if (frskyPort) { previousPortMode = frskyPort->mode; previousBaudRate = frskyPort->baudRate; //waitForSerialPortToFinishTransmitting(frskyPort); // FIXME locks up the system serialSetBaudRate(frskyPort, FRSKY_BAUDRATE); serialSetMode(frskyPort, FRSKY_INITIAL_PORT_MODE); beginSerialPortFunction(frskyPort, FUNCTION_TELEMETRY); } else { frskyPort = openSerialPort(FUNCTION_TELEMETRY, NULL, FRSKY_BAUDRATE, FRSKY_INITIAL_PORT_MODE, telemetryConfig->telemetry_inversion); // FIXME only need these values to reset the port if the port is shared previousPortMode = frskyPort->mode; previousBaudRate = frskyPort->baudRate; } }
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; } }