Romain Ardino / Mbed OS ProjetER4

ffff

Dependencies:   Servo pourtibo driver_mbed_TH02

GroveGPS.h@64:b57da430b53c, 2022-03-08 (annotated)

Committer:
paparoms
Date:
Tue Mar 08 11:01:51 2022 +0000
Revision:
64:b57da430b53c
dd

Who changed what in which revision?

UserRevisionLine numberNew contents of line
paparoms 64:b57da430b53c 1 #ifndef _GROVE_GPS_H_
paparoms 64:b57da430b53c 2 #define _GROVE_GPS_H_
paparoms 64:b57da430b53c 3
paparoms 64:b57da430b53c 4 #include "mbed.h"
paparoms 64:b57da430b53c 5 #include <stdlib.h>
paparoms 64:b57da430b53c 6 #include <string>
paparoms 64:b57da430b53c 7
paparoms 64:b57da430b53c 8 class GroveGPS {
paparoms 64:b57da430b53c 9
paparoms 64:b57da430b53c 10 public:
paparoms 64:b57da430b53c 11
paparoms 64:b57da430b53c 12 GroveGPS(PinName tx=D8, PinName rx=D2) : gps_serial(tx, rx, 9600) {
paparoms 64:b57da430b53c 13 memset(_isr_line_bufs, 0, sizeof(_isr_line_bufs));
paparoms 64:b57da430b53c 14 _first_line_in_use = true;
paparoms 64:b57da430b53c 15 _isr_line_buf_pos = 0;
paparoms 64:b57da430b53c 16 memset(_last_line, 0, sizeof(_last_line));
paparoms 64:b57da430b53c 17 _last_line_updated = false;
paparoms 64:b57da430b53c 18 gps_serial.attach(callback(this, &GroveGPS::read_serial), SerialBase::RxIrq);
paparoms 64:b57da430b53c 19 }
paparoms 64:b57da430b53c 20
paparoms 64:b57da430b53c 21 struct GGA {
paparoms 64:b57da430b53c 22 double utc_time; // Format: hhmmss.sss
paparoms 64:b57da430b53c 23 double latitude; // Format: ddmm.mmmm
paparoms 64:b57da430b53c 24 char ns_indicator; // Format: N=north or S=south
paparoms 64:b57da430b53c 25 double longitude; // Format: dddmm.mmmm
paparoms 64:b57da430b53c 26 char ew_indicator; // Format: E=east or W=west
paparoms 64:b57da430b53c 27 int position_fix; // Options: [0=not available, 1=GPS SPS mode, 2=Differential GPS, 6=dead reckoning]
paparoms 64:b57da430b53c 28 int sats_used; // Range: 0-12
paparoms 64:b57da430b53c 29 double hdop; // Horizontal Dilution of Precision
paparoms 64:b57da430b53c 30 double msl_altitude;
paparoms 64:b57da430b53c 31 char msl_altitude_units;
paparoms 64:b57da430b53c 32 double geoid_separation;
paparoms 64:b57da430b53c 33 char geoid_separation_units;
paparoms 64:b57da430b53c 34 long age_of_diff;
paparoms 64:b57da430b53c 35 long diff_ref_station_id; //gps.gps_gga.msl_altitude
paparoms 64:b57da430b53c 36 } gps_gga;
paparoms 64:b57da430b53c 37
paparoms 64:b57da430b53c 38 void getTimestamp(char* buffer) {
paparoms 64:b57da430b53c 39 m.lock();
paparoms 64:b57da430b53c 40 parseLine();
paparoms 64:b57da430b53c 41
paparoms 64:b57da430b53c 42 sprintf(buffer, "%f", gps_gga.utc_time);
paparoms 64:b57da430b53c 43 m.unlock();
paparoms 64:b57da430b53c 44 }
paparoms 64:b57da430b53c 45
paparoms 64:b57da430b53c 46 void getLatitude(char* buffer) {
paparoms 64:b57da430b53c 47 m.lock();
paparoms 64:b57da430b53c 48 parseLine();
paparoms 64:b57da430b53c 49
paparoms 64:b57da430b53c 50 double coordinate = gps_gga.latitude;
paparoms 64:b57da430b53c 51 if (gps_gga.position_fix==0)
paparoms 64:b57da430b53c 52 sprintf(buffer, "N/A");
paparoms 64:b57da430b53c 53 else
paparoms 64:b57da430b53c 54 sprintf(buffer, "%c%f", (gps_gga.ns_indicator == 'N') ? '0' : '-', coordinate);
paparoms 64:b57da430b53c 55 m.unlock();
paparoms 64:b57da430b53c 56 }
paparoms 64:b57da430b53c 57
paparoms 64:b57da430b53c 58 void getLongitude(char* buffer) {
paparoms 64:b57da430b53c 59 m.lock();
paparoms 64:b57da430b53c 60 parseLine();
paparoms 64:b57da430b53c 61
paparoms 64:b57da430b53c 62 double coordinate = gps_gga.longitude;
paparoms 64:b57da430b53c 63 if (gps_gga.position_fix==0)
paparoms 64:b57da430b53c 64 sprintf(buffer, "N/A");
paparoms 64:b57da430b53c 65 else
paparoms 64:b57da430b53c 66 sprintf(buffer, "%c%f", (gps_gga.ew_indicator == 'E') ? '0' : '-', coordinate);
paparoms 64:b57da430b53c 67 m.unlock();
paparoms 64:b57da430b53c 68 }
paparoms 64:b57da430b53c 69
paparoms 64:b57da430b53c 70 void getAltitude(char* buffer) {
paparoms 64:b57da430b53c 71 m.lock();
paparoms 64:b57da430b53c 72 parseLine();
paparoms 64:b57da430b53c 73
paparoms 64:b57da430b53c 74 double coordinate = gps_gga.msl_altitude;
paparoms 64:b57da430b53c 75 if (gps_gga.position_fix==0)
paparoms 64:b57da430b53c 76 sprintf(buffer, "N/A");
paparoms 64:b57da430b53c 77 else
paparoms 64:b57da430b53c 78 sprintf(buffer, " %f", coordinate);
paparoms 64:b57da430b53c 79 m.unlock();
paparoms 64:b57da430b53c 80 }
paparoms 64:b57da430b53c 81
paparoms 64:b57da430b53c 82 void update() {
paparoms 64:b57da430b53c 83 m.lock();
paparoms 64:b57da430b53c 84 parseLine();
paparoms 64:b57da430b53c 85 m.unlock();
paparoms 64:b57da430b53c 86 }
paparoms 64:b57da430b53c 87
paparoms 64:b57da430b53c 88 private:
paparoms 64:b57da430b53c 89 static const size_t max_line_length = 256;
paparoms 64:b57da430b53c 90 char _isr_line_bufs[2][max_line_length];
paparoms 64:b57da430b53c 91 bool _first_line_in_use;
paparoms 64:b57da430b53c 92 size_t _isr_line_buf_pos;
paparoms 64:b57da430b53c 93 char _last_line[max_line_length];
paparoms 64:b57da430b53c 94 bool _last_line_updated;
paparoms 64:b57da430b53c 95
paparoms 64:b57da430b53c 96 RawSerial gps_serial;
paparoms 64:b57da430b53c 97 Mutex m;
paparoms 64:b57da430b53c 98
paparoms 64:b57da430b53c 99 void read_serial() {
paparoms 64:b57da430b53c 100 while (gps_serial.readable()) {
paparoms 64:b57da430b53c 101
paparoms 64:b57da430b53c 102 // Check for overflow
paparoms 64:b57da430b53c 103 if (_isr_line_buf_pos > max_line_length -1 ) {
paparoms 64:b57da430b53c 104 error("GPS error - line too long");
paparoms 64:b57da430b53c 105 _isr_line_buf_pos = 0;
paparoms 64:b57da430b53c 106 }
paparoms 64:b57da430b53c 107
paparoms 64:b57da430b53c 108 // Add a character to the active buffer
paparoms 64:b57da430b53c 109 char *buf = _isr_line_bufs[_first_line_in_use ? 0 : 1];
paparoms 64:b57da430b53c 110 char value = gps_serial.getc();
paparoms 64:b57da430b53c 111 buf[_isr_line_buf_pos] = value;
paparoms 64:b57da430b53c 112 _isr_line_buf_pos++;
paparoms 64:b57da430b53c 113
paparoms 64:b57da430b53c 114 // Check for end of line
paparoms 64:b57da430b53c 115 if (value == '\n') {
paparoms 64:b57da430b53c 116 buf[_isr_line_buf_pos] = 0;
paparoms 64:b57da430b53c 117 _isr_line_buf_pos = 0;
paparoms 64:b57da430b53c 118
paparoms 64:b57da430b53c 119 // Save off this line if it is valid
paparoms 64:b57da430b53c 120 if (memcmp("$GPGGA", buf, 6) == 0) {
paparoms 64:b57da430b53c 121 _first_line_in_use = !_first_line_in_use;
paparoms 64:b57da430b53c 122 _last_line_updated = true;
paparoms 64:b57da430b53c 123 }
paparoms 64:b57da430b53c 124 }
paparoms 64:b57da430b53c 125 }
paparoms 64:b57da430b53c 126 }
paparoms 64:b57da430b53c 127
paparoms 64:b57da430b53c 128 double convertGPSToDecimal(double coordinate) {
paparoms 64:b57da430b53c 129 int degrees = coordinate/100.0;
paparoms 64:b57da430b53c 130 int minutes = ((int)coordinate) % 100;
paparoms 64:b57da430b53c 131 double seconds = coordinate - ((int)coordinate);
paparoms 64:b57da430b53c 132 return degrees + (minutes+seconds)/60;
paparoms 64:b57da430b53c 133
paparoms 64:b57da430b53c 134 }
paparoms 64:b57da430b53c 135
paparoms 64:b57da430b53c 136 void parseLine() {
paparoms 64:b57da430b53c 137 bool parse_gga = false;
paparoms 64:b57da430b53c 138
paparoms 64:b57da430b53c 139 // Atomically copy the line buffer since the ISR can change it at any time
paparoms 64:b57da430b53c 140 core_util_critical_section_enter();
paparoms 64:b57da430b53c 141 if (_last_line_updated) {
paparoms 64:b57da430b53c 142 char *buf_saved = _isr_line_bufs[_first_line_in_use ? 1 : 0];
paparoms 64:b57da430b53c 143 strcpy(_last_line, buf_saved);
paparoms 64:b57da430b53c 144 parse_gga = true;
paparoms 64:b57da430b53c 145 _last_line_updated = false;
paparoms 64:b57da430b53c 146 }
paparoms 64:b57da430b53c 147 core_util_critical_section_exit();
paparoms 64:b57da430b53c 148
paparoms 64:b57da430b53c 149 if (parse_gga) {
paparoms 64:b57da430b53c 150 parseGGA();
paparoms 64:b57da430b53c 151 }
paparoms 64:b57da430b53c 152 }
paparoms 64:b57da430b53c 153
paparoms 64:b57da430b53c 154 void parseGGA() {
paparoms 64:b57da430b53c 155 char *line_pos = _last_line;
paparoms 64:b57da430b53c 156 for (int i=0; i<14; i++) {
paparoms 64:b57da430b53c 157 if (i==0) { // NMEA Tag
paparoms 64:b57da430b53c 158 } else if (i==1) { // UTC time
paparoms 64:b57da430b53c 159 gps_gga.utc_time = strtod(line_pos, 0);
paparoms 64:b57da430b53c 160
paparoms 64:b57da430b53c 161
paparoms 64:b57da430b53c 162 } else if (i==2) { // Latitude
paparoms 64:b57da430b53c 163 gps_gga.latitude = strtod(line_pos, 0);
paparoms 64:b57da430b53c 164 gps_gga.latitude=convertGPSToDecimal(gps_gga.latitude);
paparoms 64:b57da430b53c 165
paparoms 64:b57da430b53c 166 } else if (i==3) { // Latitude North/South indicator
paparoms 64:b57da430b53c 167 gps_gga.ns_indicator = line_pos[0];
paparoms 64:b57da430b53c 168 } else if (i==4) { // Longitude
paparoms 64:b57da430b53c 169 gps_gga.longitude = strtod(line_pos, 0);
paparoms 64:b57da430b53c 170 gps_gga.longitude=convertGPSToDecimal(gps_gga.longitude);
paparoms 64:b57da430b53c 171 } else if (i==5) { // Longitude indicator
paparoms 64:b57da430b53c 172 gps_gga.ew_indicator = line_pos[0];
paparoms 64:b57da430b53c 173 } else if (i==6) {
paparoms 64:b57da430b53c 174 gps_gga.position_fix= strtod(line_pos, 0);
paparoms 64:b57da430b53c 175 }
paparoms 64:b57da430b53c 176 else if (i==7) {
paparoms 64:b57da430b53c 177 gps_gga.sats_used= strtod(line_pos, 0);//nb satellite used
paparoms 64:b57da430b53c 178 }
paparoms 64:b57da430b53c 179 else if (i==8) {
paparoms 64:b57da430b53c 180 gps_gga.hdop= strtod(line_pos, 0);//horizontal precision
paparoms 64:b57da430b53c 181 }
paparoms 64:b57da430b53c 182 else if (i==9) {
paparoms 64:b57da430b53c 183 gps_gga.msl_altitude= strtod(line_pos, 0);//altitute
paparoms 64:b57da430b53c 184 }
paparoms 64:b57da430b53c 185
paparoms 64:b57da430b53c 186
paparoms 64:b57da430b53c 187 line_pos = strchr(line_pos, ',');
paparoms 64:b57da430b53c 188 if (line_pos == NULL) {
paparoms 64:b57da430b53c 189 break;
paparoms 64:b57da430b53c 190 }
paparoms 64:b57da430b53c 191 line_pos += 1;
paparoms 64:b57da430b53c 192 }
paparoms 64:b57da430b53c 193 }
paparoms 64:b57da430b53c 194 };
paparoms 64:b57da430b53c 195
paparoms 64:b57da430b53c 196 #endif