GPS/GNSS UBX library for UART
Dependencies: Vector3
GPSUBX_UART.cpp@1:71f5168e48c8, 2021-09-17 (annotated)
- Committer:
- cocorlow
- Date:
- Fri Sep 17 15:06:47 2021 +0000
- Revision:
- 1:71f5168e48c8
- Parent:
- 0:cf7c726ec8a1
- Child:
- 2:6218fe8e54f4
work!
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
cocorlow | 0:cf7c726ec8a1 | 1 | #include "GPSUBX_UART.hpp" |
cocorlow | 0:cf7c726ec8a1 | 2 | #include "mbed.h" |
cocorlow | 0:cf7c726ec8a1 | 3 | #include <math.h> |
cocorlow | 0:cf7c726ec8a1 | 4 | #define M_PI 3.14159265358979f |
cocorlow | 0:cf7c726ec8a1 | 5 | |
cocorlow | 1:71f5168e48c8 | 6 | GPSUBX_UART::GPSUBX_UART(PinName tx, PinName rx, int baud, int timezone) |
cocorlow | 1:71f5168e48c8 | 7 | :serial(tx, rx, baud), TimeZone(timezone), receive_index(0) |
cocorlow | 0:cf7c726ec8a1 | 8 | { |
cocorlow | 0:cf7c726ec8a1 | 9 | } |
cocorlow | 0:cf7c726ec8a1 | 10 | |
cocorlow | 0:cf7c726ec8a1 | 11 | void GPSUBX_UART::Checksum(char payload[], int n, char* ck_a, char* ck_b) |
cocorlow | 0:cf7c726ec8a1 | 12 | { |
cocorlow | 0:cf7c726ec8a1 | 13 | int ca = 0; |
cocorlow | 0:cf7c726ec8a1 | 14 | int cb = 0; |
cocorlow | 0:cf7c726ec8a1 | 15 | for (int i = 0; i < n+4; i++) |
cocorlow | 0:cf7c726ec8a1 | 16 | { |
cocorlow | 0:cf7c726ec8a1 | 17 | ca += (unsigned char)payload[i+2]; |
cocorlow | 0:cf7c726ec8a1 | 18 | cb += ca; |
cocorlow | 0:cf7c726ec8a1 | 19 | } |
cocorlow | 0:cf7c726ec8a1 | 20 | *ck_a = (char)(ca & 0xff); |
cocorlow | 0:cf7c726ec8a1 | 21 | *ck_b = (char)(cb & 0xff); |
cocorlow | 0:cf7c726ec8a1 | 22 | } |
cocorlow | 0:cf7c726ec8a1 | 23 | |
cocorlow | 0:cf7c726ec8a1 | 24 | void GPSUBX_UART::Receive() |
cocorlow | 0:cf7c726ec8a1 | 25 | { |
cocorlow | 1:71f5168e48c8 | 26 | // volatile static int receive_start = 0; |
cocorlow | 1:71f5168e48c8 | 27 | // volatile static int receive_index = 0; |
cocorlow | 1:71f5168e48c8 | 28 | // volatile static int sentence_start = 0; |
cocorlow | 1:71f5168e48c8 | 29 | // volatile static int sentence_length = 0; |
cocorlow | 1:71f5168e48c8 | 30 | // volatile static int sentence_counter = 0; |
cocorlow | 1:71f5168e48c8 | 31 | // |
cocorlow | 1:71f5168e48c8 | 32 | // volatile static char m_class = 0x00; |
cocorlow | 1:71f5168e48c8 | 33 | // volatile static char m_id = 0x00; |
cocorlow | 0:cf7c726ec8a1 | 34 | |
cocorlow | 0:cf7c726ec8a1 | 35 | while (serial.readable()) |
cocorlow | 0:cf7c726ec8a1 | 36 | { |
cocorlow | 0:cf7c726ec8a1 | 37 | char c; |
cocorlow | 0:cf7c726ec8a1 | 38 | c = serial.getc(); |
cocorlow | 0:cf7c726ec8a1 | 39 | receive_buffer[receive_index] = c; |
cocorlow | 1:71f5168e48c8 | 40 | //// pc.printf("%d\r\n", c); |
cocorlow | 1:71f5168e48c8 | 41 | // if (sentence_counter >= 2) |
cocorlow | 1:71f5168e48c8 | 42 | // { |
cocorlow | 1:71f5168e48c8 | 43 | // sentence_counter++; |
cocorlow | 1:71f5168e48c8 | 44 | // if (sentence_counter == 3) |
cocorlow | 1:71f5168e48c8 | 45 | // { |
cocorlow | 1:71f5168e48c8 | 46 | // m_class = c; |
cocorlow | 1:71f5168e48c8 | 47 | //// pc.printf("@%x\r\n", m_class); |
cocorlow | 1:71f5168e48c8 | 48 | // } |
cocorlow | 1:71f5168e48c8 | 49 | // else if (sentence_counter == 4) |
cocorlow | 1:71f5168e48c8 | 50 | // { |
cocorlow | 1:71f5168e48c8 | 51 | // m_id = c; |
cocorlow | 1:71f5168e48c8 | 52 | //// pc.printf("@@%x\r\n", m_id); |
cocorlow | 1:71f5168e48c8 | 53 | // } |
cocorlow | 1:71f5168e48c8 | 54 | // else if (sentence_counter == 5) |
cocorlow | 1:71f5168e48c8 | 55 | // { |
cocorlow | 1:71f5168e48c8 | 56 | // } |
cocorlow | 1:71f5168e48c8 | 57 | // else if (sentence_counter == 6) |
cocorlow | 1:71f5168e48c8 | 58 | // { |
cocorlow | 1:71f5168e48c8 | 59 | // int sss = (receive_index+RECEIVE_SIZE-1)%RECEIVE_SIZE; |
cocorlow | 1:71f5168e48c8 | 60 | // sentence_length = (int)(c << 8 | receive_buffer[sss]); |
cocorlow | 1:71f5168e48c8 | 61 | //// printf("%d\r\n", sentence_length); |
cocorlow | 1:71f5168e48c8 | 62 | // } |
cocorlow | 1:71f5168e48c8 | 63 | // else if (sentence_counter >= sentence_length+8) |
cocorlow | 1:71f5168e48c8 | 64 | // { |
cocorlow | 1:71f5168e48c8 | 65 | // for (int i = 0; i < sentence_length+8; i++) |
cocorlow | 1:71f5168e48c8 | 66 | // { |
cocorlow | 1:71f5168e48c8 | 67 | // sentence_buffer[i] = receive_buffer[(sentence_start+i)%RECEIVE_SIZE]; |
cocorlow | 1:71f5168e48c8 | 68 | // } |
cocorlow | 1:71f5168e48c8 | 69 | //// for (int i = 0; i < sentence_length+8; i++) |
cocorlow | 1:71f5168e48c8 | 70 | //// { |
cocorlow | 1:71f5168e48c8 | 71 | //// pc.printf("%x ", sentence_buffer[i]); |
cocorlow | 1:71f5168e48c8 | 72 | //// } |
cocorlow | 1:71f5168e48c8 | 73 | //// pc.printf("\r\n"); |
cocorlow | 1:71f5168e48c8 | 74 | // char ca, cb; |
cocorlow | 1:71f5168e48c8 | 75 | // Checksum(sentence_buffer, sentence_length, &ca, &cb); |
cocorlow | 1:71f5168e48c8 | 76 | //// pc.printf("^%x %x\r\n", m_class, m_id); |
cocorlow | 1:71f5168e48c8 | 77 | // if (ca == sentence_buffer[sentence_length+6] && cb == sentence_buffer[sentence_length+7]) |
cocorlow | 1:71f5168e48c8 | 78 | // { |
cocorlow | 1:71f5168e48c8 | 79 | //// pc.printf("=%x %x\r\n", m_class, m_id); |
cocorlow | 1:71f5168e48c8 | 80 | // Decode(sentence_buffer, m_class, m_id); |
cocorlow | 1:71f5168e48c8 | 81 | // } |
cocorlow | 1:71f5168e48c8 | 82 | // sentence_start = 0; |
cocorlow | 1:71f5168e48c8 | 83 | // sentence_length = 0; |
cocorlow | 1:71f5168e48c8 | 84 | // sentence_counter = 0; |
cocorlow | 1:71f5168e48c8 | 85 | // m_class = 0x00; |
cocorlow | 1:71f5168e48c8 | 86 | // m_id = 0x00; |
cocorlow | 1:71f5168e48c8 | 87 | // } |
cocorlow | 1:71f5168e48c8 | 88 | // } |
cocorlow | 1:71f5168e48c8 | 89 | // |
cocorlow | 1:71f5168e48c8 | 90 | // int ss = (receive_index+RECEIVE_SIZE-1)%RECEIVE_SIZE; |
cocorlow | 1:71f5168e48c8 | 91 | // if (c == 0x62 && receive_buffer[ss] == 0xb5) |
cocorlow | 1:71f5168e48c8 | 92 | // { |
cocorlow | 1:71f5168e48c8 | 93 | // sentence_start = ss; |
cocorlow | 1:71f5168e48c8 | 94 | // sentence_counter = 2; |
cocorlow | 1:71f5168e48c8 | 95 | // } |
cocorlow | 1:71f5168e48c8 | 96 | receive_index = (receive_index + 1) % RECEIVE_SIZE; |
cocorlow | 1:71f5168e48c8 | 97 | } |
cocorlow | 1:71f5168e48c8 | 98 | } |
cocorlow | 1:71f5168e48c8 | 99 | |
cocorlow | 1:71f5168e48c8 | 100 | |
cocorlow | 1:71f5168e48c8 | 101 | void GPSUBX_UART::Update() |
cocorlow | 1:71f5168e48c8 | 102 | { |
cocorlow | 1:71f5168e48c8 | 103 | // volatile static int receive_start = 0; |
cocorlow | 1:71f5168e48c8 | 104 | // volatile static int receive_index = 0; |
cocorlow | 1:71f5168e48c8 | 105 | volatile static int sentence_start = 0; |
cocorlow | 1:71f5168e48c8 | 106 | volatile static int sentence_length = 0; |
cocorlow | 1:71f5168e48c8 | 107 | volatile static int sentence_counter = 0; |
cocorlow | 1:71f5168e48c8 | 108 | volatile static int read_index = 0; |
cocorlow | 1:71f5168e48c8 | 109 | |
cocorlow | 1:71f5168e48c8 | 110 | volatile static char m_class = 0x00; |
cocorlow | 1:71f5168e48c8 | 111 | volatile static char m_id = 0x00; |
cocorlow | 1:71f5168e48c8 | 112 | |
cocorlow | 1:71f5168e48c8 | 113 | while (read_index != receive_index) |
cocorlow | 1:71f5168e48c8 | 114 | { |
cocorlow | 1:71f5168e48c8 | 115 | char c; |
cocorlow | 1:71f5168e48c8 | 116 | c = receive_buffer[read_index]; |
cocorlow | 0:cf7c726ec8a1 | 117 | if (sentence_counter >= 2) |
cocorlow | 0:cf7c726ec8a1 | 118 | { |
cocorlow | 0:cf7c726ec8a1 | 119 | sentence_counter++; |
cocorlow | 0:cf7c726ec8a1 | 120 | if (sentence_counter == 3) |
cocorlow | 0:cf7c726ec8a1 | 121 | { |
cocorlow | 0:cf7c726ec8a1 | 122 | m_class = c; |
cocorlow | 0:cf7c726ec8a1 | 123 | // pc.printf("@%x\r\n", m_class); |
cocorlow | 0:cf7c726ec8a1 | 124 | } |
cocorlow | 0:cf7c726ec8a1 | 125 | else if (sentence_counter == 4) |
cocorlow | 0:cf7c726ec8a1 | 126 | { |
cocorlow | 0:cf7c726ec8a1 | 127 | m_id = c; |
cocorlow | 0:cf7c726ec8a1 | 128 | // pc.printf("@@%x\r\n", m_id); |
cocorlow | 0:cf7c726ec8a1 | 129 | } |
cocorlow | 0:cf7c726ec8a1 | 130 | else if (sentence_counter == 5) |
cocorlow | 0:cf7c726ec8a1 | 131 | { |
cocorlow | 0:cf7c726ec8a1 | 132 | } |
cocorlow | 0:cf7c726ec8a1 | 133 | else if (sentence_counter == 6) |
cocorlow | 0:cf7c726ec8a1 | 134 | { |
cocorlow | 1:71f5168e48c8 | 135 | int sss = (read_index+RECEIVE_SIZE-1)%RECEIVE_SIZE; |
cocorlow | 0:cf7c726ec8a1 | 136 | sentence_length = (int)(c << 8 | receive_buffer[sss]); |
cocorlow | 0:cf7c726ec8a1 | 137 | // printf("%d\r\n", sentence_length); |
cocorlow | 0:cf7c726ec8a1 | 138 | } |
cocorlow | 0:cf7c726ec8a1 | 139 | else if (sentence_counter >= sentence_length+8) |
cocorlow | 0:cf7c726ec8a1 | 140 | { |
cocorlow | 0:cf7c726ec8a1 | 141 | for (int i = 0; i < sentence_length+8; i++) |
cocorlow | 0:cf7c726ec8a1 | 142 | { |
cocorlow | 0:cf7c726ec8a1 | 143 | sentence_buffer[i] = receive_buffer[(sentence_start+i)%RECEIVE_SIZE]; |
cocorlow | 0:cf7c726ec8a1 | 144 | } |
cocorlow | 0:cf7c726ec8a1 | 145 | // for (int i = 0; i < sentence_length+8; i++) |
cocorlow | 0:cf7c726ec8a1 | 146 | // { |
cocorlow | 0:cf7c726ec8a1 | 147 | // pc.printf("%x ", sentence_buffer[i]); |
cocorlow | 0:cf7c726ec8a1 | 148 | // } |
cocorlow | 0:cf7c726ec8a1 | 149 | // pc.printf("\r\n"); |
cocorlow | 0:cf7c726ec8a1 | 150 | char ca, cb; |
cocorlow | 0:cf7c726ec8a1 | 151 | Checksum(sentence_buffer, sentence_length, &ca, &cb); |
cocorlow | 0:cf7c726ec8a1 | 152 | // pc.printf("^%x %x\r\n", m_class, m_id); |
cocorlow | 0:cf7c726ec8a1 | 153 | if (ca == sentence_buffer[sentence_length+6] && cb == sentence_buffer[sentence_length+7]) |
cocorlow | 0:cf7c726ec8a1 | 154 | { |
cocorlow | 0:cf7c726ec8a1 | 155 | // pc.printf("=%x %x\r\n", m_class, m_id); |
cocorlow | 0:cf7c726ec8a1 | 156 | Decode(sentence_buffer, m_class, m_id); |
cocorlow | 0:cf7c726ec8a1 | 157 | } |
cocorlow | 0:cf7c726ec8a1 | 158 | sentence_start = 0; |
cocorlow | 0:cf7c726ec8a1 | 159 | sentence_length = 0; |
cocorlow | 0:cf7c726ec8a1 | 160 | sentence_counter = 0; |
cocorlow | 0:cf7c726ec8a1 | 161 | m_class = 0x00; |
cocorlow | 0:cf7c726ec8a1 | 162 | m_id = 0x00; |
cocorlow | 0:cf7c726ec8a1 | 163 | } |
cocorlow | 0:cf7c726ec8a1 | 164 | } |
cocorlow | 0:cf7c726ec8a1 | 165 | |
cocorlow | 1:71f5168e48c8 | 166 | int ss = (read_index+RECEIVE_SIZE-1)%RECEIVE_SIZE; |
cocorlow | 0:cf7c726ec8a1 | 167 | if (c == 0x62 && receive_buffer[ss] == 0xb5) |
cocorlow | 0:cf7c726ec8a1 | 168 | { |
cocorlow | 0:cf7c726ec8a1 | 169 | sentence_start = ss; |
cocorlow | 0:cf7c726ec8a1 | 170 | sentence_counter = 2; |
cocorlow | 0:cf7c726ec8a1 | 171 | } |
cocorlow | 1:71f5168e48c8 | 172 | read_index = (read_index + 1) % RECEIVE_SIZE; |
cocorlow | 0:cf7c726ec8a1 | 173 | } |
cocorlow | 0:cf7c726ec8a1 | 174 | } |
cocorlow | 0:cf7c726ec8a1 | 175 | |
cocorlow | 0:cf7c726ec8a1 | 176 | void GPSUBX_UART::Attach() |
cocorlow | 0:cf7c726ec8a1 | 177 | { |
cocorlow | 0:cf7c726ec8a1 | 178 | serial.attach(this, &GPSUBX_UART::Receive, Serial::RxIrq); |
cocorlow | 0:cf7c726ec8a1 | 179 | } |
cocorlow | 0:cf7c726ec8a1 | 180 | |
cocorlow | 0:cf7c726ec8a1 | 181 | void GPSUBX_UART::Decode(char buffer[], int mc, int mi) |
cocorlow | 0:cf7c726ec8a1 | 182 | { |
cocorlow | 0:cf7c726ec8a1 | 183 | // POSLLH |
cocorlow | 0:cf7c726ec8a1 | 184 | if (mc == 0x01 && mi == 0x02) |
cocorlow | 0:cf7c726ec8a1 | 185 | { |
cocorlow | 0:cf7c726ec8a1 | 186 | POSLLH posllh; |
cocorlow | 0:cf7c726ec8a1 | 187 | for (int i = 0; i < POSLLH_LEN; i++) |
cocorlow | 0:cf7c726ec8a1 | 188 | { |
cocorlow | 0:cf7c726ec8a1 | 189 | posllh.byte_data[i] = buffer[i+6]; |
cocorlow | 0:cf7c726ec8a1 | 190 | } |
cocorlow | 0:cf7c726ec8a1 | 191 | iTOW_POSLLH = posllh.data.iTOW; |
cocorlow | 0:cf7c726ec8a1 | 192 | Longitude = (float)posllh.data.lon * 1e-7f; |
cocorlow | 0:cf7c726ec8a1 | 193 | Latitude = (float)posllh.data.lat * 1e-7f; |
cocorlow | 0:cf7c726ec8a1 | 194 | Height = (float)posllh.data.height / 1000.0f; |
cocorlow | 0:cf7c726ec8a1 | 195 | pc.printf("!%d, %f, %f, %f\r\n", iTOW_POSLLH, Longitude, Latitude, Height); |
cocorlow | 0:cf7c726ec8a1 | 196 | } |
cocorlow | 0:cf7c726ec8a1 | 197 | // TIMEUTC |
cocorlow | 0:cf7c726ec8a1 | 198 | else if (mc == 0x01 && mi == 0x21) |
cocorlow | 0:cf7c726ec8a1 | 199 | { |
cocorlow | 0:cf7c726ec8a1 | 200 | TIMEUTC timeutc; |
cocorlow | 0:cf7c726ec8a1 | 201 | for (int i = 0; i < TIMEUTC_LEN; i++) |
cocorlow | 0:cf7c726ec8a1 | 202 | { |
cocorlow | 0:cf7c726ec8a1 | 203 | timeutc.byte_data[i] = buffer[i+6]; |
cocorlow | 0:cf7c726ec8a1 | 204 | } |
cocorlow | 0:cf7c726ec8a1 | 205 | Year = timeutc.data.year; |
cocorlow | 0:cf7c726ec8a1 | 206 | Month = timeutc.data.month; |
cocorlow | 0:cf7c726ec8a1 | 207 | Day = timeutc.data.day; |
cocorlow | 1:71f5168e48c8 | 208 | Hours = (int)timeutc.data.hour + TimeZone; |
cocorlow | 0:cf7c726ec8a1 | 209 | if (Hours >= 24) |
cocorlow | 0:cf7c726ec8a1 | 210 | { |
cocorlow | 0:cf7c726ec8a1 | 211 | Hours -= 24; |
cocorlow | 0:cf7c726ec8a1 | 212 | Day += 1; |
cocorlow | 0:cf7c726ec8a1 | 213 | } |
cocorlow | 0:cf7c726ec8a1 | 214 | else if (Hours < 0) |
cocorlow | 0:cf7c726ec8a1 | 215 | { |
cocorlow | 0:cf7c726ec8a1 | 216 | Hours += 24; |
cocorlow | 0:cf7c726ec8a1 | 217 | Day -= 1; |
cocorlow | 0:cf7c726ec8a1 | 218 | } |
cocorlow | 0:cf7c726ec8a1 | 219 | Minutes = timeutc.data.min; |
cocorlow | 0:cf7c726ec8a1 | 220 | Seconds = timeutc.data.sec; |
cocorlow | 0:cf7c726ec8a1 | 221 | pc.printf("&%4d/%2d/%2d %2d:%2d %2d\r\n", Year, Month, Day, Hours, Minutes, Seconds); |
cocorlow | 0:cf7c726ec8a1 | 222 | } |
cocorlow | 0:cf7c726ec8a1 | 223 | // VELNED |
cocorlow | 0:cf7c726ec8a1 | 224 | if (mc == 0x01 && mi == 0x12) |
cocorlow | 0:cf7c726ec8a1 | 225 | { |
cocorlow | 0:cf7c726ec8a1 | 226 | VELNED velned; |
cocorlow | 0:cf7c726ec8a1 | 227 | for (int i = 0; i < VELNED_LEN; i++) |
cocorlow | 0:cf7c726ec8a1 | 228 | { |
cocorlow | 0:cf7c726ec8a1 | 229 | velned.byte_data[i] = buffer[i+6]; |
cocorlow | 0:cf7c726ec8a1 | 230 | } |
cocorlow | 0:cf7c726ec8a1 | 231 | iTOW_VELNED = velned.data.iTOW; |
cocorlow | 0:cf7c726ec8a1 | 232 | VelocityNED.x = (float)velned.data.velN / 100.0f; |
cocorlow | 0:cf7c726ec8a1 | 233 | VelocityNED.y = (float)velned.data.velE / 100.0f; |
cocorlow | 0:cf7c726ec8a1 | 234 | VelocityNED.z = (float)velned.data.velD / 100.0f; |
cocorlow | 0:cf7c726ec8a1 | 235 | Speed = (float)velned.data.speed / 100.0f; |
cocorlow | 0:cf7c726ec8a1 | 236 | GroundSpeed = (float)velned.data.gSpeed / 100.0f; |
cocorlow | 0:cf7c726ec8a1 | 237 | Heading = (float)velned.data.heading * 1e-5f; |
cocorlow | 0:cf7c726ec8a1 | 238 | pc.printf("#%d, %f, %f, %f\r\n", iTOW_VELNED, VelocityNED.x, VelocityNED.y, VelocityNED.z); |
cocorlow | 0:cf7c726ec8a1 | 239 | } |
cocorlow | 0:cf7c726ec8a1 | 240 | } |
cocorlow | 0:cf7c726ec8a1 | 241 | |
cocorlow | 0:cf7c726ec8a1 | 242 | Vector3 GPSUBX_UART::ToUniversalUnit() |
cocorlow | 0:cf7c726ec8a1 | 243 | { |
cocorlow | 0:cf7c726ec8a1 | 244 | // 東経180度、北緯0度で精度最大 |
cocorlow | 0:cf7c726ec8a1 | 245 | float pi_2_theta = Latitude * M_PI / 180.0f; |
cocorlow | 0:cf7c726ec8a1 | 246 | float pi_phi = ((Longitude > 0.0f) ? (Longitude - 180.0f) : (Longitude + 180.0f)) * M_PI / 180.0f; |
cocorlow | 0:cf7c726ec8a1 | 247 | float x = - cosf(pi_2_theta) * cosf(pi_phi); |
cocorlow | 0:cf7c726ec8a1 | 248 | float y = - cosf(pi_2_theta) * sinf(pi_phi); |
cocorlow | 0:cf7c726ec8a1 | 249 | float z = sinf(pi_2_theta); |
cocorlow | 0:cf7c726ec8a1 | 250 | Vector3 v(x, y, z); |
cocorlow | 0:cf7c726ec8a1 | 251 | return v; |
cocorlow | 0:cf7c726ec8a1 | 252 | } |
cocorlow | 0:cf7c726ec8a1 | 253 | |
cocorlow | 0:cf7c726ec8a1 | 254 | Vector3 GPSUBX_UART::ToUniversal() |
cocorlow | 0:cf7c726ec8a1 | 255 | { |
cocorlow | 0:cf7c726ec8a1 | 256 | Vector3 v = ToUniversalUnit(); |
cocorlow | 0:cf7c726ec8a1 | 257 | return (Radius + Height) * v; |
cocorlow | 0:cf7c726ec8a1 | 258 | } |
cocorlow | 0:cf7c726ec8a1 | 259 | |
cocorlow | 0:cf7c726ec8a1 | 260 | void GPSUBX_UART::CalculateUnit() |
cocorlow | 0:cf7c726ec8a1 | 261 | { |
cocorlow | 0:cf7c726ec8a1 | 262 | Vector3 _d = -1.0f * ToUniversalUnit(); |
cocorlow | 0:cf7c726ec8a1 | 263 | |
cocorlow | 0:cf7c726ec8a1 | 264 | UniversalZeroPosition = -(Radius+Height)*_d; |
cocorlow | 0:cf7c726ec8a1 | 265 | Vector3 _z(0.0f, 0.0f, 1.0f); |
cocorlow | 0:cf7c726ec8a1 | 266 | Vector3 _e = _d * _z; |
cocorlow | 0:cf7c726ec8a1 | 267 | Vector3 _n = _e * _d; |
cocorlow | 0:cf7c726ec8a1 | 268 | UniversalZeroUnitN = _n; |
cocorlow | 0:cf7c726ec8a1 | 269 | UniversalZeroUnitE = _e; |
cocorlow | 0:cf7c726ec8a1 | 270 | UniversalZeroUnitD = _d; |
cocorlow | 0:cf7c726ec8a1 | 271 | } |
cocorlow | 0:cf7c726ec8a1 | 272 | |
cocorlow | 0:cf7c726ec8a1 | 273 | void GPSUBX_UART::Calculate() |
cocorlow | 0:cf7c726ec8a1 | 274 | { |
cocorlow | 0:cf7c726ec8a1 | 275 | UniversalPosition = ToUniversal(); |
cocorlow | 0:cf7c726ec8a1 | 276 | Vector3 relative = UniversalPosition - UniversalZeroPosition; |
cocorlow | 0:cf7c726ec8a1 | 277 | Vector3 _position(relative % UniversalZeroUnitN, relative % UniversalZeroUnitE, relative % UniversalZeroUnitD); |
cocorlow | 0:cf7c726ec8a1 | 278 | PositionNED = _position; |
cocorlow | 0:cf7c726ec8a1 | 279 | } |
cocorlow | 0:cf7c726ec8a1 | 280 | |
cocorlow | 0:cf7c726ec8a1 | 281 | Vector3 GPSUBX_UART::Calculate(Vector3 position) |
cocorlow | 0:cf7c726ec8a1 | 282 | { |
cocorlow | 0:cf7c726ec8a1 | 283 | UniversalPosition = ToUniversal(); |
cocorlow | 0:cf7c726ec8a1 | 284 | Vector3 relative = position - UniversalZeroPosition; |
cocorlow | 0:cf7c726ec8a1 | 285 | Vector3 _position(relative % UniversalZeroUnitN, relative % UniversalZeroUnitE, relative % UniversalZeroUnitD); |
cocorlow | 0:cf7c726ec8a1 | 286 | return _position; |
cocorlow | 0:cf7c726ec8a1 | 287 | } |