GPS GNSS
Dependencies: Vector3
GPS_HAPS.cpp@4:708e05b10257, 2021-09-08 (annotated)
- Committer:
- cocorlow
- Date:
- Wed Sep 08 13:02:17 2021 +0000
- Revision:
- 4:708e05b10257
- Parent:
- 3:b0d594e5d9e2
- Child:
- 5:f23265e8d4aa
Calcurate->Calculate
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 | 3:b0d594e5d9e2 | 78 | if (abs(latitude) < ERROR_F) |
cocorlow | 3:b0d594e5d9e2 | 79 | { |
cocorlow | 3:b0d594e5d9e2 | 80 | Latitude = -1.0f; |
cocorlow | 3:b0d594e5d9e2 | 81 | } |
cocorlow | 3:b0d594e5d9e2 | 82 | else |
cocorlow | 3:b0d594e5d9e2 | 83 | { |
cocorlow | 3:b0d594e5d9e2 | 84 | float lat = (int)latitude/100; |
cocorlow | 3:b0d594e5d9e2 | 85 | Latitude = lat + (latitude-lat*100)/60.0f; |
cocorlow | 3:b0d594e5d9e2 | 86 | } |
cocorlow | 3:b0d594e5d9e2 | 87 | if (abs(longitude) < ERROR_F) |
cocorlow | 3:b0d594e5d9e2 | 88 | { |
cocorlow | 3:b0d594e5d9e2 | 89 | Longitude = -1.0f; |
cocorlow | 3:b0d594e5d9e2 | 90 | } |
cocorlow | 3:b0d594e5d9e2 | 91 | else |
cocorlow | 3:b0d594e5d9e2 | 92 | { |
cocorlow | 3:b0d594e5d9e2 | 93 | float lon = (int)longitude/100; |
cocorlow | 3:b0d594e5d9e2 | 94 | Longitude = lon + (longitude-lon*100)/60.0f; |
cocorlow | 3:b0d594e5d9e2 | 95 | } |
cocorlow | 0:6c7f7ed85f2c | 96 | N_S = ns; |
cocorlow | 0:6c7f7ed85f2c | 97 | E_W = ew; |
cocorlow | 0:6c7f7ed85f2c | 98 | Quality = fq; |
cocorlow | 0:6c7f7ed85f2c | 99 | Satellites = nst; |
cocorlow | 3:b0d594e5d9e2 | 100 | if (abs(altitude) < ERROR_F) |
cocorlow | 3:b0d594e5d9e2 | 101 | { |
cocorlow | 3:b0d594e5d9e2 | 102 | Altitude = -1.0f; |
cocorlow | 3:b0d594e5d9e2 | 103 | } |
cocorlow | 3:b0d594e5d9e2 | 104 | { |
cocorlow | 3:b0d594e5d9e2 | 105 | Altitude = altitude; |
cocorlow | 3:b0d594e5d9e2 | 106 | } |
cocorlow | 0:6c7f7ed85f2c | 107 | // pc.printf("%s\n", cmd); |
cocorlow | 0:6c7f7ed85f2c | 108 | // 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 | 109 | } |
cocorlow | 0:6c7f7ed85f2c | 110 | |
cocorlow | 0:6c7f7ed85f2c | 111 | // // Satellite status |
cocorlow | 0:6c7f7ed85f2c | 112 | // else if(strncmp(cmd,"$GPGSA", 6) == 0) |
cocorlow | 0:6c7f7ed85f2c | 113 | // { |
cocorlow | 0:6c7f7ed85f2c | 114 | // sscanf(cmd, "$GPGSA,%c,%d,%d", &tf, &fix, &nst); |
cocorlow | 0:6c7f7ed85f2c | 115 | // pc.printf("GPGSA Type fix: %c, 3D fix: %d, number of sat: %d\r\n", tf, fix, nst); |
cocorlow | 0:6c7f7ed85f2c | 116 | // } |
cocorlow | 0:6c7f7ed85f2c | 117 | |
cocorlow | 0:6c7f7ed85f2c | 118 | // Geographic position, Latitude and Longitude |
cocorlow | 0:6c7f7ed85f2c | 119 | // else if(strncmp(cmd,"$GPGLL", 6) == 0) |
cocorlow | 0:6c7f7ed85f2c | 120 | // { |
cocorlow | 0:6c7f7ed85f2c | 121 | // sscanf(cmd, "$GPGLL,%f,%c,%f,%c,%f", &latitude, &ns, &longitude, &ew, &timefix); |
cocorlow | 0:6c7f7ed85f2c | 122 | // pc.printf("GPGLL Latitude: %f %c, Longitude: %f %c, Fix taken at: %f\n", latitude, ns, longitude, ew, timefix); |
cocorlow | 0:6c7f7ed85f2c | 123 | // } |
cocorlow | 0:6c7f7ed85f2c | 124 | |
cocorlow | 0:6c7f7ed85f2c | 125 | // Geographic position, Latitude and Longitude |
cocorlow | 0:6c7f7ed85f2c | 126 | // $GNRMC,035344.00,A,3542.91153,N,13945.74153,E,0.239,,070921,,,A,V*15 |
cocorlow | 0:6c7f7ed85f2c | 127 | else if(strstr(cmd,"RMC") != NULL) |
cocorlow | 0:6c7f7ed85f2c | 128 | { |
cocorlow | 0:6c7f7ed85f2c | 129 | sscanf(cmd, "%[^,],%f,%c,%f,%c,%f,%c,%f,,%d", s, &timefix, &status, &latitude, &ns, &longitude, &ew, &speed, &date); |
cocorlow | 0:6c7f7ed85f2c | 130 | // pc.printf("%s,%f,%c,%f,%c,%f,%c,%f,,%d", s, timefix, status, latitude, ns, longitude, ew, speed, date); |
cocorlow | 3:b0d594e5d9e2 | 131 | if (abs(timefix) > ERROR_F) |
cocorlow | 3:b0d594e5d9e2 | 132 | { |
cocorlow | 3:b0d594e5d9e2 | 133 | float time = timefix + 90000.00f; |
cocorlow | 3:b0d594e5d9e2 | 134 | Hours = int(time) / 10000; |
cocorlow | 3:b0d594e5d9e2 | 135 | Minutes = (int(time) % 10000) / 100; |
cocorlow | 3:b0d594e5d9e2 | 136 | Seconds = int(time) % 100; |
cocorlow | 3:b0d594e5d9e2 | 137 | Milliseconds = (int)(((time - (Hours*10000+Minutes*100+Seconds)))/0.001f); |
cocorlow | 3:b0d594e5d9e2 | 138 | Time = Hours*3600000+Minutes*60000+Seconds*1000+Milliseconds; |
cocorlow | 3:b0d594e5d9e2 | 139 | if (abs(speed) < ERROR_F) |
cocorlow | 3:b0d594e5d9e2 | 140 | { |
cocorlow | 3:b0d594e5d9e2 | 141 | Speed = -1.0f; |
cocorlow | 3:b0d594e5d9e2 | 142 | } |
cocorlow | 3:b0d594e5d9e2 | 143 | else |
cocorlow | 3:b0d594e5d9e2 | 144 | { |
cocorlow | 3:b0d594e5d9e2 | 145 | Speed = speed * 0.514444f; |
cocorlow | 3:b0d594e5d9e2 | 146 | } |
cocorlow | 3:b0d594e5d9e2 | 147 | Year = date % 100; |
cocorlow | 3:b0d594e5d9e2 | 148 | date /= 100; |
cocorlow | 3:b0d594e5d9e2 | 149 | Month = date % 100; |
cocorlow | 3:b0d594e5d9e2 | 150 | date /= 100; |
cocorlow | 3:b0d594e5d9e2 | 151 | Day = date; |
cocorlow | 3:b0d594e5d9e2 | 152 | } |
cocorlow | 0:6c7f7ed85f2c | 153 | // pc.printf("%s\n", cmd); |
cocorlow | 0:6c7f7ed85f2c | 154 | // 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 | 155 | } |
cocorlow | 0:6c7f7ed85f2c | 156 | |
cocorlow | 0:6c7f7ed85f2c | 157 | // |
cocorlow | 0:6c7f7ed85f2c | 158 | // $GNVTG,,T,,M,0.444,N,0.822,K,A*31 |
cocorlow | 0:6c7f7ed85f2c | 159 | else if(strstr(cmd,"VTG") != NULL) |
cocorlow | 0:6c7f7ed85f2c | 160 | { |
cocorlow | 0:6c7f7ed85f2c | 161 | // pc.printf("%s\n", cmd); |
cocorlow | 0:6c7f7ed85f2c | 162 | Direction = -1.0f; |
cocorlow | 0:6c7f7ed85f2c | 163 | if(strstr(cmd, "VTG,,") == NULL){ |
cocorlow | 0:6c7f7ed85f2c | 164 | sscanf(cmd, "%[^,],%f", s, &Direction); |
cocorlow | 0:6c7f7ed85f2c | 165 | // pc.printf("<<%f\r\n", Direction); |
cocorlow | 0:6c7f7ed85f2c | 166 | // pc.printf("its a Vector Track message.\n"); |
cocorlow | 0:6c7f7ed85f2c | 167 | } |
cocorlow | 0:6c7f7ed85f2c | 168 | } |
cocorlow | 0:6c7f7ed85f2c | 169 | |
cocorlow | 0:6c7f7ed85f2c | 170 | // else if(strncmp(cmd,"$GNGGA", 6) == 0) |
cocorlow | 0:6c7f7ed85f2c | 171 | // { |
cocorlow | 0:6c7f7ed85f2c | 172 | // sscanf(cmd, "$GNGGA,%f,%f,%c,%f,%c,%d,%d,%*f,%f", &timefix, &latitude, &ns, &longitude, &ew, &fq, &nst, &altitude); |
cocorlow | 0:6c7f7ed85f2c | 173 | // 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 | 174 | // parseTime(timefix); |
cocorlow | 0:6c7f7ed85f2c | 175 | // pc.printf("Time: %d:%d:%d\n", h_time, m_time, s_time); |
cocorlow | 0:6c7f7ed85f2c | 176 | // } |
cocorlow | 0:6c7f7ed85f2c | 177 | |
cocorlow | 0:6c7f7ed85f2c | 178 | // else if(strncmp(cmd,"$GNGSA", 6) == 0) |
cocorlow | 0:6c7f7ed85f2c | 179 | // { |
cocorlow | 0:6c7f7ed85f2c | 180 | // sscanf(cmd, "$GNGSA,%c,%d,%d", &tf, &fix, &nst); |
cocorlow | 0:6c7f7ed85f2c | 181 | // pc.printf("GNGSA Type fix: %c, 3D fix: %d, number of sat: %d\r\n", tf, fix, nst); |
cocorlow | 0:6c7f7ed85f2c | 182 | // } |
cocorlow | 0:6c7f7ed85f2c | 183 | |
cocorlow | 0:6c7f7ed85f2c | 184 | // else if(strncmp(cmd,"$GPGSV", 6) == 0) |
cocorlow | 0:6c7f7ed85f2c | 185 | // { |
cocorlow | 0:6c7f7ed85f2c | 186 | // // pc.printf("its a Satellite details message.\n"); |
cocorlow | 0:6c7f7ed85f2c | 187 | // } |
cocorlow | 0:6c7f7ed85f2c | 188 | |
cocorlow | 0:6c7f7ed85f2c | 189 | // else if(strncmp(cmd,"$GNGLL", 6) == 0) |
cocorlow | 0:6c7f7ed85f2c | 190 | // { |
cocorlow | 0:6c7f7ed85f2c | 191 | // sscanf(cmd, "$GNGLL,%f,%c,%f,%c,%f", &latitude, &ns, &longitude, &ew, &timefix); |
cocorlow | 0:6c7f7ed85f2c | 192 | // pc.printf("GNGLL Latitude: %f %c, Longitude: %f %c, Fix taken at: %f\n", latitude, ns, longitude, ew, timefix); |
cocorlow | 0:6c7f7ed85f2c | 193 | // } |
cocorlow | 0:6c7f7ed85f2c | 194 | |
cocorlow | 0:6c7f7ed85f2c | 195 | // else |
cocorlow | 0:6c7f7ed85f2c | 196 | // { |
cocorlow | 0:6c7f7ed85f2c | 197 | // pc.printf("Unknown message type\n"); |
cocorlow | 0:6c7f7ed85f2c | 198 | // } |
cocorlow | 0:6c7f7ed85f2c | 199 | } |
cocorlow | 0:6c7f7ed85f2c | 200 | |
cocorlow | 0:6c7f7ed85f2c | 201 | void GPS_HAPS::Punctuate() |
cocorlow | 0:6c7f7ed85f2c | 202 | { |
cocorlow | 0:6c7f7ed85f2c | 203 | } |
cocorlow | 0:6c7f7ed85f2c | 204 | |
cocorlow | 0:6c7f7ed85f2c | 205 | void GPS_HAPS::Attach() |
cocorlow | 0:6c7f7ed85f2c | 206 | { |
cocorlow | 0:6c7f7ed85f2c | 207 | serial.attach(this, &GPS_HAPS::Receive, Serial::RxIrq); |
cocorlow | 0:6c7f7ed85f2c | 208 | } |
cocorlow | 0:6c7f7ed85f2c | 209 | |
cocorlow | 0:6c7f7ed85f2c | 210 | void GPS_HAPS::Update() |
cocorlow | 0:6c7f7ed85f2c | 211 | { |
cocorlow | 0:6c7f7ed85f2c | 212 | } |
cocorlow | 0:6c7f7ed85f2c | 213 | |
cocorlow | 0:6c7f7ed85f2c | 214 | void GPS_HAPS::Initialize() |
cocorlow | 0:6c7f7ed85f2c | 215 | { |
cocorlow | 0:6c7f7ed85f2c | 216 | Satellites = 0; |
cocorlow | 0:6c7f7ed85f2c | 217 | while (Satellites <= 4 || Latitude == 0.0f || Longitude == 0.0f || Elevation == 0.0f) |
cocorlow | 0:6c7f7ed85f2c | 218 | { |
cocorlow | 0:6c7f7ed85f2c | 219 | } |
cocorlow | 4:708e05b10257 | 220 | CalculateUnit(); |
cocorlow | 0:6c7f7ed85f2c | 221 | } |
cocorlow | 0:6c7f7ed85f2c | 222 | |
cocorlow | 0:6c7f7ed85f2c | 223 | Vector3 GPS_HAPS::ToUniversalUnit() |
cocorlow | 0:6c7f7ed85f2c | 224 | { |
cocorlow | 0:6c7f7ed85f2c | 225 | // 東経180度、北緯0度で精度最大 |
cocorlow | 0:6c7f7ed85f2c | 226 | float pi_2_theta = (Latitude * ((N_S == 'N') ? 1.0f : -1.0f)) * M_PI / 180.0f; |
cocorlow | 0:6c7f7ed85f2c | 227 | float pi_phi = ((E_W == 'E') ? Longitude - 180.0f : 180.0f - Longitude) * M_PI / 180.0f; |
cocorlow | 0:6c7f7ed85f2c | 228 | float x = - cosf(pi_2_theta) * cosf(pi_phi); |
cocorlow | 0:6c7f7ed85f2c | 229 | float y = - cosf(pi_2_theta) * sinf(pi_phi); |
cocorlow | 0:6c7f7ed85f2c | 230 | float z = sinf(pi_2_theta); |
cocorlow | 0:6c7f7ed85f2c | 231 | Vector3 v(x, y, z); |
cocorlow | 0:6c7f7ed85f2c | 232 | return v; |
cocorlow | 0:6c7f7ed85f2c | 233 | } |
cocorlow | 0:6c7f7ed85f2c | 234 | |
cocorlow | 0:6c7f7ed85f2c | 235 | Vector3 GPS_HAPS::ToUniversal() |
cocorlow | 0:6c7f7ed85f2c | 236 | { |
cocorlow | 0:6c7f7ed85f2c | 237 | Vector3 v = ToUniversalUnit(); |
cocorlow | 0:6c7f7ed85f2c | 238 | return (Radius + Elevation) * v; |
cocorlow | 0:6c7f7ed85f2c | 239 | } |
cocorlow | 0:6c7f7ed85f2c | 240 | |
cocorlow | 4:708e05b10257 | 241 | void GPS_HAPS::CalculateUnit() |
cocorlow | 0:6c7f7ed85f2c | 242 | { |
cocorlow | 0:6c7f7ed85f2c | 243 | Vector3 _d = -1.0f * ToUniversalUnit(); |
cocorlow | 0:6c7f7ed85f2c | 244 | |
cocorlow | 0:6c7f7ed85f2c | 245 | UniversalZeroPosition = -(Radius+Elevation)*_d; |
cocorlow | 0:6c7f7ed85f2c | 246 | Vector3 _z(0.0f, 0.0f, 1.0f); |
cocorlow | 0:6c7f7ed85f2c | 247 | Vector3 _e = _d * _z; |
cocorlow | 0:6c7f7ed85f2c | 248 | Vector3 _n = _e * _d; |
cocorlow | 0:6c7f7ed85f2c | 249 | UniversalZeroUnitN = _n; |
cocorlow | 0:6c7f7ed85f2c | 250 | UniversalZeroUnitE = _e; |
cocorlow | 0:6c7f7ed85f2c | 251 | UniversalZeroUnitD = _d; |
cocorlow | 0:6c7f7ed85f2c | 252 | } |
cocorlow | 0:6c7f7ed85f2c | 253 | |
cocorlow | 4:708e05b10257 | 254 | void GPS_HAPS::Calculate() |
cocorlow | 0:6c7f7ed85f2c | 255 | { |
cocorlow | 0:6c7f7ed85f2c | 256 | UniversalPosition = ToUniversal(); |
cocorlow | 0:6c7f7ed85f2c | 257 | Vector3 relative = UniversalPosition - UniversalZeroPosition; |
cocorlow | 0:6c7f7ed85f2c | 258 | Vector3 _position(relative % UniversalZeroUnitN, relative % UniversalZeroUnitE, relative % UniversalZeroUnitD); |
cocorlow | 0:6c7f7ed85f2c | 259 | Position = _position; |
cocorlow | 0:6c7f7ed85f2c | 260 | } |
cocorlow | 0:6c7f7ed85f2c | 261 | |
cocorlow | 4:708e05b10257 | 262 | Vector3 GPS_HAPS::Calculate(Vector3 position) |
cocorlow | 0:6c7f7ed85f2c | 263 | { |
cocorlow | 0:6c7f7ed85f2c | 264 | UniversalPosition = ToUniversal(); |
cocorlow | 0:6c7f7ed85f2c | 265 | Vector3 relative = position - UniversalZeroPosition; |
cocorlow | 0:6c7f7ed85f2c | 266 | Vector3 _position(relative % UniversalZeroUnitN, relative % UniversalZeroUnitE, relative % UniversalZeroUnitD); |
cocorlow | 0:6c7f7ed85f2c | 267 | return _position; |
cocorlow | 0:6c7f7ed85f2c | 268 | } |