HAPSRG / GPSUBX_UART

GPS/GNSS UBX library for UART

Dependencies:   Vector3

Dependents:   GPS_0002

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?

UserRevisionLine numberNew 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 }