Task1Gps.cpp

00001 #include "Task1Gps.h"
00002 
00003 void GpsSerialTask(void) 
00004 {/**
00005  *\brief TASK 1, wait for the event then get the input char
00006  */
00007  while(1)
00008  {
00009     os.WaitEvent(GPS_SERIAL_IN_EVT);
00010     GpsStringParse();
00011             
00012     Dest.lat= 4151.32496; // ***************debug
00013     Dest.lon= 1229.34; // *************debug
00014 
00015     GpsDist();
00016  }
00017 }
00018 
00019 void GpsSerialIsr(void) 
00020 {/**
00021  *\brief Interrupt handler for serial Rx
00022          set the event for the serial task
00023  */
00024     char c = gps.getc();
00025     msgBuff[bufferSelect & 1][writePointer] = c;
00026     if (writePointer++ == BUFF_SIZE)
00027     {
00028         writePointer = 0;
00029     }
00030     if (writePointer > 200) 
00031     {// GPS input buffer full, start computing coordinates
00032         os.SetEvent(GPS_SERIAL_IN_EVT, GPS_SERIAL_TASK);
00033     }
00034 }
00035 
00036 void trace_h(const char *str, int str_size) 
00037 {/**
00038  *\brief output on console what's received on GPS serial
00039           Callback function for NMEA parser buffer trace
00040  */
00041     if( PcMonitor==5 || PcMonitor>5)
00042     {
00043         for (int i = 0; i < str_size; i++)
00044         {
00045             pc.putc(*str++);
00046         }        
00047     }
00048 }
00049 
00050 void error_h(const char *str, int str_size) 
00051 {/**
00052  *\brief Callback function for NMEA parser error
00053  */
00054     for (int i = 0; i < str_size; i++)
00055     {
00056         pc.putc(*str++);
00057     }    
00058 }
00059 
00060 
00061 double trunc(double v) 
00062 {/**
00063  *\brief Return nearest integer vaule less than input
00064  *
00065  *\parameters double variable to get nearest ingeger
00066  *
00067  *\return double 
00068  */
00069     if(v < 0.0) 
00070     {
00071         v*= -1.0;
00072         v = floor(v);
00073         v*=-1.0;
00074     } else {
00075         v = floor(v);
00076     }
00077     return v;
00078 }
00079 
00080 void GpsStringParse(void)
00081 {/**
00082  *\brief parse the GPS string to extract info
00083  */
00084     gps.attach(NULL);   // Suspend serial interrupt while buffer change
00085     size = writePointer;
00086     writePointer = 0;
00087     bufferSelect++;     // Change buffer
00088     gps.attach( &GpsSerialIsr );  // Resume serial interrupt
00089     nmea_parse(&parser, msgBuff[(bufferSelect-1)&1], size, &info);
00090     Coordinates(); // transform nmea coordinates in decimal degrees
00091 }
00092 
00093 void GpsDist(void)
00094 {/**
00095  *\brief compute the distance and direction (forward and reverse) 
00096     from point A to point B on the ellipsoid
00097  */
00098   
00099     nmea_info2pos(&info, &Pos[0]);  // current position  
00100     nmea_info2pos(&Dest, &Pos[1]);  // destination 
00101 
00102     Path.Dist = nmea_distance_ellipsoid(&Pos[0], &Pos[1], &Path.Azimuth[1], &Path.Azimuth[0]);
00103 
00104     if(Path.Azimuth[0] > NMEA_PI) 
00105     {// reverse direction
00106         Path.Azimuth[0] = Path.Azimuth[0] - NMEA_PI; 
00107     }
00108     else
00109     {
00110         Path.Azimuth[1] = Path.Azimuth[1] + NMEA_PI; 
00111     }
00112 
00113     for(int i=0; i < 2; i++)
00114     {
00115         Path.Azimuth[i]=(nmea_radian2degree(Path.Azimuth[i]));
00116         if(Path.Azimuth[i] > 360)
00117         {
00118             Path.Azimuth[i] = Path.Azimuth[i] - 360;
00119         }
00120         if(Path.Azimuth[i] < 0)
00121         {
00122             Path.Azimuth[i] = Path.Azimuth[i] + 360;
00123         }
00124     }
00125 }
00126 
00127 void Coordinates(void)
00128 {/**
00129  *\brief transform nmea coordinates in decimal degrees
00130  */
00131     degrees = trunc(info.lat / 100.0);
00132     minutes = info.lat - (degrees * 100.0);
00133     latitude = degrees + minutes / 60.0;
00134     degrees = trunc(info.lon / 100.0);
00135     minutes = info.lon - (degrees * 100.0);
00136     longitude = degrees + minutes / 60.0;
00137  }
00138   
00139 void Deg2DegMinSec(double DecDeg, DegMinSec *DecSec)
00140 {/**
00141  *\brief convert decimalDeg to Deg Min decimalSec
00142  */
00143     DecSec->Deg = trunc(DecDeg);
00144     double MinDec = (DecDeg - DecSec->Deg);
00145     DecSec->Min = trunc(MinDec * 60);
00146     DecSec->Sec = (MinDec * 3600) - (DecSec->Min * 60);
00147  }