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;
}
}