Guido Ottaviani
A porting of a GPS decoding and presenting program within the mbos RTOS. It is not a definitive application but a study program to test NMEA full decoding library and a first approach to an RTOS. Many thanks to Andrew Levido for his support and his patience on teaching me the RTOS principles from the other side of the Earth. It uses NMEA library by Tim (xtimor@gmail.com) ported by Ken Todotani (http://mbed.org/users/todotani/) on public mbed library (http://mbed.org/users/todotani/programs/GPS_nmeaLib/5yo4h) also available, as original universal C library, on http://nmea.sourceforge.net
Dependencies: mbos Watchdog TextLCD mbed ConfigFile
Task1Gps.cpp@3:a2f9eb3b8a16, 2012-02-03 (annotated)
- Committer:
- guiott
- Date:
- Fri Feb 03 16:29:52 2012 +0000
- Revision:
- 3:a2f9eb3b8a16
- Parent:
- 2:8917036cbf69
Who changed what in which revision?
| User | Revision | Line number | New contents of line |
|---|---|---|---|
| guiott | 2:8917036cbf69 | 1 | #include "Task1Gps.h" |
| guiott | 2:8917036cbf69 | 2 | |
| guiott | 2:8917036cbf69 | 3 | void GpsSerialTask(void) |
| guiott | 2:8917036cbf69 | 4 | {/** |
| guiott | 2:8917036cbf69 | 5 | *\brief TASK 1, wait for the event then get the input char |
| guiott | 2:8917036cbf69 | 6 | */ |
| guiott | 2:8917036cbf69 | 7 | while(1) |
| guiott | 2:8917036cbf69 | 8 | { |
| guiott | 2:8917036cbf69 | 9 | os.WaitEvent(GPS_SERIAL_IN_EVT); |
| guiott | 2:8917036cbf69 | 10 | GpsStringParse(); |
| guiott | 2:8917036cbf69 | 11 | |
| guiott | 2:8917036cbf69 | 12 | Dest.lat= 4151.32496; // ***************debug |
| guiott | 2:8917036cbf69 | 13 | Dest.lon= 1229.34; // *************debug |
| guiott | 2:8917036cbf69 | 14 | |
| guiott | 2:8917036cbf69 | 15 | GpsDist(); |
| guiott | 2:8917036cbf69 | 16 | } |
| guiott | 2:8917036cbf69 | 17 | } |
| guiott | 2:8917036cbf69 | 18 | |
| guiott | 2:8917036cbf69 | 19 | void GpsSerialIsr(void) |
| guiott | 2:8917036cbf69 | 20 | {/** |
| guiott | 2:8917036cbf69 | 21 | *\brief Interrupt handler for serial Rx |
| guiott | 2:8917036cbf69 | 22 | set the event for the serial task |
| guiott | 2:8917036cbf69 | 23 | */ |
| guiott | 2:8917036cbf69 | 24 | char c = gps.getc(); |
| guiott | 2:8917036cbf69 | 25 | msgBuff[bufferSelect & 1][writePointer] = c; |
| guiott | 2:8917036cbf69 | 26 | if (writePointer++ == BUFF_SIZE) |
| guiott | 2:8917036cbf69 | 27 | { |
| guiott | 2:8917036cbf69 | 28 | writePointer = 0; |
| guiott | 2:8917036cbf69 | 29 | } |
| guiott | 2:8917036cbf69 | 30 | if (writePointer > 200) |
| guiott | 2:8917036cbf69 | 31 | {// GPS input buffer full, start computing coordinates |
| guiott | 2:8917036cbf69 | 32 | os.SetEvent(GPS_SERIAL_IN_EVT, GPS_SERIAL_TASK); |
| guiott | 2:8917036cbf69 | 33 | } |
| guiott | 2:8917036cbf69 | 34 | } |
| guiott | 2:8917036cbf69 | 35 | |
| guiott | 2:8917036cbf69 | 36 | void trace_h(const char *str, int str_size) |
| guiott | 2:8917036cbf69 | 37 | {/** |
| guiott | 2:8917036cbf69 | 38 | *\brief output on console what's received on GPS serial |
| guiott | 2:8917036cbf69 | 39 | Callback function for NMEA parser buffer trace |
| guiott | 2:8917036cbf69 | 40 | */ |
| guiott | 2:8917036cbf69 | 41 | if( PcMonitor==5 || PcMonitor>5) |
| guiott | 2:8917036cbf69 | 42 | { |
| guiott | 2:8917036cbf69 | 43 | for (int i = 0; i < str_size; i++) |
| guiott | 2:8917036cbf69 | 44 | { |
| guiott | 2:8917036cbf69 | 45 | pc.putc(*str++); |
| guiott | 2:8917036cbf69 | 46 | } |
| guiott | 2:8917036cbf69 | 47 | } |
| guiott | 2:8917036cbf69 | 48 | } |
| guiott | 2:8917036cbf69 | 49 | |
| guiott | 2:8917036cbf69 | 50 | void error_h(const char *str, int str_size) |
| guiott | 2:8917036cbf69 | 51 | {/** |
| guiott | 2:8917036cbf69 | 52 | *\brief Callback function for NMEA parser error |
| guiott | 2:8917036cbf69 | 53 | */ |
| guiott | 2:8917036cbf69 | 54 | for (int i = 0; i < str_size; i++) |
| guiott | 2:8917036cbf69 | 55 | { |
| guiott | 2:8917036cbf69 | 56 | pc.putc(*str++); |
| guiott | 2:8917036cbf69 | 57 | } |
| guiott | 2:8917036cbf69 | 58 | } |
| guiott | 2:8917036cbf69 | 59 | |
| guiott | 2:8917036cbf69 | 60 | |
| guiott | 2:8917036cbf69 | 61 | double trunc(double v) |
| guiott | 2:8917036cbf69 | 62 | {/** |
| guiott | 2:8917036cbf69 | 63 | *\brief Return nearest integer vaule less than input |
| guiott | 2:8917036cbf69 | 64 | * |
| guiott | 2:8917036cbf69 | 65 | *\parameters double variable to get nearest ingeger |
| guiott | 2:8917036cbf69 | 66 | * |
| guiott | 2:8917036cbf69 | 67 | *\return double |
| guiott | 2:8917036cbf69 | 68 | */ |
| guiott | 2:8917036cbf69 | 69 | if(v < 0.0) |
| guiott | 2:8917036cbf69 | 70 | { |
| guiott | 2:8917036cbf69 | 71 | v*= -1.0; |
| guiott | 2:8917036cbf69 | 72 | v = floor(v); |
| guiott | 2:8917036cbf69 | 73 | v*=-1.0; |
| guiott | 2:8917036cbf69 | 74 | } else { |
| guiott | 2:8917036cbf69 | 75 | v = floor(v); |
| guiott | 2:8917036cbf69 | 76 | } |
| guiott | 2:8917036cbf69 | 77 | return v; |
| guiott | 2:8917036cbf69 | 78 | } |
| guiott | 2:8917036cbf69 | 79 | |
| guiott | 2:8917036cbf69 | 80 | void GpsStringParse(void) |
| guiott | 2:8917036cbf69 | 81 | {/** |
| guiott | 2:8917036cbf69 | 82 | *\brief parse the GPS string to extract info |
| guiott | 2:8917036cbf69 | 83 | */ |
| guiott | 2:8917036cbf69 | 84 | gps.attach(NULL); // Suspend serial interrupt while buffer change |
| guiott | 2:8917036cbf69 | 85 | size = writePointer; |
| guiott | 2:8917036cbf69 | 86 | writePointer = 0; |
| guiott | 2:8917036cbf69 | 87 | bufferSelect++; // Change buffer |
| guiott | 2:8917036cbf69 | 88 | gps.attach( &GpsSerialIsr ); // Resume serial interrupt |
| guiott | 2:8917036cbf69 | 89 | nmea_parse(&parser, msgBuff[(bufferSelect-1)&1], size, &info); |
| guiott | 2:8917036cbf69 | 90 | Coordinates(); // transform nmea coordinates in decimal degrees |
| guiott | 2:8917036cbf69 | 91 | } |
| guiott | 2:8917036cbf69 | 92 | |
| guiott | 2:8917036cbf69 | 93 | void GpsDist(void) |
| guiott | 2:8917036cbf69 | 94 | {/** |
| guiott | 2:8917036cbf69 | 95 | *\brief compute the distance and direction (forward and reverse) |
| guiott | 2:8917036cbf69 | 96 | from point A to point B on the ellipsoid |
| guiott | 2:8917036cbf69 | 97 | */ |
| guiott | 2:8917036cbf69 | 98 | |
| guiott | 2:8917036cbf69 | 99 | nmea_info2pos(&info, &Pos[0]); // current position |
| guiott | 2:8917036cbf69 | 100 | nmea_info2pos(&Dest, &Pos[1]); // destination |
| guiott | 2:8917036cbf69 | 101 | |
| guiott | 2:8917036cbf69 | 102 | Path.Dist = nmea_distance_ellipsoid(&Pos[0], &Pos[1], &Path.Azimuth[1], &Path.Azimuth[0]); |
| guiott | 2:8917036cbf69 | 103 | |
| guiott | 2:8917036cbf69 | 104 | if(Path.Azimuth[0] > NMEA_PI) |
| guiott | 2:8917036cbf69 | 105 | {// reverse direction |
| guiott | 2:8917036cbf69 | 106 | Path.Azimuth[0] = Path.Azimuth[0] - NMEA_PI; |
| guiott | 2:8917036cbf69 | 107 | } |
| guiott | 2:8917036cbf69 | 108 | else |
| guiott | 2:8917036cbf69 | 109 | { |
| guiott | 2:8917036cbf69 | 110 | Path.Azimuth[1] = Path.Azimuth[1] + NMEA_PI; |
| guiott | 2:8917036cbf69 | 111 | } |
| guiott | 2:8917036cbf69 | 112 | |
| guiott | 2:8917036cbf69 | 113 | for(int i=0; i < 2; i++) |
| guiott | 2:8917036cbf69 | 114 | { |
| guiott | 2:8917036cbf69 | 115 | Path.Azimuth[i]=(nmea_radian2degree(Path.Azimuth[i])); |
| guiott | 2:8917036cbf69 | 116 | if(Path.Azimuth[i] > 360) |
| guiott | 2:8917036cbf69 | 117 | { |
| guiott | 2:8917036cbf69 | 118 | Path.Azimuth[i] = Path.Azimuth[i] - 360; |
| guiott | 2:8917036cbf69 | 119 | } |
| guiott | 2:8917036cbf69 | 120 | if(Path.Azimuth[i] < 0) |
| guiott | 2:8917036cbf69 | 121 | { |
| guiott | 2:8917036cbf69 | 122 | Path.Azimuth[i] = Path.Azimuth[i] + 360; |
| guiott | 2:8917036cbf69 | 123 | } |
| guiott | 2:8917036cbf69 | 124 | } |
| guiott | 2:8917036cbf69 | 125 | } |
| guiott | 2:8917036cbf69 | 126 | |
| guiott | 2:8917036cbf69 | 127 | void Coordinates(void) |
| guiott | 2:8917036cbf69 | 128 | {/** |
| guiott | 2:8917036cbf69 | 129 | *\brief transform nmea coordinates in decimal degrees |
| guiott | 2:8917036cbf69 | 130 | */ |
| guiott | 2:8917036cbf69 | 131 | degrees = trunc(info.lat / 100.0); |
| guiott | 2:8917036cbf69 | 132 | minutes = info.lat - (degrees * 100.0); |
| guiott | 2:8917036cbf69 | 133 | latitude = degrees + minutes / 60.0; |
| guiott | 2:8917036cbf69 | 134 | degrees = trunc(info.lon / 100.0); |
| guiott | 2:8917036cbf69 | 135 | minutes = info.lon - (degrees * 100.0); |
| guiott | 2:8917036cbf69 | 136 | longitude = degrees + minutes / 60.0; |
| guiott | 2:8917036cbf69 | 137 | } |
| guiott | 2:8917036cbf69 | 138 | |
| guiott | 2:8917036cbf69 | 139 | void Deg2DegMinSec(double DecDeg, DegMinSec *DecSec) |
| guiott | 2:8917036cbf69 | 140 | {/** |
| guiott | 2:8917036cbf69 | 141 | *\brief convert decimalDeg to Deg Min decimalSec |
| guiott | 2:8917036cbf69 | 142 | */ |
| guiott | 2:8917036cbf69 | 143 | DecSec->Deg = trunc(DecDeg); |
| guiott | 2:8917036cbf69 | 144 | double MinDec = (DecDeg - DecSec->Deg); |
| guiott | 2:8917036cbf69 | 145 | DecSec->Min = trunc(MinDec * 60); |
| guiott | 2:8917036cbf69 | 146 | DecSec->Sec = (MinDec * 3600) - (DecSec->Min * 60); |
| guiott | 2:8917036cbf69 | 147 | } |