GPS GNSS
Dependencies: Vector3
GPS_HAPS.cpp@2:abd509e1f4d1, 2021-09-07 (annotated)
- Committer:
- cocorlow
- Date:
- Tue Sep 07 06:58:56 2021 +0000
- Revision:
- 2:abd509e1f4d1
- Parent:
- 1:543687550091
- Child:
- 3:b0d594e5d9e2
Time modified;
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
cocorlow | 0:6c7f7ed85f2c | 1 | #include "mbed.h" |
cocorlow | 0:6c7f7ed85f2c | 2 | #include "GPS_HAPS.hpp" |
cocorlow | 0:6c7f7ed85f2c | 3 | #include <string.h> |
cocorlow | 0:6c7f7ed85f2c | 4 | #include <stdlib.h> |
cocorlow | 0:6c7f7ed85f2c | 5 | #include "Vector3.hpp" |
cocorlow | 0:6c7f7ed85f2c | 6 | #include <math.h> |
cocorlow | 0:6c7f7ed85f2c | 7 | #define M_PI 3.14159265358979f |
cocorlow | 0:6c7f7ed85f2c | 8 | |
cocorlow | 0:6c7f7ed85f2c | 9 | GPS_HAPS::GPS_HAPS(PinName tx, PinName rx, int baud) |
cocorlow | 0:6c7f7ed85f2c | 10 | :serial(tx, rx, baud), receive_start(0), receive_index(0), receive_request(0) |
cocorlow | 0:6c7f7ed85f2c | 11 | { |
cocorlow | 0:6c7f7ed85f2c | 12 | } |
cocorlow | 0:6c7f7ed85f2c | 13 | |
cocorlow | 0:6c7f7ed85f2c | 14 | void GPS_HAPS::Receive() |
cocorlow | 0:6c7f7ed85f2c | 15 | { |
cocorlow | 0:6c7f7ed85f2c | 16 | while (serial.readable()) |
cocorlow | 0:6c7f7ed85f2c | 17 | { |
cocorlow | 0:6c7f7ed85f2c | 18 | char c; |
cocorlow | 0:6c7f7ed85f2c | 19 | c = serial.getc(); |
cocorlow | 0:6c7f7ed85f2c | 20 | // pc.putc(c); |
cocorlow | 0:6c7f7ed85f2c | 21 | receive_buffer[receive_index] = c; |
cocorlow | 0:6c7f7ed85f2c | 22 | if (c == '\r') |
cocorlow | 0:6c7f7ed85f2c | 23 | { |
cocorlow | 0:6c7f7ed85f2c | 24 | receive_request++; |
cocorlow | 0:6c7f7ed85f2c | 25 | } |
cocorlow | 0:6c7f7ed85f2c | 26 | receive_index = (receive_index + 1) % RECEIVE_SIZE; |
cocorlow | 0:6c7f7ed85f2c | 27 | } |
cocorlow | 0:6c7f7ed85f2c | 28 | } |
cocorlow | 0:6c7f7ed85f2c | 29 | |
cocorlow | 0:6c7f7ed85f2c | 30 | void GPS_HAPS::Loop() |
cocorlow | 0:6c7f7ed85f2c | 31 | { |
cocorlow | 0:6c7f7ed85f2c | 32 | if (receive_request > 0) |
cocorlow | 0:6c7f7ed85f2c | 33 | { |
cocorlow | 0:6c7f7ed85f2c | 34 | if (receive_buffer[receive_start] == '$') |
cocorlow | 0:6c7f7ed85f2c | 35 | { |
cocorlow | 0:6c7f7ed85f2c | 36 | for (int i = 0; i < SENTENCE_SIZE; i++) |
cocorlow | 0:6c7f7ed85f2c | 37 | { |
cocorlow | 0:6c7f7ed85f2c | 38 | char c; |
cocorlow | 0:6c7f7ed85f2c | 39 | c = receive_buffer[(receive_start + i) % RECEIVE_SIZE]; |
cocorlow | 0:6c7f7ed85f2c | 40 | sentence_buffer[i] = c; |
cocorlow | 0:6c7f7ed85f2c | 41 | if (c == '\r') |
cocorlow | 0:6c7f7ed85f2c | 42 | { |
cocorlow | 0:6c7f7ed85f2c | 43 | receive_start = (receive_start + i + 1) % RECEIVE_SIZE; |
cocorlow | 0:6c7f7ed85f2c | 44 | sentence_buffer[i+1] = 0; |
cocorlow | 0:6c7f7ed85f2c | 45 | break; |
cocorlow | 0:6c7f7ed85f2c | 46 | } |
cocorlow | 0:6c7f7ed85f2c | 47 | } |
cocorlow | 0:6c7f7ed85f2c | 48 | Parse(sentence_buffer); |
cocorlow | 0:6c7f7ed85f2c | 49 | receive_request--; |
cocorlow | 0:6c7f7ed85f2c | 50 | } |
cocorlow | 0:6c7f7ed85f2c | 51 | else |
cocorlow | 0:6c7f7ed85f2c | 52 | { |
cocorlow | 0:6c7f7ed85f2c | 53 | while (receive_start != receive_index) |
cocorlow | 0:6c7f7ed85f2c | 54 | { |
cocorlow | 0:6c7f7ed85f2c | 55 | receive_start = (receive_start + 1) % RECEIVE_SIZE; |
cocorlow | 0:6c7f7ed85f2c | 56 | if (receive_buffer[receive_start] == '$') |
cocorlow | 0:6c7f7ed85f2c | 57 | { |
cocorlow | 0:6c7f7ed85f2c | 58 | break; |
cocorlow | 0:6c7f7ed85f2c | 59 | } |
cocorlow | 0:6c7f7ed85f2c | 60 | } |
cocorlow | 0:6c7f7ed85f2c | 61 | } |
cocorlow | 0:6c7f7ed85f2c | 62 | } |
cocorlow | 0:6c7f7ed85f2c | 63 | } |
cocorlow | 0:6c7f7ed85f2c | 64 | |
cocorlow | 0:6c7f7ed85f2c | 65 | void GPS_HAPS::Parse(char *cmd) |
cocorlow | 0:6c7f7ed85f2c | 66 | { |
cocorlow | 0:6c7f7ed85f2c | 67 | char ns, ew, tf, status; |
cocorlow | 0:6c7f7ed85f2c | 68 | int fq, nst, fix, date; // fix quality, Number of satellites being tracked, 3D fix |
cocorlow | 0:6c7f7ed85f2c | 69 | float latitude, longitude, timefix, speed, altitude; |
cocorlow | 0:6c7f7ed85f2c | 70 | char s[16]; |
cocorlow | 0:6c7f7ed85f2c | 71 | |
cocorlow | 0:6c7f7ed85f2c | 72 | // Global Positioning System Fix Data |
cocorlow | 0:6c7f7ed85f2c | 73 | // $GNGGA,030503.00,3542.92568,N,13945.76087,E,1,05,10.02,225.2,M,39.4,M,,*7C |
cocorlow | 0:6c7f7ed85f2c | 74 | // if(strncmp(cmd,"$GPGGA", 6) == 0) |
cocorlow | 0:6c7f7ed85f2c | 75 | if(strstr(cmd,"GGA") != NULL) |
cocorlow | 0:6c7f7ed85f2c | 76 | { |
cocorlow | 0:6c7f7ed85f2c | 77 | sscanf(cmd, "%[^,],%f,%f,%c,%f,%c,%d,%d,%f", s, &timefix, &latitude, &ns, &longitude, &ew, &fq, &nst, &altitude); |
cocorlow | 0:6c7f7ed85f2c | 78 | float lat = (int)latitude/100; |
cocorlow | 0:6c7f7ed85f2c | 79 | Latitude = lat + (latitude-lat*100)/60.0f; |
cocorlow | 0:6c7f7ed85f2c | 80 | float lon = (int)longitude/100; |
cocorlow | 0:6c7f7ed85f2c | 81 | Longitude = lon + (longitude-lon*100)/60.0f; |
cocorlow | 0:6c7f7ed85f2c | 82 | N_S = ns; |
cocorlow | 0:6c7f7ed85f2c | 83 | E_W = ew; |
cocorlow | 0:6c7f7ed85f2c | 84 | Quality = fq; |
cocorlow | 0:6c7f7ed85f2c | 85 | Satellites = nst; |
cocorlow | 0:6c7f7ed85f2c | 86 | Altitude = altitude; |
cocorlow | 0:6c7f7ed85f2c | 87 | // pc.printf("%s\n", cmd); |
cocorlow | 0:6c7f7ed85f2c | 88 | // pc.printf("##GPGGA Fix taken at: %f, Latitude: %f %c, Longitude: %f %c, Fix quality: %d, Number of sat: %d, Altitude: %f M\r\n", timefix, latitude, ns, longitude, ew, fq, nst, altitude); |
cocorlow | 0:6c7f7ed85f2c | 89 | } |
cocorlow | 0:6c7f7ed85f2c | 90 | |
cocorlow | 0:6c7f7ed85f2c | 91 | // // Satellite status |
cocorlow | 0:6c7f7ed85f2c | 92 | // else if(strncmp(cmd,"$GPGSA", 6) == 0) |
cocorlow | 0:6c7f7ed85f2c | 93 | // { |
cocorlow | 0:6c7f7ed85f2c | 94 | // sscanf(cmd, "$GPGSA,%c,%d,%d", &tf, &fix, &nst); |
cocorlow | 0:6c7f7ed85f2c | 95 | // pc.printf("GPGSA Type fix: %c, 3D fix: %d, number of sat: %d\r\n", tf, fix, nst); |
cocorlow | 0:6c7f7ed85f2c | 96 | // } |
cocorlow | 0:6c7f7ed85f2c | 97 | |
cocorlow | 0:6c7f7ed85f2c | 98 | // Geographic position, Latitude and Longitude |
cocorlow | 0:6c7f7ed85f2c | 99 | // else if(strncmp(cmd,"$GPGLL", 6) == 0) |
cocorlow | 0:6c7f7ed85f2c | 100 | // { |
cocorlow | 0:6c7f7ed85f2c | 101 | // sscanf(cmd, "$GPGLL,%f,%c,%f,%c,%f", &latitude, &ns, &longitude, &ew, &timefix); |
cocorlow | 0:6c7f7ed85f2c | 102 | // pc.printf("GPGLL Latitude: %f %c, Longitude: %f %c, Fix taken at: %f\n", latitude, ns, longitude, ew, timefix); |
cocorlow | 0:6c7f7ed85f2c | 103 | // } |
cocorlow | 0:6c7f7ed85f2c | 104 | |
cocorlow | 0:6c7f7ed85f2c | 105 | // Geographic position, Latitude and Longitude |
cocorlow | 0:6c7f7ed85f2c | 106 | // $GNRMC,035344.00,A,3542.91153,N,13945.74153,E,0.239,,070921,,,A,V*15 |
cocorlow | 0:6c7f7ed85f2c | 107 | else if(strstr(cmd,"RMC") != NULL) |
cocorlow | 0:6c7f7ed85f2c | 108 | { |
cocorlow | 0:6c7f7ed85f2c | 109 | sscanf(cmd, "%[^,],%f,%c,%f,%c,%f,%c,%f,,%d", s, &timefix, &status, &latitude, &ns, &longitude, &ew, &speed, &date); |
cocorlow | 0:6c7f7ed85f2c | 110 | // pc.printf("%s,%f,%c,%f,%c,%f,%c,%f,,%d", s, timefix, status, latitude, ns, longitude, ew, speed, date); |
cocorlow | 0:6c7f7ed85f2c | 111 | float time = timefix + 90000.00f; |
cocorlow | 0:6c7f7ed85f2c | 112 | Hours = int(time) / 10000; |
cocorlow | 0:6c7f7ed85f2c | 113 | Minutes = (int(time) % 10000) / 100; |
cocorlow | 0:6c7f7ed85f2c | 114 | Seconds = int(time) % 100; |
cocorlow | 0:6c7f7ed85f2c | 115 | Milliseconds = (int)(((time - (Hours*10000+Minutes*100+Seconds)))/0.001f); |
cocorlow | 2:abd509e1f4d1 | 116 | Time = Hours*3600000+Minutes*60000+Seconds*1000+Milliseconds; |
cocorlow | 0:6c7f7ed85f2c | 117 | Speed = speed * 0.514444f; |
cocorlow | 0:6c7f7ed85f2c | 118 | Year = date % 100; |
cocorlow | 0:6c7f7ed85f2c | 119 | date /= 100; |
cocorlow | 0:6c7f7ed85f2c | 120 | Month = date % 100; |
cocorlow | 0:6c7f7ed85f2c | 121 | date /= 100; |
cocorlow | 0:6c7f7ed85f2c | 122 | Day = date; |
cocorlow | 0:6c7f7ed85f2c | 123 | // pc.printf("%s\n", cmd); |
cocorlow | 0:6c7f7ed85f2c | 124 | // pc.printf("##GPRMC Fix taken at: %f, Status: %c, Latitude: %f %c, Longitude: %f %c, Speed: %f, Date: %d\r\n", timefix, status, latitude, ns, longitude, ew, speed, date); |
cocorlow | 0:6c7f7ed85f2c | 125 | } |
cocorlow | 0:6c7f7ed85f2c | 126 | |
cocorlow | 0:6c7f7ed85f2c | 127 | // |
cocorlow | 0:6c7f7ed85f2c | 128 | // $GNVTG,,T,,M,0.444,N,0.822,K,A*31 |
cocorlow | 0:6c7f7ed85f2c | 129 | else if(strstr(cmd,"VTG") != NULL) |
cocorlow | 0:6c7f7ed85f2c | 130 | { |
cocorlow | 0:6c7f7ed85f2c | 131 | // pc.printf("%s\n", cmd); |
cocorlow | 0:6c7f7ed85f2c | 132 | Direction = -1.0f; |
cocorlow | 0:6c7f7ed85f2c | 133 | if(strstr(cmd, "VTG,,") == NULL){ |
cocorlow | 0:6c7f7ed85f2c | 134 | sscanf(cmd, "%[^,],%f", s, &Direction); |
cocorlow | 0:6c7f7ed85f2c | 135 | // pc.printf("<<%f\r\n", Direction); |
cocorlow | 0:6c7f7ed85f2c | 136 | // pc.printf("its a Vector Track message.\n"); |
cocorlow | 0:6c7f7ed85f2c | 137 | } |
cocorlow | 0:6c7f7ed85f2c | 138 | } |
cocorlow | 0:6c7f7ed85f2c | 139 | |
cocorlow | 0:6c7f7ed85f2c | 140 | // else if(strncmp(cmd,"$GNGGA", 6) == 0) |
cocorlow | 0:6c7f7ed85f2c | 141 | // { |
cocorlow | 0:6c7f7ed85f2c | 142 | // sscanf(cmd, "$GNGGA,%f,%f,%c,%f,%c,%d,%d,%*f,%f", &timefix, &latitude, &ns, &longitude, &ew, &fq, &nst, &altitude); |
cocorlow | 0:6c7f7ed85f2c | 143 | // pc.printf("GNGGA Fix taken at: %f, Latitude: %f %c, Longitude: %f %c, Fix quality: %d, Number of sat: %d, Altitude: %f M\n", timefix, latitude, ns, longitude, ew, fq, nst, altitude); |
cocorlow | 0:6c7f7ed85f2c | 144 | // parseTime(timefix); |
cocorlow | 0:6c7f7ed85f2c | 145 | // pc.printf("Time: %d:%d:%d\n", h_time, m_time, s_time); |
cocorlow | 0:6c7f7ed85f2c | 146 | // } |
cocorlow | 0:6c7f7ed85f2c | 147 | |
cocorlow | 0:6c7f7ed85f2c | 148 | // else if(strncmp(cmd,"$GNGSA", 6) == 0) |
cocorlow | 0:6c7f7ed85f2c | 149 | // { |
cocorlow | 0:6c7f7ed85f2c | 150 | // sscanf(cmd, "$GNGSA,%c,%d,%d", &tf, &fix, &nst); |
cocorlow | 0:6c7f7ed85f2c | 151 | // pc.printf("GNGSA Type fix: %c, 3D fix: %d, number of sat: %d\r\n", tf, fix, nst); |
cocorlow | 0:6c7f7ed85f2c | 152 | // } |
cocorlow | 0:6c7f7ed85f2c | 153 | |
cocorlow | 0:6c7f7ed85f2c | 154 | // else if(strncmp(cmd,"$GPGSV", 6) == 0) |
cocorlow | 0:6c7f7ed85f2c | 155 | // { |
cocorlow | 0:6c7f7ed85f2c | 156 | // // pc.printf("its a Satellite details message.\n"); |
cocorlow | 0:6c7f7ed85f2c | 157 | // } |
cocorlow | 0:6c7f7ed85f2c | 158 | |
cocorlow | 0:6c7f7ed85f2c | 159 | // else if(strncmp(cmd,"$GNGLL", 6) == 0) |
cocorlow | 0:6c7f7ed85f2c | 160 | // { |
cocorlow | 0:6c7f7ed85f2c | 161 | // sscanf(cmd, "$GNGLL,%f,%c,%f,%c,%f", &latitude, &ns, &longitude, &ew, &timefix); |
cocorlow | 0:6c7f7ed85f2c | 162 | // pc.printf("GNGLL Latitude: %f %c, Longitude: %f %c, Fix taken at: %f\n", latitude, ns, longitude, ew, timefix); |
cocorlow | 0:6c7f7ed85f2c | 163 | // } |
cocorlow | 0:6c7f7ed85f2c | 164 | |
cocorlow | 0:6c7f7ed85f2c | 165 | // else |
cocorlow | 0:6c7f7ed85f2c | 166 | // { |
cocorlow | 0:6c7f7ed85f2c | 167 | // pc.printf("Unknown message type\n"); |
cocorlow | 0:6c7f7ed85f2c | 168 | // } |
cocorlow | 0:6c7f7ed85f2c | 169 | } |
cocorlow | 0:6c7f7ed85f2c | 170 | |
cocorlow | 0:6c7f7ed85f2c | 171 | void GPS_HAPS::Punctuate() |
cocorlow | 0:6c7f7ed85f2c | 172 | { |
cocorlow | 0:6c7f7ed85f2c | 173 | } |
cocorlow | 0:6c7f7ed85f2c | 174 | |
cocorlow | 0:6c7f7ed85f2c | 175 | void GPS_HAPS::Attach() |
cocorlow | 0:6c7f7ed85f2c | 176 | { |
cocorlow | 0:6c7f7ed85f2c | 177 | serial.attach(this, &GPS_HAPS::Receive, Serial::RxIrq); |
cocorlow | 0:6c7f7ed85f2c | 178 | } |
cocorlow | 0:6c7f7ed85f2c | 179 | |
cocorlow | 0:6c7f7ed85f2c | 180 | void GPS_HAPS::Update() |
cocorlow | 0:6c7f7ed85f2c | 181 | { |
cocorlow | 0:6c7f7ed85f2c | 182 | } |
cocorlow | 0:6c7f7ed85f2c | 183 | |
cocorlow | 0:6c7f7ed85f2c | 184 | void GPS_HAPS::Initialize() |
cocorlow | 0:6c7f7ed85f2c | 185 | { |
cocorlow | 0:6c7f7ed85f2c | 186 | Satellites = 0; |
cocorlow | 0:6c7f7ed85f2c | 187 | while (Satellites <= 4 || Latitude == 0.0f || Longitude == 0.0f || Elevation == 0.0f) |
cocorlow | 0:6c7f7ed85f2c | 188 | { |
cocorlow | 0:6c7f7ed85f2c | 189 | } |
cocorlow | 0:6c7f7ed85f2c | 190 | CalcurateUnit(); |
cocorlow | 0:6c7f7ed85f2c | 191 | } |
cocorlow | 0:6c7f7ed85f2c | 192 | |
cocorlow | 0:6c7f7ed85f2c | 193 | Vector3 GPS_HAPS::ToUniversalUnit() |
cocorlow | 0:6c7f7ed85f2c | 194 | { |
cocorlow | 0:6c7f7ed85f2c | 195 | // 東経180度、北緯0度で精度最大 |
cocorlow | 0:6c7f7ed85f2c | 196 | float pi_2_theta = (Latitude * ((N_S == 'N') ? 1.0f : -1.0f)) * M_PI / 180.0f; |
cocorlow | 0:6c7f7ed85f2c | 197 | float pi_phi = ((E_W == 'E') ? Longitude - 180.0f : 180.0f - Longitude) * M_PI / 180.0f; |
cocorlow | 0:6c7f7ed85f2c | 198 | float x = - cosf(pi_2_theta) * cosf(pi_phi); |
cocorlow | 0:6c7f7ed85f2c | 199 | float y = - cosf(pi_2_theta) * sinf(pi_phi); |
cocorlow | 0:6c7f7ed85f2c | 200 | float z = sinf(pi_2_theta); |
cocorlow | 0:6c7f7ed85f2c | 201 | Vector3 v(x, y, z); |
cocorlow | 0:6c7f7ed85f2c | 202 | return v; |
cocorlow | 0:6c7f7ed85f2c | 203 | } |
cocorlow | 0:6c7f7ed85f2c | 204 | |
cocorlow | 0:6c7f7ed85f2c | 205 | Vector3 GPS_HAPS::ToUniversal() |
cocorlow | 0:6c7f7ed85f2c | 206 | { |
cocorlow | 0:6c7f7ed85f2c | 207 | Vector3 v = ToUniversalUnit(); |
cocorlow | 0:6c7f7ed85f2c | 208 | return (Radius + Elevation) * v; |
cocorlow | 0:6c7f7ed85f2c | 209 | } |
cocorlow | 0:6c7f7ed85f2c | 210 | |
cocorlow | 0:6c7f7ed85f2c | 211 | void GPS_HAPS::CalcurateUnit() |
cocorlow | 0:6c7f7ed85f2c | 212 | { |
cocorlow | 0:6c7f7ed85f2c | 213 | Vector3 _d = -1.0f * ToUniversalUnit(); |
cocorlow | 0:6c7f7ed85f2c | 214 | |
cocorlow | 0:6c7f7ed85f2c | 215 | UniversalZeroPosition = -(Radius+Elevation)*_d; |
cocorlow | 0:6c7f7ed85f2c | 216 | Vector3 _z(0.0f, 0.0f, 1.0f); |
cocorlow | 0:6c7f7ed85f2c | 217 | Vector3 _e = _d * _z; |
cocorlow | 0:6c7f7ed85f2c | 218 | Vector3 _n = _e * _d; |
cocorlow | 0:6c7f7ed85f2c | 219 | UniversalZeroUnitN = _n; |
cocorlow | 0:6c7f7ed85f2c | 220 | UniversalZeroUnitE = _e; |
cocorlow | 0:6c7f7ed85f2c | 221 | UniversalZeroUnitD = _d; |
cocorlow | 0:6c7f7ed85f2c | 222 | } |
cocorlow | 0:6c7f7ed85f2c | 223 | |
cocorlow | 0:6c7f7ed85f2c | 224 | void GPS_HAPS::Calcurate() |
cocorlow | 0:6c7f7ed85f2c | 225 | { |
cocorlow | 0:6c7f7ed85f2c | 226 | UniversalPosition = ToUniversal(); |
cocorlow | 0:6c7f7ed85f2c | 227 | Vector3 relative = UniversalPosition - UniversalZeroPosition; |
cocorlow | 0:6c7f7ed85f2c | 228 | Vector3 _position(relative % UniversalZeroUnitN, relative % UniversalZeroUnitE, relative % UniversalZeroUnitD); |
cocorlow | 0:6c7f7ed85f2c | 229 | Position = _position; |
cocorlow | 0:6c7f7ed85f2c | 230 | } |
cocorlow | 0:6c7f7ed85f2c | 231 | |
cocorlow | 0:6c7f7ed85f2c | 232 | Vector3 GPS_HAPS::Calcurate(Vector3 position) |
cocorlow | 0:6c7f7ed85f2c | 233 | { |
cocorlow | 0:6c7f7ed85f2c | 234 | UniversalPosition = ToUniversal(); |
cocorlow | 0:6c7f7ed85f2c | 235 | Vector3 relative = position - UniversalZeroPosition; |
cocorlow | 0:6c7f7ed85f2c | 236 | Vector3 _position(relative % UniversalZeroUnitN, relative % UniversalZeroUnitE, relative % UniversalZeroUnitD); |
cocorlow | 0:6c7f7ed85f2c | 237 | return _position; |
cocorlow | 0:6c7f7ed85f2c | 238 | } |