Mindboards

Code: Select all

/*******************************************************************************
 *      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"
#include "CommExecuteFunctionType cefArgs"
#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 8: 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<alen;ii++) {
      if (sarray[ii] == asciibegin) {
         if (startsplit == true && beg == -1 && ends == -1){
            beg = ii;
            startsplit=false;
            continue;
         }
         if (startsplit == false && beg > -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;
   }
}

Code: Select all

/*
Program  ID: NXTcommM01.nxc
Create Date: 2009-5-20
NXT BT LCP testing/Master side
*/

#define BT_CHANNEL    1
#define SLAVE_DEVICE  "CH_NXT_B"
#define SLAVE_PROGRAM "NRLink-8183CPM3.rxe"

/* ********************************************************
 * Search slave device index on My contacts list, then
 * establish connection
 * ******************************************************** */
bool Build_Connection(byte BtChannel, const string BtDevname)
{
     string msg;
     bool isOK = FALSE;
     byte idx  = 0;

     ClearScreen();
     TextOut(0, LCD_LINE1,StrCat("Search " ,BtDevname,"        " ));
     TextOut(0, LCD_LINE2,"in My Contact   " );
     for (byte xii=0; xii<BTDeviceCount(); xii++)
     {
         msg = BTDeviceName(xii);
         TextOut(0, LCD_LINE3, StrCat("Table: " ,msg,"         " ));
         if (BTDeviceName(xii) == BtDevname)
         {
            idx  = xii;
            isOK = TRUE;
            break;
         }
     }

     if (!isOK)
     {
        TextOut(0, LCD_LINE4, "Not in MyContact" );
        TextOut(0, LCD_LINE8, "Enter to Exit   " );
        until (ButtonPressed(BTNCENTER, TRUE));
        return FALSE;
     }

     msg = NumToStr(BtChannel);
     ClearScreen();
     TextOut(0, LCD_LINE1,StrCat("Connect " ,BtDevname));
     TextOut(0, LCD_LINE2,StrCat("on channel " ,msg));

     // Using Comm module IOMAP accessing method to establish connection
     // see NXC Programmer's Guide P39
     CommExecuteFunctionType cefArgs;
     cefArgs.Cmd    = INTF_CONNECT;
     cefArgs.Param1 = idx;
     cefArgs.Param2 = BtChannel;

     isOK  = FALSE;
     for (byte xii=0; xii<20; xii++)
     {
         msg = NumToStr(xii);
         TextOut(0, LCD_LINE3, StrCat("Try " , msg));
         SysCommExecuteFunction(cefArgs);
         Wait(500);

         msg = NumToStr(cefArgs.Result);
         TextOut(0, LCD_LINE4, StrCat("Result: " , msg,"       " ));
         if (cefArgs.Result == LDR_SUCCESS)
         {
            isOK = TRUE;
            break;
         }
     }
     if (!isOK)
     {
        TextOut(0, LCD_LINE4, "Connect failed  " );
        TextOut(0, LCD_LINE5, "Check Slave !   " );
        TextOut(0, LCD_LINE8, "Enter to Exit   " );
        until (ButtonPressed(BTNCENTER, TRUE));
     }
     return isOK;
}
/* ********************************************************
 * Verify wether connection ready, if not establish it
 * ******************************************************** */
bool Check_Connection(byte BtChannel, const string BtDevname)
{
     bool isOK = FALSE;

     ClearScreen();
     TextOut(0, LCD_LINE1,"Check connection" );
     if (BluetoothStatus(BtChannel) == NO_ERR)        isOK = TRUE;
     else if (Build_Connection(BtChannel, BtDevname)) isOK = TRUE;
          else
          {
              ClearScreen();
              TextOut(0, LCD_LINE1,"Cannot connect  " );
              TextOut(0, LCD_LINE2,"    with Slave  " );
              TextOut(0, LCD_LINE8,"Enter to Exit   " );
              until (ButtonPressed(BTNCENTER, TRUE));
          }
     return isOK;
}
/* ********************************************************
 *
 * ******************************************************** */
task main()
{
     if (Check_Connection(BT_CHANNEL, SLAVE_DEVICE))
     {
        ClearScreen();
        TextOut(0, LCD_LINE1,"Connect OK      " );
        TextOut(0, LCD_LINE8,"Enter to Run Rmt" );
        until (ButtonPressed(BTNCENTER, TRUE));

        RemoteStartProgram(BT_CHANNEL, SLAVE_PROGRAM);

        ClearScreen();
        TextOut(0, LCD_LINE1,StrCat("Run ",SLAVE_PROGRAM),"           " );
        TextOut(0, LCD_LINE8,"Enter to Exit   " );
        until (ButtonPressed(BTNCENTER, TRUE));
     }
}

Code: Select all

#define BT_CONN 1           //Bluetooth connection port