/******************************************************************************* * MASTER CONNECTION for Bluetooth GPS Receiver -GPS Data in NMEA Format- * ******************************************************************************* * * * * * Workarround to handle bluetooth GPS receiver Data stream * * with NXC language from John Hansen version 1.0.1.32beta * * and NXT-Lego Firmware 1.04. Bluetooth GPS-Receiver in this * * example is a Navilock BT-348 with a NMEA GPS-Data output * * format, but other models of such kind should also work. * * Both devices (NXT and GPS receiver) work with 4600 baud. * * Normally, this workarround should not be possible, because * * of some NXC-Language or NXT-Firmware lacks, but so what, * * I have beaten the devil! * * * * Author: Antonio Scarfone * * Email: a.scarfone@web.de * * Date: 10.09.2007 * * * * * *******************************************************************************/ #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 "GPSData.txt" //Name of the file to write the GPS data #define WAITCYCLEFAST 100 //Wait Cycle, because of 4600 baud , is the GPS-receiver connected to a satelite yes or no? #define WAITCYCLESLOW 1500 //If no wait longer, because the data rate is not so high /*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 string out,tmpstr; //out: output for the sentences, all in one string . tmpstr: debug string // temp. buffer 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; int dataflow = 0; //Speed of the data flow, depends if the GPS-Receiver is connected to satelite or not //If yes there is higher data rate per time unit. //Sub routine to check Bluetooth connection, exits the program if no connection sub BTCheck(int conn){ if (!BluetoothStatus(conn) == NO_ERR){ TextOut(5,LCD_LINE2,"Error"); Wait(1000); Stop(true); } } // 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); } } } 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; } } } astr=SubStr(ret.sentence1, 1, 9); TextOut(0,LCD_LINE1,astr); num++; return num; } task main(){ int numsent = 0;// Number of sentences received in one read cycle //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 = ""; //make sure one receive sequence is not pending or not in error until(BluetoothStatus(BT_CONN) == NO_ERR); //Start a receive sequence to fill up the input buffer with GPS data , before reading the input buffer ReceiveRemoteString(INBOX, 1, startreadbuf); //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); //make sure one receive sequence is not pending or not in error until(BluetoothStatus(BT_CONN) == NO_ERR); //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. 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 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); //debug output tmpstr=SubStr(out, 5, 13); TextOut(0,LCD_LINE4,tmpstr); NumOut(0,LCD_LINE5,numsent); //write the sentences in a file WriteData(FILENAME,out); numsent = 0; //wait between 1500ms, in order to respect the baudrate of 4600 baud //if data rate is slow, that means no connection to the satelite //wait only 100ms, if the data rate is high, connection to the satelite is established. //the nxt firmware works with 4600 baud //my bluetooth GPS-receiver too. //the model is a Navilock BT-348 if (numsent == 0 && dataflow == 0) //no data available { dataflow = 0; Wait(WAITCYCLEFAST); } if ((numsent == 0 || numsent == 1) && dataflow > 60) //data rate is high, less sentences per read cycle in the in buffer { dataflow = 0; Wait(WAITCYCLEFAST); } if ((numsent == 2 || numsent == 3) && dataflow > 100) //data rate is slow, more sentences per read cycle in the in buffer { dataflow = 0; Wait(WAITCYCLESLOW); } } }