main.cpp

00001 /*
00002  * Author: Edoardo De Marchi
00003  * Editor : Say Pulung
00004  * Country : Indonesia (Lampung)
00005  * Date: 26-04-15
00006  * Notes: Firmware for GPS U-Blox NEO-6M
00007 */
00008 
00009 #include "main.h"
00010 #include "string.h"
00011 #include "rtos.h"
00012 #include "mbed.h"
00013 #include <stdint.h>
00014 #include <math.h>
00015 #include <ctype.h>
00016 uint8_t hour, minute, seconds, year, month, day;
00017 uint16_t milliseconds;
00018 float latitude, longitude, geoidheight, altitude;
00019 float speed, angle, magvariation, HDOP;
00020 char lat, lon, mag;
00021 bool fix;
00022 float trunc(float v) {
00023     if(v < 0.0) {
00024         v*= -1.0;
00025         v = floor(v);
00026         v*=-1.0;
00027     } else {
00028         v = floor(v);
00029     }
00030     return v;
00031 }
00032 void parseRMC(char* buffer)
00033 {
00034     char *p = buffer;
00035 
00036     // get time
00037     p = strchr(p, ',')+1;
00038     float timef = atof(p);
00039     uint32_t time = timef;
00040     hour = time / 10000;
00041     minute = (time % 10000) / 100;
00042     seconds = (time % 100);
00043 
00044     milliseconds = fmod((double) timef, 1.0) * 1000;
00045 
00046     p = strchr(p, ',')+1;
00047     // Serial.println(p);
00048     if (p[0] == 'A') 
00049       fix = true;
00050     else if (p[0] == 'V')
00051       fix = false;
00052     else
00053       fix= false;
00054 
00055     // parse out latitude
00056     p = strchr(p, ',')+1;
00057     latitude = atof(p);
00058 
00059     p = strchr(p, ',')+1;
00060     if (p[0] == 'N') lat = 'N';
00061     else if (p[0] == 'S') lat = 'S';
00062     else if (p[0] == ',') lat = 0;
00063     else lat=0;
00064 
00065     // parse out longitude
00066     p = strchr(p, ',')+1;
00067     longitude = atof(p);
00068 
00069     p = strchr(p, ',')+1;
00070     if (p[0] == 'W') lon = 'W';
00071     else if (p[0] == 'E') lon = 'E';
00072     else if (p[0] == ',') lon = 0;
00073     else lon=0;
00074     if(lat == 'S') {    latitude  *= -1; }
00075     if(lon == 'W') {    longitude *= -1; }
00076     float degrees = trunc(latitude / 100.0f);
00077     float minutes = latitude - (degrees * 100.0f);
00078     latitude = degrees + minutes / 60.0f;    
00079     
00080     degrees = trunc(longitude / 100.0f );
00081     minutes = longitude - (degrees * 100.0f);
00082     longitude = degrees + minutes / 60.0f;
00083     // speed
00084     p = strchr(p, ',')+1;
00085     speed = atof(p);
00086 
00087     // angle
00088     p = strchr(p, ',')+1;
00089     angle = atof(p);
00090 
00091     p = strchr(p, ',')+1;
00092     uint32_t fulldate = atof(p);
00093     day = fulldate / 10000;
00094     month = (fulldate % 10000) / 100;
00095     year = (fulldate % 100);
00096 
00097 }
00098 void Init()
00099 {
00100     gps.baud(9600);
00101     pc.baud(9600);
00102 
00103     pc.printf("Init OK\n");
00104 }
00105 
00106 void readGPSThread(void const *args)
00107 {
00108     char c;
00109     while(true) 
00110     {
00111         if(gps.readable())
00112         { 
00113             if(gps.getc() == '$');           // wait a $
00114             {
00115                 for(int i=0; i<sizeof(cDataBuffer); i++)
00116                 {
00117                     c = gps.getc();
00118                     if( c == '\r' )
00119                     {
00120                         
00121                         
00122                         if(strncmp(cDataBuffer,"$GPRMC",6)==0)
00123                         {
00124                             parseRMC(cDataBuffer);
00125                         }
00126                         //pc.printf("Longitude = %f, Latitude = %f, Speed = %f,Angle = %f, Day = %d, Month = %d, Year = %d,H = %d, M = %d, S = %d\n",longitude,latitude,speed,angle,day,month,year,hour,minute,seconds);
00127                         i = sizeof(cDataBuffer);
00128                     }
00129                     else
00130                     {
00131                         cDataBuffer[i] = c;
00132                     }                 
00133                 }
00134             }
00135             
00136          } 
00137     }
00138 }
00139 
00140 int main() 
00141 {   
00142     Init();
00143     DigitalOut l1(LED1);
00144     Thread gpsReader(readGPSThread);
00145     
00146     while(1){
00147         l1 = 1;
00148         wait(0.5);
00149         l1=0;
00150         wait(0.5);
00151         pc.printf("Longitude = %f, Latitude = %f, Speed = %f,Angle = %f, Day = %d, Month = %d, Year = %d,H = %d, M = %d, S = %d\n",longitude,latitude,speed,angle,day,month,year,hour,minute,seconds);
00152     }
00153     
00154 }
00155 
00156 
00157 
00158 
00159