Prototyping the Adaptable Emergency System on an C027 board.
Dependencies: C027_Support mbed
Fork of c027_prototyping by
gps_locate.cpp@17:726bbc1b73ee, 2014-10-05 (annotated)
- Committer:
- aroulin
- Date:
- Sun Oct 05 12:38:04 2014 +0000
- Revision:
- 17:726bbc1b73ee
- Parent:
- 16:2b2f2a3bde5a
PIN in argument of sms_init;
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
aroulin | 4:f1708f6ec905 | 1 | #include "gps_locate.h" |
aroulin | 4:f1708f6ec905 | 2 | #include "GPS.h" |
aroulin | 4:f1708f6ec905 | 3 | |
aroulin | 17:726bbc1b73ee | 4 | int gps_locate(struct gps_data_t* gps_data, int timeout) |
aroulin | 7:eeef6f9fa1db | 5 | { |
aroulin | 16:2b2f2a3bde5a | 6 | printf("GPS Location begins\r\n"); |
aroulin | 16:2b2f2a3bde5a | 7 | |
aroulin | 4:f1708f6ec905 | 8 | // Power on gps |
aroulin | 4:f1708f6ec905 | 9 | GPSI2C gps; |
aroulin | 16:2b2f2a3bde5a | 10 | |
aroulin | 16:2b2f2a3bde5a | 11 | // Timeout timer |
aroulin | 16:2b2f2a3bde5a | 12 | Timer timer; |
aroulin | 16:2b2f2a3bde5a | 13 | timer.start(); |
aroulin | 4:f1708f6ec905 | 14 | |
aroulin | 7:eeef6f9fa1db | 15 | bool coord_ok = false, altitude_ok = false, speed_ok = false; |
aroulin | 7:eeef6f9fa1db | 16 | |
aroulin | 4:f1708f6ec905 | 17 | int ret = 0; |
aroulin | 4:f1708f6ec905 | 18 | char buf[512] = {0}; |
aroulin | 16:2b2f2a3bde5a | 19 | while(!coord_ok || !altitude_ok || !speed_ok) { |
aroulin | 16:2b2f2a3bde5a | 20 | |
aroulin | 17:726bbc1b73ee | 21 | if(timer.read() > timeout) { |
aroulin | 16:2b2f2a3bde5a | 22 | printf("GPS Location TimeOut, abort...\r\n"); |
aroulin | 16:2b2f2a3bde5a | 23 | timer.stop(); |
aroulin | 16:2b2f2a3bde5a | 24 | return 0; |
aroulin | 16:2b2f2a3bde5a | 25 | } |
aroulin | 16:2b2f2a3bde5a | 26 | |
aroulin | 7:eeef6f9fa1db | 27 | while ((ret = gps.getMessage(buf, sizeof(buf))) > 0) { |
aroulin | 7:eeef6f9fa1db | 28 | int len = LENGTH(ret); |
aroulin | 7:eeef6f9fa1db | 29 | if ((PROTOCOL(ret) == GPSParser::NMEA) && (len > 6)) { |
aroulin | 7:eeef6f9fa1db | 30 | if (!strncmp("$GPGLL", buf, 6)) { |
aroulin | 7:eeef6f9fa1db | 31 | double la = 0, lo = 0; |
aroulin | 7:eeef6f9fa1db | 32 | char ch; |
aroulin | 7:eeef6f9fa1db | 33 | if (gps.getNmeaAngle(1,buf,len,la) && |
aroulin | 7:eeef6f9fa1db | 34 | gps.getNmeaAngle(3,buf,len,lo) && |
aroulin | 7:eeef6f9fa1db | 35 | gps.getNmeaItem(6,buf,len,ch) && ch == 'A') { |
aroulin | 7:eeef6f9fa1db | 36 | gps_data->lo = lo; |
aroulin | 7:eeef6f9fa1db | 37 | gps_data->la = la; |
aroulin | 7:eeef6f9fa1db | 38 | coord_ok = true; |
aroulin | 7:eeef6f9fa1db | 39 | } |
aroulin | 7:eeef6f9fa1db | 40 | } else if (!strncmp("$GPGGA", buf, 6)) { |
aroulin | 7:eeef6f9fa1db | 41 | double a = 0; |
aroulin | 7:eeef6f9fa1db | 42 | if (gps.getNmeaItem(9,buf,len,a)) // altitude msl [m] |
aroulin | 7:eeef6f9fa1db | 43 | gps_data->altitude = a; |
aroulin | 7:eeef6f9fa1db | 44 | altitude_ok = true; |
aroulin | 7:eeef6f9fa1db | 45 | } else if (!strncmp("$GPVTG", buf, 6)) { |
aroulin | 7:eeef6f9fa1db | 46 | double s = 0; |
aroulin | 7:eeef6f9fa1db | 47 | if (gps.getNmeaItem(7,buf,len,s)) // speed [km/h] |
aroulin | 7:eeef6f9fa1db | 48 | gps_data->speed = s; |
aroulin | 7:eeef6f9fa1db | 49 | speed_ok = true; |
aroulin | 7:eeef6f9fa1db | 50 | } |
aroulin | 4:f1708f6ec905 | 51 | |
aroulin | 4:f1708f6ec905 | 52 | } |
aroulin | 4:f1708f6ec905 | 53 | } |
aroulin | 7:eeef6f9fa1db | 54 | } |
aroulin | 16:2b2f2a3bde5a | 55 | |
aroulin | 16:2b2f2a3bde5a | 56 | timer.stop(); |
aroulin | 16:2b2f2a3bde5a | 57 | return 1; |
aroulin | 4:f1708f6ec905 | 58 | } |