void UbloxForceBaud(uint32_t baud)
{
uint8_t i;
for (i = 0; i < 5; i++)
{
delay(50);
uart2ChangeBaud(init_speed[i]);
delay(250);
switch(baud)
{
case 19200:
gpsPrint("$PUBX,41,1,0003,0001,19200,0*23\r\n");
break;
case 38400:
gpsPrint("$PUBX,41,1,0003,0001,38400,0*26\r\n");
break;
case 57600:
gpsPrint("$PUBX,41,1,0003,0001,57600,0*2D\r\n");
break;
case 115200:
gpsPrint("$PUBX,41,1,0003,0001,115200,0*1E\r\n");
break;
}
}
uart2ChangeBaud(baud);
delay(200);
}
////////////////////////////////////////////////////////////////////////////////////
// *** GPS INIT ***
////////////////////////////////////////////////////////////////////////////////////
void gpsInit(uint32_t baudrate) // Called in Main
{
uint8_t i;
uint32_t timeout;
GPS_Present = 0;
delay(2000); // let it init
timeout = millis() + 12000; // 12 sec timeout
while (!GPS_Present && millis() < timeout) // Repeat while no GPS Data
{
uart2Init(baudrate, GPS_NewData, false); // Set up Interrupthandler
switch (cfg.gps_type) // GPS_NMEA = 0, GPS_UBLOX = 1, GPS_MTK16 = 2, GPS_MTK19 = 3, GPS_UBLOX_DUMB = 4
{
case 0: // GPS_NMEA
break;
case 1: // GPS_UBLOX
UbloxForceBaud(baudrate);
for (i = 0; i < sizeof(ubloxInit); i++)
{
delay(7);
uart2Write(ubloxInit[i]); // send ubx init binary
}
break;
case 2: // GPS_MTK16
case 3: // GPS_MTK19
for (i = 0; i < 5; i++)
{
uart2ChangeBaud(init_speed[i]);
delay(200);
gpsPrint(MTK_BAUD_RATE_57600);
}
uart2ChangeBaud(57600);
delay(200);
gpsPrint(MTK_SET_BINARY);
delay(200);
gpsPrint(MTK_OUTPUT_5HZ);
delay(200);
gpsPrint(MTK_SBAS_INTEGRITYMODE);
delay(200);
gpsPrint(MTK_NAVTHRES_OFF);
delay(200);
gpsPrint(MTK_SBAS_ON);
delay(200);
gpsPrint(MTK_WAAS_ON);
break;
case 4: // GPS_UBLOX_DUMB = 4
break;
}
delay(1000);
}
if (GPS_Present) sensorsSet(SENSOR_GPS); // Do we get Data? Is GPS present?
}
void gpsInit(uint32_t baudrate)
{
int i;
int offset = 0;
gps.bytes_rx = 0;
gps.frames_rx = 0;
// Create semaphore for OSD frame drawing syncronization
vSemaphoreCreateBinary(gpsSemaphore);
// Create GPS sensor lost timer at 3 seconds
gpsLostTimer = xTimerCreate((signed char *) "TimGPSlost", 3000, pdFALSE, (void *) NULL, gpsLostTimerCallback);
// Start GPS lost timer
xTimerStart(gpsLostTimer, 10);
GPS_set_pids();
if (cfg.gps_baudrate == 0 || cfg.hil_mode != 0)
return; // GPS not configured or HIL mode
uartInit(GPS_UART, baudrate, GPS_NewData);
if (cfg.gps_type == GPS_UBLOX)
offset = 0;
else if (cfg.gps_type == GPS_MTK)
offset = 4;
if (cfg.gps_type != GPS_NMEA) {
for (i = 0; i < 5; i++) {
uartChangeBaud(GPS_UART, init_speed[i]);
switch (baudrate) {
case 19200:
gpsPrint(gpsInitStrings[offset]);
break;
case 38400:
gpsPrint(gpsInitStrings[offset + 1]);
break;
case 57600:
gpsPrint(gpsInitStrings[offset + 2]);
break;
case 115200:
gpsPrint(gpsInitStrings[offset + 3]);
break;
}
vTaskDelay(10 / portTICK_RATE_MS); // Delay on 10 ms
}
}
uartChangeBaud(GPS_UART, baudrate);
if (cfg.gps_type == GPS_UBLOX) {
for (i = 0; i < sizeof(ubloxInit); i++) {
uartWrite(GPS_UART, ubloxInit[i]); // send ubx init binary
vTaskDelay(4 / portTICK_RATE_MS); // Delay on 4 ms
}
} else if (cfg.gps_type == GPS_MTK) {
gpsPrint("$PMTK314,0,1,0,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0*28\r\n"); // only GGA and RMC sentence
gpsPrint("$PMTK220,200*2C\r\n"); // 5 Hz update rate
}
}
void initGPS(void)
{
uint8_t i;
switch(eepromConfig.gpsType)
{
///////////////////////////////
case NO_GPS:
break;
///////////////////////////////
case MEDIATEK_3329_BINARY: // MediaTek 3329 in binary mode
gpsPrint("$PGCMD,16,0,0,0,0,0*6A\r\n"); // Set Binary Output
gpsPrint("$PMTK220,200*2C\r\n"); // Set 5 Hz Output
gpsPrint("$PMTK313,1*2E\r\n"); // Set SBAS On - Not sure if this does anything on MTK16 software
gpsPrint("$PMTK301,2*2E\r\n"); // Set WAAS On - Not sure if this does anything on MTK16 software
gpsPrint("$PMTK397,0*23\r\n"); // Set Nav Speed Threshold to 0
mtk19ProcessDataState = MTK19_WAIT_SYNC1;
break;
///////////////////////////////
case MEDIATEK_3329_NMEA: // MediaTek 3329 in NMEA mode
gpsPrint("$PMTK314,0,1,0,1,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0*29\r\n"); // GPRMC, GPGGA and GPGSA
gpsPrint("$PMTK220,200*2C\r\n"); // Set 5 Hz Update Rate
gpsPrint("$PMTK313,1*2E\r\n"); // Set SBAS On
gpsPrint("$PMTK301,2*2E\r\n"); // Set WAAS On
gpsPrint("$PMTK397,0*23\r\n"); // Set Nav Speed Threshold to 0
nmeaProcessDataState = WAIT_START;
break;
///////////////////////////////
case UBLOX: // UBLOX in binary mode
for (i = 0; i < sizeof(ubx5Hz); i++) // Set 5 Hz Update Rate
gpsWrite(ubx5Hz[i]);
gpsPrint("$PUBX,41,1,0003,0001,38400,0*26\r\n"); // Set Binary Output
ubloxProcessDataState = WAIT_SYNC1;
break;
///////////////////////////////
}
sensors.gpsLatitude = GPS_INVALID_ANGLE;
sensors.gpsLongitude = GPS_INVALID_ANGLE;
sensors.gpsAltitude = GPS_INVALID_ALTITUDE;
sensors.gpsGroundSpeed = GPS_INVALID_SPEED;
sensors.gpsGroundTrack = GPS_INVALID_ANGLE;
sensors.gpsNumSats = GPS_INVALID_SATS;
sensors.gpsFix = GPS_INVALID_FIX;
sensors.gpsDate = GPS_INVALID_DATE;
sensors.gpsTime = GPS_INVALID_TIME;
sensors.gpsHdop = GPS_INVALID_HDOP;
gpsClearBuffer();
}
