HAPSRG / GPSUBX_UART

GPS/GNSS UBX library for UART

Dependencies:   Vector3

Dependents:   GPS_0002

GPSUBX_UART.cpp@17:2e6147aeba54, 2021-12-10 (annotated)

Committer:
cocorlow
Date:
Fri Dec 10 10:41:56 2021 +0000
Revision:
17:2e6147aeba54
Parent:
16:8181909e88f8 
Eigen Dense

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