//////////////////////////////////////////////////////////////////////////////////////////////////////// // // Sample Program to Extract GPS Data from a Bluetooth Enabled GPS Receiver // // Some BT receivers have bluetooth capability. This sample program illustrates reading the data from the // GPS receiver (as ASCII characters)) and parsing the data into latitude and longitude. // // A separate task is used to read and parse the GPS data. // // The GPS receiver typically sends the data once per second. There are a couple of standardized // formats for the data. Some slight modifications may be required for other formats. // // The program has a "TestMode" where it simulates reading from Bluetooth. Instead of BT, it gets the // serial stream from a constant array. Measurements in test mode are that it takes about 10 milliseconds // to parse a GPS character string. // //////////////////////////////////////////////////////////////////////////////////////////////////////// #pragma platform(NXT) // // Conditional compile flag -- actual GPS or simulated mode // #define TestMode //#undef TestMode const string kGPSName = "Dick2"; // // Variables to store the GPS data // float gpsTime; float gpsLat; float gpsLon; float gpsSats; long nCycles = 0; void DisplayGPSData() { nxtDisplayTextLine(1,"Time %10.3f", gpsTime); nxtDisplayTextLine(2,"Lat %10.3f", gpsLat); nxtDisplayTextLine(3,"Long %10.3f", gpsLon); nxtDisplayTextLine(4,"Sat %10.0f", gpsSats); nxtDisplayTextLine(6,"Cycles%9d", ++nCycles); nxtDisplayTextLine(7,"#/Second%7.1f", nCycles * 1000 / (float) nPgmTime); return; } //////////////////////////////////////////////////////////////////////////////////////////////////////// // // getChar() // // Try for up to 200 milliseconds to get a character. Returns '-1' if no character available. // //////////////////////////////////////////////////////////////////////////////////////////////////////// #if !defined(TestMode) int getChar() { ubyte BytesRead[1]; // Circular buffer of last bytes read. int nNumbBytesRead; int nAttempts; for (nAttempts = 0; nAttempts < 20; ++nAttempts) { // Get a single character from BT nNumbBytesRead = nxtReadRawBluetooth(BytesRead[0], 1); if (nNumbBytesRead == 1) return BytesRead[0]; wait1Msec(10); } // No character is available in the last 200 milliseconds return -1; // No character } #else // // "Test Mode" -- simulate input over Bluetooth. This enables testing of parsing routines. // static int nCurrTestCharIndex = 0; int getChar() { static const char cTest[] = { '$', 'G', 'P', 'G', 'G', 'A', ',', // Header '1', '4', '0', '8', '1', '8', '.', '0', '0', '0', ',', // Time '4', '4', '3', '3', '.', '2', '9', '8', '4', ',', // Latitude 'N', ',', // Direction '0', '8', '0', '5', '6', '.', '3', '9', '6', '4', ',', // Longitude 'W', ',', // Direction '6', ',', // Fix '0', // Satellite '0', ',', '5', '0', '.', '0', ',', '1', '6', '8', '.', '7', ',', 'M', ',', '-', '3', '6', '.', '0', ',', 'M', ',', ',', '0', '0', '0', '0', '*', '5', 'C'}; if (nCurrTestCharIndex >= sizeof(cTest)) { nCurrTestCharIndex = 0; return -1; } int nResult; nResult = cTest[nCurrTestCharIndex++]; nResult &= 0x00FF; return nResult; } #endif //////////////////////////////////////////////////////////////////////////////////////////////////////// // // parseNumber() // // Parses a floating point number // //////////////////////////////////////////////////////////////////////////////////////////////////////// bool parseNumber(float &fNumber) { int aChar; bool bFoundDecimalPoint = false; bool bFoundDigit = false; float fDivisor = 1.0; #if !defined(TestMode) wait1Msec(2);// To ensure other tasks get timeslices even if this is high priority. #endif fNumber = 0.0; while (true) { aChar = getChar(); switch (aChar) { case '0': case '1': case '2': case '3': case '4': case '5': case '6': case '7': case '8': case '9': fNumber *= 10; fNumber += aChar - '0'; if (bFoundDecimalPoint) fDivisor *= 10; bFoundDigit = true; break; case '.': if (bFoundDecimalPoint) return false; bFoundDecimalPoint = true; break; case ',': //case -1: // No character received default: if (!bFoundDigit) return false; fNumber /= fDivisor; return true; } } } bool parseDirection() { #if !defined(TestMode) wait1Msec(1);// To ensure other tasks get timeslices even if this is high priority. #endif switch (getChar()) { case 'N': case 'S': case 'E': case 'W': break; default: return false; } if (getChar() != ',') return false; return true; } //////////////////////////////////////////////////////////////////////////////////////////////////////// // // taskWatchGPS // // Infinite loop reading data from GPS receiver // //////////////////////////////////////////////////////////////////////////////////////////////////////// task taskWatchGPS() { // process gps string // Sample string: // $GPGGA,140818.000,4433.2984,N,08056.3964,W,6,00,50.0,168.7,M,-36.0,M,,0000*5C nSchedulePriority = 200; setBluetoothRawDataMode(); // There was compiler bug. Overwriting cHeader[0] with return code! wait1Msec(50); while (!bBTRawMode) { wait1Msec(5); } while (true) { const int kHeaderSize = 7; static const byte cHeader[kHeaderSize] = {'$', 'G', 'P', 'G', 'G', 'A', ','}; int nIndex = 0; int aChar = -1; // // Scan input characters until Header string ("GPGGA" is found // while (true) { while (aChar < 0) { aChar = getChar(); nIndex = 0; } if (aChar != cHeader[nIndex]) { if (nIndex > 0) { nIndex = 0; continue; } } else ++nIndex; if (nIndex >= kHeaderSize) break; aChar = getChar(); } // // "Shadow" copy of data. Only update when all data is received. // float tempLat; float tempLon; float tempSats; float tempTime; float fixIndicator; if (!parseNumber(tempTime)) continue; if (!parseNumber(tempLat)) continue; if (!parseDirection()) continue; if (!parseNumber(tempLon)) continue; if (!parseDirection()) continue; if (!parseNumber(fixIndicator)) continue; if (!parseNumber(tempSats)) continue; // Transfer data in one block gpsTime = tempTime; gpsLat = tempLat; gpsLon = tempLon; gpsSats = tempSats; DisplayGPSData(); } } //////////////////////////////////////////////////////////////////////////////////////////////////////// // // checkBTLinkConnected // // Utility function to check whether Bluetooth link is set up. It should be manually set up before // launching this program. [You can also set it up within an application program; there's a separate // sample program for this. // //////////////////////////////////////////////////////////////////////////////////////////////////////// #if defined(TestMode) #define checkBTLinkConnected() #else void checkBTLinkConnected() { if (nBTCurrentStreamIndex >= 0) return; // An existing Bluetooth connection is present. // // Not connected. Audible notification and LCD error display // PlaySound(soundLowBuzz); PlaySound(soundLowBuzz); // eraseDisplay(); nxtDisplayCenteredTextLine(3, "GPS not"); nxtDisplayCenteredTextLine(4, "Connected"); wait1Msec(3000); StopAllTasks(); } #endif /////////////////////////////////////////////////////////////////////////////////////////////////////// task main() { // // Make connection to GPS if not already connected // #if !defined(TestMode) if (nBTCurrentStreamIndex < 0) { // Not currently connected. Need to make the connection. setSessionPIN("1234"); bBTSkipPswdPrompt = true; btConnect(2, kGPSName); } #endif while (bBTBusy) {} checkBTLinkConnected(); StartTask(taskWatchGPS); while(true) { wait1Msec(1000); } }