Diff: Task1Gps.cpp

Revision:

2:8917036cbf69

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Task1Gps.cpp	Tue Jan 31 00:22:58 2012 +0000
@@ -0,0 +1,147 @@
+#include "Task1Gps.h"
+
+void GpsSerialTask(void) 
+{/**
+ *\brief TASK 1, wait for the event then get the input char
+ */
+ while(1)
+ {
+    os.WaitEvent(GPS_SERIAL_IN_EVT);
+    GpsStringParse();
+            
+    Dest.lat= 4151.32496; // ***************debug
+    Dest.lon= 1229.34; // *************debug
+
+    GpsDist();
+ }
+}
+
+void GpsSerialIsr(void) 
+{/**
+ *\brief Interrupt handler for serial Rx
+         set the event for the serial task
+ */
+    char c = gps.getc();
+    msgBuff[bufferSelect & 1][writePointer] = c;
+    if (writePointer++ == BUFF_SIZE)
+    {
+        writePointer = 0;
+    }
+    if (writePointer > 200) 
+    {// GPS input buffer full, start computing coordinates
+        os.SetEvent(GPS_SERIAL_IN_EVT, GPS_SERIAL_TASK);
+    }
+}
+
+void trace_h(const char *str, int str_size) 
+{/**
+ *\brief output on console what's received on GPS serial
+          Callback function for NMEA parser buffer trace
+ */
+    if( PcMonitor==5 || PcMonitor>5)
+    {
+        for (int i = 0; i < str_size; i++)
+        {
+            pc.putc(*str++);
+        }        
+    }
+}
+
+void error_h(const char *str, int str_size) 
+{/**
+ *\brief Callback function for NMEA parser error
+ */
+    for (int i = 0; i < str_size; i++)
+    {
+        pc.putc(*str++);
+    }    
+}
+
+
+double trunc(double v) 
+{/**
+ *\brief Return nearest integer vaule less than input
+ *
+ *\parameters double variable to get nearest ingeger
+ *
+ *\return double 
+ */
+    if(v < 0.0) 
+    {
+        v*= -1.0;
+        v = floor(v);
+        v*=-1.0;
+    } else {
+        v = floor(v);
+    }
+    return v;
+}
+
+void GpsStringParse(void)
+{/**
+ *\brief parse the GPS string to extract info
+ */
+    gps.attach(NULL);   // Suspend serial interrupt while buffer change
+    size = writePointer;
+    writePointer = 0;
+    bufferSelect++;     // Change buffer
+    gps.attach( &GpsSerialIsr );  // Resume serial interrupt
+    nmea_parse(&parser, msgBuff[(bufferSelect-1)&1], size, &info);
+    Coordinates(); // transform nmea coordinates in decimal degrees
+}
+
+void GpsDist(void)
+{/**
+ *\brief compute the distance and direction (forward and reverse) 
+    from point A to point B on the ellipsoid
+ */
+  
+    nmea_info2pos(&info, &Pos[0]);  // current position  
+    nmea_info2pos(&Dest, &Pos[1]);  // destination 
+
+    Path.Dist = nmea_distance_ellipsoid(&Pos[0], &Pos[1], &Path.Azimuth[1], &Path.Azimuth[0]);
+
+    if(Path.Azimuth[0] > NMEA_PI) 
+    {// reverse direction
+        Path.Azimuth[0] = Path.Azimuth[0] - NMEA_PI; 
+    }
+    else
+    {
+        Path.Azimuth[1] = Path.Azimuth[1] + NMEA_PI; 
+    }
+
+    for(int i=0; i < 2; i++)
+    {
+        Path.Azimuth[i]=(nmea_radian2degree(Path.Azimuth[i]));
+        if(Path.Azimuth[i] > 360)
+        {
+            Path.Azimuth[i] = Path.Azimuth[i] - 360;
+        }
+        if(Path.Azimuth[i] < 0)
+        {
+            Path.Azimuth[i] = Path.Azimuth[i] + 360;
+        }
+    }
+}
+
+void Coordinates(void)
+{/**
+ *\brief transform nmea coordinates in decimal degrees
+ */
+    degrees = trunc(info.lat / 100.0);
+    minutes = info.lat - (degrees * 100.0);
+    latitude = degrees + minutes / 60.0;
+    degrees = trunc(info.lon / 100.0);
+    minutes = info.lon - (degrees * 100.0);
+    longitude = degrees + minutes / 60.0;
+ }
+  
+void Deg2DegMinSec(double DecDeg, DegMinSec *DecSec)
+{/**
+ *\brief convert decimalDeg to Deg Min decimalSec
+ */
+    DecSec->Deg = trunc(DecDeg);
+    double MinDec = (DecDeg - DecSec->Deg);
+    DecSec->Min = trunc(MinDec * 60);
+    DecSec->Sec = (MinDec * 3600) - (DecSec->Min * 60);
+ }
\ No newline at end of file