/******************************************************************************* * Library for bluetooth GPS Receiver * ******************************************************************************* * This is a library to handle Bluetooth communication with Gps receivers, * * Holux M-1000, M-1200 and Navilock BT-348 have been tested, but other * * models with NMEA GPS-Data output 4600 baud capable should also work. * * This library is able to parse GGA and RMC string, and retreive * * informations such as Longitude, Latitude and Date or UTC Time. * * * * Provided "as is" by Benco, french AFOL, contact via the nxtasy.org * * forum. You can find some articles around NXT and electronic, in french * * and english at http://freelug.org/auteur.php3?id_auteur=135 * * * * -Original first working NXC source posted by Antonio Scarfone * * -Original first GPGGA string analyser (based on Antonio code) given * * by Robokalle * * * *******************************************************************************/ #include "NXCDefs.h" #define BT_CONN 1 //Bluetooth connection port #define INBOX 1 //Mailbox or QUEUE number #define INBUFFER 128 //The input buffer, where the GPS data are written #define INBUFFERSMALL 58 //Workarround buffer of 58 bytes, because nxc is only able to read max 58 bytes #define INBUFFERSMALL2 12 //Workarround buffer 2 x 58 = 116, remains 12 bytes to 128 bytes #define STARTOFFSET 0 //Offset start #define FILENAME "NMEA2.txt" //Name of the file to write the GPS data /*Struct, where to hold the gps data stream sentences for one read cycle, one read cycle brings up to 2 or 3 sentences. For security reason, there is a fourth sentence struct variable implemented*/ struct sentence_struct { string sentence1; string sentence2; string sentence3; string sentence4; }; sentence_struct gpsSentences; //Global struct variable for the GPS sentences // temp. strings string out; // out: output for the sentences, "all in one string" // temp. buffers for the workarround input buffer to read splitted in 58,58,12 bytes buffers byte tmpbuffer0[]; byte tmpbuffer1[]; byte tmpbuffer2[]; //Buffer to clear or init the real 128 byte input buffer byte clearbuffer[]; //Buffer for init the request of the GPS Data byte startreadbuf[]; //Create or append file for output data only for debug phase bool filewritten; //Speed of the data flow, depends if the GPS-Receiver is connected to satelite or not int dataflow = 0; struct GPS_struct { int UTC_Hour; int UTC_Min; int UTC_Sec; int UTC_mSec; int Date_Day; int Date_Month; int Date_Year; unsigned int Latitude_high; int Latitude_low; string Latitude_direction; unsigned int Longitude_high; int Longitude_low; string Longitude_direction; int Ground_speed; byte Quality; string Receiver_Status; byte Sat_number; byte accurateness_high; byte accurateness_low; int altitude_high; int altitude_low; int Magnetic_deg; string Magnetic_dir; string altitude_measure; }; GPS_struct GPSData; //Global struct variable for the GPS data void Display_GPSData() { ClearScreen(); TextOut(0,LCD_LINE1,"Date:"); NumOut(30,LCD_LINE1,GPSData.Date_Day); TextOut(45,LCD_LINE1,"/"); NumOut(50,LCD_LINE1,GPSData.Date_Month); TextOut(65,LCD_LINE1,"/"); NumOut(70,LCD_LINE1,GPSData.Date_Year); TextOut(0,LCD_LINE2,"Time:"); NumOut(30,LCD_LINE2,GPSData.UTC_Hour); TextOut(45,LCD_LINE2,":"); NumOut(50,LCD_LINE2,GPSData.UTC_Min); TextOut(65,LCD_LINE2,":"); NumOut(70,LCD_LINE2,GPSData.UTC_Sec); TextOut(85,LCD_LINE2,":"); NumOut(90,LCD_LINE2,GPSData.UTC_mSec); TextOut(0,LCD_LINE3,"Lat :"); NumOut(30,LCD_LINE3,GPSData.Latitude_high); TextOut(56,LCD_LINE3,"."); NumOut(60,LCD_LINE3,GPSData.Latitude_low); TextOut(90,LCD_LINE3,GPSData.Latitude_direction); TextOut(0,LCD_LINE4,"Long:"); NumOut(30,LCD_LINE4,GPSData.Longitude_high); TextOut(56,LCD_LINE4,"."); NumOut(60,LCD_LINE4,GPSData.Longitude_low); TextOut(90,LCD_LINE4,GPSData.Longitude_direction); TextOut(0,LCD_LINE5,"Alt :"); NumOut(30,LCD_LINE5,GPSData.altitude_high); TextOut(56,LCD_LINE5,"."); NumOut(60,LCD_LINE5,GPSData.altitude_low); TextOut(70,LCD_LINE5,GPSData.altitude_measure); TextOut(0,LCD_LINE6,"Speed:"); NumOut(40,LCD_LINE6,GPSData.Ground_speed); TextOut(0,LCD_LINE7,"Mag:"); NumOut(30,LCD_LINE7,GPSData.Magnetic_deg); NumOut(60,LCD_LINE7,GPSData.Magnetic_dir); } /* GGA Global Positioning System Fix Data. Time, Position and fix related data for a GPS receiver $--GGA,hhmmss.ss,llll.ll,a,yyyyy.yy,a,x,xx,x.x,x.x,M,x.x,M,x.x,xxxx*hh 1) Time (UTC) 2) Latitude 3) N or S (North or South) 4) Longitude 5) E or W (East or West) 6) GPS Quality Indicator, [0 - fix not available, 1 - GPS fix, 2 - Differential GPS fix] 7) Number of satellites in view, 00 - 12 8) Horizontal Dilution of precision 9) Antenna Altitude above/below mean-sea-level (geoid) 10) Units of antenna altitude, meters 11) Geoidal separation, the difference between the WGS-84 earth ellipsoid and mean-sea-level (geoid), "-" means mean-sea-level below ellipsoid 12) Units of geoidal separation, meters 13) Age of differential GPS data, time in seconds since last SC104 type 1 or 9 update, null field when DGPS is not used 14) Differential reference station ID, 0000-1023 15) Checksum */ void GGAParsing(int item,string sdumy){ int len,j; string dumy, dumy1; len= StrLen(sdumy); switch(item){ case 1: for (j=0; j < len ;j++){ if (StrIndex(sdumy,j)== 46){ // "." searching dumy1= SubStr( sdumy,0,2 ) ; GPSData.UTC_Hour = StrToNum(dumy1); dumy1= SubStr( sdumy,2,2 ) ; GPSData.UTC_Min = StrToNum(dumy1); dumy1= SubStr( sdumy,4,2 ) ; GPSData.UTC_Sec = StrToNum(dumy1); dumy1=SubStr( sdumy,j+1, len - j); GPSData.UTC_mSec =StrToNum(dumy1); } } break; case 2: for (j=0; j < len ;j++){ if (StrIndex(sdumy,j)== 46){ dumy1= SubStr( sdumy,0,j ) ; GPSData.Latitude_high = StrToNum(dumy1); dumy1=SubStr( sdumy,j+1, len - j); GPSData.Latitude_low =StrToNum(dumy1); } } break; case 3: GPSData.Latitude_direction=sdumy; break; case 4: for (j=0; j < len ;j++){ if (StrIndex(sdumy,j)== 46){ dumy1= SubStr( sdumy,0,j ) ; GPSData.Longitude_high = StrToNum(dumy1); dumy1=SubStr( sdumy,j+1, len - j); GPSData.Longitude_low =StrToNum(dumy1); } } break; case 5: GPSData.Longitude_direction=sdumy; break; case 6: GPSData.Quality=StrToNum(sdumy); break; case 7: GPSData.Sat_number=StrToNum(sdumy); break; case 8: for (j=0; j < len ;j++){ if (StrIndex(sdumy,j)== 46){ dumy1= SubStr( sdumy,0,j ) ; GPSData.accurateness_high = StrToNum(dumy1); dumy1=SubStr( sdumy,j+1, len - j); GPSData.accurateness_low =StrToNum(dumy1); } } break; case 9: for (j=0; j < len ;j++){ if (StrIndex(sdumy,j)== 46){ dumy1= SubStr( sdumy,0,j ) ; GPSData.altitude_high = StrToNum(dumy1); dumy1=SubStr( sdumy,j+1, len - j); GPSData.altitude_low =StrToNum(dumy1); } } break; case 10: GPSData.altitude_measure=sdumy; break; } } /*RMC Recommended Minimum Navigation Information $--RMC,hhmmss.ss,A,llll.ll,a,yyyyy.yy,a,x.x,x.x,xxxx,x.x,a*hh 1) Time (UTC) 2) Status, V = Navigation receiver warning 3) Latitude 4) N or S 5) Longitude 6) E or W 7) Speed over ground, knots 8) Track made good, degrees true 9) Date, ddmmyy 10) Magnetic Variation, degrees 11) E or W 12) Checksum */ void RMCParsing(int item,string sdumy){ int len,j; string dumy, dumy1; len= StrLen(sdumy); switch(item){ case 1: for (j=0; j < len ;j++){ if (StrIndex(sdumy,j)== 46){ // "." searching dumy1= SubStr( sdumy,0,2 ) ; GPSData.UTC_Hour = StrToNum(dumy1); dumy1= SubStr( sdumy,2,2 ) ; GPSData.UTC_Min = StrToNum(dumy1); dumy1= SubStr( sdumy,4,2 ) ; GPSData.UTC_Sec = StrToNum(dumy1); dumy1=SubStr( sdumy,j+1, len - j); GPSData.UTC_mSec =StrToNum(dumy1); } } break; case 2: GPSData.Receiver_Status=sdumy; break; case 3: for (j=0; j < len ;j++){ if (StrIndex(sdumy,j)== 46){ dumy1= SubStr( sdumy,0,j ) ; GPSData.Latitude_high = StrToNum(dumy1); dumy1=SubStr( sdumy,j+1, len - j); GPSData.Latitude_low =StrToNum(dumy1); } } break; case 4: GPSData.Latitude_direction=sdumy; break; case 5: for (j=0; j < len ;j++){ if (StrIndex(sdumy,j)== 46){ dumy1= SubStr( sdumy,0,j ) ; GPSData.Longitude_high = StrToNum(dumy1); dumy1=SubStr( sdumy,j+1, len - j); GPSData.Longitude_low =StrToNum(dumy1); } } break; case 6: GPSData.Longitude_direction=sdumy; break; case 7: GPSData.Ground_speed=StrToNum(sdumy); break; case 7: GPSData.Sat_number=StrToNum(sdumy); break; case 9: dumy1= SubStr( sdumy,0,2 ) ; GPSData.Date_Day = StrToNum(dumy1); dumy1= SubStr( sdumy,2,2 ) ; GPSData.Date_Month = StrToNum(dumy1); dumy1= SubStr( sdumy,4,2 ) ; GPSData.Date_Year = StrToNum(dumy1); break; case 10: GPSData.Magnetic_deg=StrToNum(sdumy); break; case 11: GPSData.Magnetic_dir=sdumy; break; } } // Writes the sentences stored in out to a file, only for debug void WriteData (string afile, string Data) { byte handle; int slen; int fsize; if (!filewritten) { if (CreateFile(afile, 10048, handle) == NO_ERR ) { slen = StrLen(Data); WriteBytes(handle, Data, slen); CloseFile(handle); filewritten = true; } } else { if (OpenFileAppend(afile, fsize, handle) == NO_ERR ) { slen = StrLen(Data); WriteBytes(handle, Data, slen); CloseFile(handle); } } } // strip the buffer string into pieces, searching ",", and send them // to the good parser, depending the NMEA header void strip_gps_data(string GPS_data){ int lendata, i, begin, last, item ; string sdumy; bool GGA = false; bool RMC = false; i=0; lendata= StrLen(GPS_data); begin = -1; item=0; while (StrIndex(GPS_data,i) ==36 && i < lendata){i++; begin =i;} if (begin >-1){ for (i=begin; i < lendata ;i++) { if (StrIndex(GPS_data,i)== 44){ last=i; sdumy=SubStr(GPS_data,begin,last-begin); if (sdumy == "GPGGA") {GGA = true;} if (sdumy == "GPRMC") {RMC = true;} if (GGA){ GGAParsing(item,sdumy); begin=i+1; item++; } else { if (RMC){ RMCParsing(item,sdumy); begin=i+1; item++; } else { i=lendata;} } } } } } //Sub routine to check Bluetooth connection, exits the program if no connection sub BTCheck(int conn){ byte index, trials; byte event; if (!BluetoothStatus(conn) == NO_ERR){ TextOut(5,LCD_LINE2,"Error, Verify"); TextOut(5,LCD_LINE3,"Bluetooth Port"); Wait(1000); Stop(true); } } //Submit the three temp. input buffers, in order to extract the sentences. //Each sentence starts with a '$' char ascii=36 and ends with a '*' char ascii=42 to follow a checksum and a CR+LF. int GetSentences (sentence_struct &ret,byte buffer1[],byte buffer2[],byte buffer3[],byte asciibegin, byte asciiend) { byte sarray[],rarray[]; string astr = ""; int beg = -1; int ends = -1; int ii = 0; int xx = 0; int num = -1; byte alen = 0; byte lensplit = 0; bool startsplit = true; ArrayInit(sarray, 0, INBUFFER); ArrayInit(rarray, 0, INBUFFER); ArrayBuild(sarray, buffer1, buffer2, buffer3); alen = ArrayLen(sarray); for (ii=0;ii -1 && ends == -1){ ends = ii; num++; lensplit = (ends - beg); dataflow+=lensplit; ArraySubset(rarray, sarray, beg, lensplit); ByteArrayToStrEx(rarray, astr); if (xx == 0) ret.sentence1 = astr; if (xx == 1) ret.sentence2 = astr; if (xx == 2) ret.sentence3 = astr; if (xx == 3) ret.sentence4 = astr; ArrayInit(rarray, 0, INBUFFER); astr = ""; startsplit = true; ends = -1; beg = -1; xx++; ii = ii - 1; } } } num++; return num; } task main(){ int numsent = 0;// Number of sentences received in one read cycle int n = 0; byte inbufPtr = 0; IOMapReadType InBufferfull; //Struct to check if input is full and ready for the next read cycle InBufferfull.ModuleName = CommModuleName; InBufferfull.Offset = CommOffsetBtInBufInPtr; InBufferfull.Count = 1; InBufferfull.Buffer[0]=0; CommExecuteFunctionType cefArgs; //init the following buffers ArrayInit(tmpbuffer0, 0, INBUFFERSMALL); ArrayInit(tmpbuffer1, 0, INBUFFERSMALL); ArrayInit(tmpbuffer2, 0, INBUFFERSMALL2); ArrayInit(clearbuffer, 0, INBUFFER); ArrayInit(startreadbuf, 0, INBUFFERSMALL); BTCheck(BT_CONN); //check slave connection //Check if file is already written filewritten = false; ReceiveRemoteString(INBOX, 1, startreadbuf); //Start a receive sequence to fill up the input buffer with GPS data while(true){ //init the struct to hold the sentences gpsSentences.sentence1 = ""; gpsSentences.sentence2 = ""; gpsSentences.sentence3 = ""; gpsSentences.sentence4 = ""; //init the output string, which hold up the gpsSentences struct out = ""; //Start a receive sequence to fill up the input buffer with GPS data , before reading the input buffer ReceiveRemoteString(INBOX, 1, startreadbuf); //make sure one receive sequence is not pending or not in error SysIOMapRead(InBufferfull); while(InBufferfull.Buffer[0]==0) { SysIOMapRead(InBufferfull); } SetBluetoothState(NO_ERR); InBufferfull.Buffer[0]=0; //Get the 128 byte large input buffer in three temp. buffer splitted up in 58,58 and 12 bytes long bytes arrays, //in order to workarround the ability of nxc to handle only 58 bytes large buffers. GetBTInputBuffer(STARTOFFSET, INBUFFERSMALL, tmpbuffer0); GetBTInputBuffer(INBUFFERSMALL, INBUFFERSMALL, tmpbuffer1); GetBTInputBuffer(2*INBUFFERSMALL, INBUFFERSMALL2, tmpbuffer2); numsent = GetSentences(gpsSentences, tmpbuffer0, tmpbuffer1,tmpbuffer2, 36, 42); //Output the extracted sentences to out string if (gpsSentences.sentence1 != "") out=StrCat(out,gpsSentences.sentence1); if (gpsSentences.sentence2 != "") out=StrCat(out,gpsSentences.sentence2); if (gpsSentences.sentence3 != "") out=StrCat(out,gpsSentences.sentence3); if (gpsSentences.sentence4 != "") out=StrCat(out,gpsSentences.sentence4); //Clear up the input buffer and set the input buffer pointer at the beginning of the buffer inbufPtr = BTInputBufferInPtr(); if (inbufPtr == INBUFFER){ SetBTInputBuffer(STARTOFFSET, INBUFFER, clearbuffer); SetBTInputBufferInPtr(0); } //clear up the temp. buffers ArrayInit(tmpbuffer0, 0, INBUFFERSMALL); ArrayInit(tmpbuffer1, 0, INBUFFERSMALL); ArrayInit(tmpbuffer2, 0, INBUFFERSMALL2); ArrayInit(clearbuffer, 0, INBUFFER); n+=numsent; strip_gps_data(out); //WriteData(FILENAME,out); //write the sentences in a file (for debug) Display_GPSData(); numsent = 0; } }