HAPSRG / GPSUBX_UART

GPS/GNSS UBX library for UART

Dependencies:   Vector3

Dependents:   GPS_0002

GPSUBX_UART.cpp@19:ae1a294cb30b, 2021-12-01 (annotated)

Committer:
NaotoMorita
Date:
Wed Dec 01 05:29:32 2021 +0000
Revision:
19:ae1a294cb30b
Parent:
15:e77382079cd9 
ECEF

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 0:cf7c726ec8a1 26 while (serial.readable())
cocorlow 0:cf7c726ec8a1 27 {
cocorlow 0:cf7c726ec8a1 28 char c;
cocorlow 0:cf7c726ec8a1 29 c = serial.getc();
cocorlow 0:cf7c726ec8a1 30 receive_buffer[receive_index] = c;
cocorlow 1:71f5168e48c8 31 receive_index = (receive_index + 1) % RECEIVE_SIZE;
cocorlow 1:71f5168e48c8 32 }
cocorlow 1:71f5168e48c8 33 }
cocorlow 1:71f5168e48c8 34
cocorlow 1:71f5168e48c8 35
cocorlow 1:71f5168e48c8 36 void GPSUBX_UART::Update()
cocorlow 1:71f5168e48c8 37 {
cocorlow 1:71f5168e48c8 38 volatile static int sentence_start = 0;
cocorlow 1:71f5168e48c8 39 volatile static int sentence_length = 0;
cocorlow 1:71f5168e48c8 40 volatile static int sentence_counter = 0;
cocorlow 1:71f5168e48c8 41 volatile static int read_index = 0;
cocorlow 1:71f5168e48c8 42
cocorlow 1:71f5168e48c8 43 volatile static char m_class = 0x00;
cocorlow 1:71f5168e48c8 44 volatile static char m_id = 0x00;
cocorlow 1:71f5168e48c8 45
cocorlow 1:71f5168e48c8 46 while (read_index != receive_index)
cocorlow 1:71f5168e48c8 47 {
cocorlow 1:71f5168e48c8 48 char c;
cocorlow 1:71f5168e48c8 49 c = receive_buffer[read_index];
cocorlow 0:cf7c726ec8a1 50 if (sentence_counter >= 2)
cocorlow 0:cf7c726ec8a1 51 {
cocorlow 0:cf7c726ec8a1 52 sentence_counter++;
cocorlow 0:cf7c726ec8a1 53 if (sentence_counter == 3)
cocorlow 0:cf7c726ec8a1 54 {
cocorlow 0:cf7c726ec8a1 55 m_class = c;
cocorlow 0:cf7c726ec8a1 56 }
cocorlow 0:cf7c726ec8a1 57 else if (sentence_counter == 4)
cocorlow 0:cf7c726ec8a1 58 {
cocorlow 0:cf7c726ec8a1 59 m_id = c;
cocorlow 0:cf7c726ec8a1 60 }
cocorlow 0:cf7c726ec8a1 61 else if (sentence_counter == 5)
cocorlow 0:cf7c726ec8a1 62 {
cocorlow 0:cf7c726ec8a1 63 }
cocorlow 0:cf7c726ec8a1 64 else if (sentence_counter == 6)
cocorlow 0:cf7c726ec8a1 65 {
cocorlow 1:71f5168e48c8 66 int sss = (read_index+RECEIVE_SIZE-1)%RECEIVE_SIZE;
cocorlow 0:cf7c726ec8a1 67 sentence_length = (int)(c << 8 | receive_buffer[sss]);
cocorlow 0:cf7c726ec8a1 68 }
cocorlow 0:cf7c726ec8a1 69 else if (sentence_counter >= sentence_length+8)
cocorlow 0:cf7c726ec8a1 70 {
cocorlow 0:cf7c726ec8a1 71 for (int i = 0; i < sentence_length+8; i++)
cocorlow 0:cf7c726ec8a1 72 {
cocorlow 0:cf7c726ec8a1 73 sentence_buffer[i] = receive_buffer[(sentence_start+i)%RECEIVE_SIZE];
cocorlow 0:cf7c726ec8a1 74 }
cocorlow 0:cf7c726ec8a1 75 char ca, cb;
cocorlow 0:cf7c726ec8a1 76 Checksum(sentence_buffer, sentence_length, &ca, &cb);
cocorlow 0:cf7c726ec8a1 77 if (ca == sentence_buffer[sentence_length+6] && cb == sentence_buffer[sentence_length+7])
cocorlow 0:cf7c726ec8a1 78 {
cocorlow 0:cf7c726ec8a1 79 Decode(sentence_buffer, m_class, m_id);
cocorlow 0:cf7c726ec8a1 80 }
cocorlow 0:cf7c726ec8a1 81 sentence_start = 0;
cocorlow 0:cf7c726ec8a1 82 sentence_length = 0;
cocorlow 0:cf7c726ec8a1 83 sentence_counter = 0;
cocorlow 0:cf7c726ec8a1 84 m_class = 0x00;
cocorlow 0:cf7c726ec8a1 85 m_id = 0x00;
cocorlow 0:cf7c726ec8a1 86 }
cocorlow 0:cf7c726ec8a1 87 }
cocorlow 0:cf7c726ec8a1 88
cocorlow 1:71f5168e48c8 89 int ss = (read_index+RECEIVE_SIZE-1)%RECEIVE_SIZE;
cocorlow 0:cf7c726ec8a1 90 if (c == 0x62 && receive_buffer[ss] == 0xb5)
cocorlow 0:cf7c726ec8a1 91 {
cocorlow 0:cf7c726ec8a1 92 sentence_start = ss;
cocorlow 0:cf7c726ec8a1 93 sentence_counter = 2;
cocorlow 0:cf7c726ec8a1 94 }
cocorlow 1:71f5168e48c8 95 read_index = (read_index + 1) % RECEIVE_SIZE;
cocorlow 0:cf7c726ec8a1 96 }
cocorlow 0:cf7c726ec8a1 97 }
cocorlow 0:cf7c726ec8a1 98
cocorlow 0:cf7c726ec8a1 99 void GPSUBX_UART::Attach()
cocorlow 0:cf7c726ec8a1 100 {
cocorlow 0:cf7c726ec8a1 101 serial.attach(this, &GPSUBX_UART::Receive, Serial::RxIrq);
cocorlow 0:cf7c726ec8a1 102 }
cocorlow 0:cf7c726ec8a1 103
cocorlow 0:cf7c726ec8a1 104 void GPSUBX_UART::Decode(char buffer[], int mc, int mi)
cocorlow 0:cf7c726ec8a1 105 {
NaotoMorita 19:ae1a294cb30b 106 // POSECEF
NaotoMorita 19:ae1a294cb30b 107 if (mc == 0x01 && mi == 0x01)
NaotoMorita 19:ae1a294cb30b 108 {
NaotoMorita 19:ae1a294cb30b 109 POSECEF posecef;
NaotoMorita 19:ae1a294cb30b 110 for (int i = 0; i < POSECEF_LEN; i++)
NaotoMorita 19:ae1a294cb30b 111 {
NaotoMorita 19:ae1a294cb30b 112 posecef.byte_data[i] = buffer[i+6];
NaotoMorita 19:ae1a294cb30b 113 }
NaotoMorita 19:ae1a294cb30b 114 iTOW_POSECEF = posecef.data.iTOW;
NaotoMorita 19:ae1a294cb30b 115 PositionECEF.x = (float)posecef.data.ecefX/100.0f;
NaotoMorita 19:ae1a294cb30b 116 PositionECEF.y = (float)posecef.data.ecefY/100.0f;
NaotoMorita 19:ae1a294cb30b 117 PositionECEF.z = (float)posecef.data.ecefZ/100.0f;
NaotoMorita 19:ae1a294cb30b 118 // pc.printf("!%d, %f, %f, %f\r\n", iTOW_POSLLH, Longitude, Latitude, Height);
NaotoMorita 19:ae1a294cb30b 119 }
NaotoMorita 19:ae1a294cb30b 120
cocorlow 0:cf7c726ec8a1 121 // POSLLH
cocorlow 0:cf7c726ec8a1 122 if (mc == 0x01 && mi == 0x02)
cocorlow 0:cf7c726ec8a1 123 {
cocorlow 0:cf7c726ec8a1 124 POSLLH posllh;
cocorlow 0:cf7c726ec8a1 125 for (int i = 0; i < POSLLH_LEN; i++)
cocorlow 0:cf7c726ec8a1 126 {
cocorlow 0:cf7c726ec8a1 127 posllh.byte_data[i] = buffer[i+6];
cocorlow 0:cf7c726ec8a1 128 }
cocorlow 0:cf7c726ec8a1 129 iTOW_POSLLH = posllh.data.iTOW;
cocorlow 0:cf7c726ec8a1 130 Longitude = (float)posllh.data.lon * 1e-7f;
cocorlow 0:cf7c726ec8a1 131 Latitude = (float)posllh.data.lat * 1e-7f;
cocorlow 0:cf7c726ec8a1 132 Height = (float)posllh.data.height / 1000.0f;
cocorlow 2:6218fe8e54f4 133 // pc.printf("!%d, %f, %f, %f\r\n", iTOW_POSLLH, Longitude, Latitude, Height);
cocorlow 0:cf7c726ec8a1 134 }
NaotoMorita 13:facd8e54f2eb 135 // STATUS
NaotoMorita 13:facd8e54f2eb 136 if (mc == 0x01 && mi == 0x03)
cocorlow 10:a90d07e4c34d 137 {
NaotoMorita 13:facd8e54f2eb 138 STATUS status;
NaotoMorita 13:facd8e54f2eb 139 for (int i = 0; i < STATUS_LEN; i++)
cocorlow 10:a90d07e4c34d 140 {
NaotoMorita 14:1ed344c662d2 141 status.byte_data[i] = buffer[i+6];
cocorlow 10:a90d07e4c34d 142 }
NaotoMorita 14:1ed344c662d2 143 iTOW_STATUS = status.data.iTOW;
NaotoMorita 14:1ed344c662d2 144 gpsFix = status.data.gpsFix;
cocorlow 10:a90d07e4c34d 145 }
cocorlow 0:cf7c726ec8a1 146 // TIMEUTC
cocorlow 0:cf7c726ec8a1 147 else if (mc == 0x01 && mi == 0x21)
cocorlow 0:cf7c726ec8a1 148 {
cocorlow 0:cf7c726ec8a1 149 TIMEUTC timeutc;
cocorlow 0:cf7c726ec8a1 150 for (int i = 0; i < TIMEUTC_LEN; i++)
cocorlow 0:cf7c726ec8a1 151 {
cocorlow 0:cf7c726ec8a1 152 timeutc.byte_data[i] = buffer[i+6];
cocorlow 0:cf7c726ec8a1 153 }
cocorlow 0:cf7c726ec8a1 154 Year = timeutc.data.year;
cocorlow 0:cf7c726ec8a1 155 Month = timeutc.data.month;
cocorlow 0:cf7c726ec8a1 156 Day = timeutc.data.day;
cocorlow 1:71f5168e48c8 157 Hours = (int)timeutc.data.hour + TimeZone;
cocorlow 0:cf7c726ec8a1 158 if (Hours >= 24)
cocorlow 0:cf7c726ec8a1 159 {
cocorlow 0:cf7c726ec8a1 160 Hours -= 24;
cocorlow 0:cf7c726ec8a1 161 Day += 1;
cocorlow 0:cf7c726ec8a1 162 }
cocorlow 0:cf7c726ec8a1 163 else if (Hours < 0)
cocorlow 0:cf7c726ec8a1 164 {
cocorlow 0:cf7c726ec8a1 165 Hours += 24;
cocorlow 0:cf7c726ec8a1 166 Day -= 1;
cocorlow 0:cf7c726ec8a1 167 }
cocorlow 0:cf7c726ec8a1 168 Minutes = timeutc.data.min;
cocorlow 0:cf7c726ec8a1 169 Seconds = timeutc.data.sec;
cocorlow 2:6218fe8e54f4 170 // pc.printf("&%4d/%2d/%2d %2d:%2d %2d\r\n", Year, Month, Day, Hours, Minutes, Seconds);
cocorlow 0:cf7c726ec8a1 171 }
cocorlow 0:cf7c726ec8a1 172 // VELNED
cocorlow 0:cf7c726ec8a1 173 if (mc == 0x01 && mi == 0x12)
cocorlow 0:cf7c726ec8a1 174 {
cocorlow 0:cf7c726ec8a1 175 VELNED velned;
cocorlow 0:cf7c726ec8a1 176 for (int i = 0; i < VELNED_LEN; i++)
cocorlow 0:cf7c726ec8a1 177 {
cocorlow 0:cf7c726ec8a1 178 velned.byte_data[i] = buffer[i+6];
cocorlow 0:cf7c726ec8a1 179 }
cocorlow 0:cf7c726ec8a1 180 iTOW_VELNED = velned.data.iTOW;
cocorlow 0:cf7c726ec8a1 181 VelocityNED.x = (float)velned.data.velN / 100.0f;
cocorlow 0:cf7c726ec8a1 182 VelocityNED.y = (float)velned.data.velE / 100.0f;
cocorlow 0:cf7c726ec8a1 183 VelocityNED.z = (float)velned.data.velD / 100.0f;
cocorlow 0:cf7c726ec8a1 184 Speed = (float)velned.data.speed / 100.0f;
cocorlow 0:cf7c726ec8a1 185 GroundSpeed = (float)velned.data.gSpeed / 100.0f;
cocorlow 0:cf7c726ec8a1 186 Heading = (float)velned.data.heading * 1e-5f;
cocorlow 2:6218fe8e54f4 187 // pc.printf("#%d, %f, %f, %f\r\n", iTOW_VELNED, VelocityNED.x, VelocityNED.y, VelocityNED.z);
cocorlow 0:cf7c726ec8a1 188 }
cocorlow 0:cf7c726ec8a1 189 }
NaotoMorita 19:ae1a294cb30b 190 /*
cocorlow 0:cf7c726ec8a1 191 Vector3 GPSUBX_UART::ToUniversalUnit()
cocorlow 0:cf7c726ec8a1 192 {
cocorlow 0:cf7c726ec8a1 193 // 東経180度、北緯0度で精度最大
cocorlow 0:cf7c726ec8a1 194 float pi_2_theta = Latitude * M_PI / 180.0f;
cocorlow 0:cf7c726ec8a1 195 float pi_phi = ((Longitude > 0.0f) ? (Longitude - 180.0f) : (Longitude + 180.0f)) * M_PI / 180.0f;
cocorlow 0:cf7c726ec8a1 196 float x = - cosf(pi_2_theta) * cosf(pi_phi);
cocorlow 0:cf7c726ec8a1 197 float y = - cosf(pi_2_theta) * sinf(pi_phi);
cocorlow 0:cf7c726ec8a1 198 float z = sinf(pi_2_theta);
cocorlow 0:cf7c726ec8a1 199 Vector3 v(x, y, z);
NaotoMorita 12:2ffb2fcaac23 200
cocorlow 0:cf7c726ec8a1 201 return v;
cocorlow 0:cf7c726ec8a1 202 }
cocorlow 0:cf7c726ec8a1 203
cocorlow 0:cf7c726ec8a1 204 Vector3 GPSUBX_UART::ToUniversal()
cocorlow 0:cf7c726ec8a1 205 {
cocorlow 0:cf7c726ec8a1 206 Vector3 v = ToUniversalUnit();
cocorlow 0:cf7c726ec8a1 207 return (Radius + Height) * v;
cocorlow 0:cf7c726ec8a1 208 }
NaotoMorita 19:ae1a294cb30b 209 */
cocorlow 0:cf7c726ec8a1 210 void GPSUBX_UART::CalculateUnit()
cocorlow 0:cf7c726ec8a1 211 {
cocorlow 0:cf7c726ec8a1 212
NaotoMorita 19:ae1a294cb30b 213 UniversalZeroPosition = PositionECEF;
NaotoMorita 19:ae1a294cb30b 214 UniversalLatitude = Latitude;
NaotoMorita 19:ae1a294cb30b 215 UniversalLongitude = Longitude;
NaotoMorita 19:ae1a294cb30b 216 float lat = Latitude * M_PI / 180.0f;
NaotoMorita 19:ae1a294cb30b 217 float lon = Longitude * M_PI / 180.0f;
NaotoMorita 19:ae1a294cb30b 218 UniversalZeroUnit1.x = -sinf(lat)*cosf(lon);
NaotoMorita 19:ae1a294cb30b 219 UniversalZeroUnit1.y = -sinf(lat)*sinf(lon);
NaotoMorita 19:ae1a294cb30b 220 UniversalZeroUnit1.z = cosf(lat);
NaotoMorita 19:ae1a294cb30b 221
NaotoMorita 19:ae1a294cb30b 222 UniversalZeroUnit2.x = -sinf(lon);
NaotoMorita 19:ae1a294cb30b 223 UniversalZeroUnit2.y = cosf(lon);
NaotoMorita 19:ae1a294cb30b 224 UniversalZeroUnit2.z = 0.0f;
NaotoMorita 19:ae1a294cb30b 225
NaotoMorita 19:ae1a294cb30b 226 UniversalZeroUnit3.x = -cosf(lat)*cosf(lon);
NaotoMorita 19:ae1a294cb30b 227 UniversalZeroUnit3.y = -cosf(lat)*sinf(lon);
NaotoMorita 19:ae1a294cb30b 228 UniversalZeroUnit3.z = -sinf(lat);
NaotoMorita 19:ae1a294cb30b 229
NaotoMorita 12:2ffb2fcaac23 230 //twelite.printf("%f %f %f \r\n",_n,_e,_d);
cocorlow 0:cf7c726ec8a1 231 }
cocorlow 0:cf7c726ec8a1 232
cocorlow 0:cf7c726ec8a1 233 void GPSUBX_UART::Calculate()
cocorlow 0:cf7c726ec8a1 234 {
NaotoMorita 19:ae1a294cb30b 235 Vector3 relative = PositionECEF - UniversalZeroPosition;
NaotoMorita 19:ae1a294cb30b 236 Vector3 _position(relative % UniversalZeroUnit1, relative % UniversalZeroUnit2, relative % UniversalZeroUnit3);
cocorlow 0:cf7c726ec8a1 237 PositionNED = _position;
cocorlow 0:cf7c726ec8a1 238 }