HAPSRG
GPS/GNSS UBX library for UART
Dependencies: Vector3
Diff: GPSUBX_UART.cpp
- Revision:
- 0:cf7c726ec8a1
- Child:
- 1:71f5168e48c8
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/GPSUBX_UART.cpp Mon Sep 13 16:04:40 2021 +0000
@@ -0,0 +1,211 @@
+#include "GPSUBX_UART.hpp"
+#include "mbed.h"
+#include <math.h>
+#define M_PI 3.14159265358979f
+
+GPSUBX_UART::GPSUBX_UART(PinName tx, PinName rx, int baud, int timezone = +9)
+ :serial(tx, rx, baud), TimeZone(timezone)
+{
+}
+
+void GPSUBX_UART::Checksum(char payload[], int n, char* ck_a, char* ck_b)
+{
+ int ca = 0;
+ int cb = 0;
+ for (int i = 0; i < n+4; i++)
+ {
+ ca += (unsigned char)payload[i+2];
+ cb += ca;
+ }
+ *ck_a = (char)(ca & 0xff);
+ *ck_b = (char)(cb & 0xff);
+}
+
+void GPSUBX_UART::Receive()
+{
+ volatile static int receive_start = 0;
+ volatile static int receive_index = 0;
+ volatile static int sentence_start = 0;
+ volatile static int sentence_length = 0;
+ volatile static int sentence_counter = 0;
+
+ volatile static char m_class = 0x00;
+ volatile static char m_id = 0x00;
+
+ while (serial.readable())
+ {
+ char c;
+ c = serial.getc();
+ receive_buffer[receive_index] = c;
+// pc.printf("%d\r\n", c);
+ if (sentence_counter >= 2)
+ {
+ sentence_counter++;
+ if (sentence_counter == 3)
+ {
+ m_class = c;
+// pc.printf("@%x\r\n", m_class);
+ }
+ else if (sentence_counter == 4)
+ {
+ m_id = c;
+// pc.printf("@@%x\r\n", m_id);
+ }
+ else if (sentence_counter == 5)
+ {
+ }
+ else if (sentence_counter == 6)
+ {
+ int sss = (receive_index+RECEIVE_SIZE-1)%RECEIVE_SIZE;
+ sentence_length = (int)(c << 8 | receive_buffer[sss]);
+// printf("%d\r\n", sentence_length);
+ }
+ else if (sentence_counter >= sentence_length+8)
+ {
+ for (int i = 0; i < sentence_length+8; i++)
+ {
+ sentence_buffer[i] = receive_buffer[(sentence_start+i)%RECEIVE_SIZE];
+ }
+// for (int i = 0; i < sentence_length+8; i++)
+// {
+// pc.printf("%x ", sentence_buffer[i]);
+// }
+// pc.printf("\r\n");
+ char ca, cb;
+ Checksum(sentence_buffer, sentence_length, &ca, &cb);
+// pc.printf("^%x %x\r\n", m_class, m_id);
+ if (ca == sentence_buffer[sentence_length+6] && cb == sentence_buffer[sentence_length+7])
+ {
+// pc.printf("=%x %x\r\n", m_class, m_id);
+ Decode(sentence_buffer, m_class, m_id);
+ }
+ sentence_start = 0;
+ sentence_length = 0;
+ sentence_counter = 0;
+ m_class = 0x00;
+ m_id = 0x00;
+ }
+ }
+
+ int ss = (receive_index+RECEIVE_SIZE-1)%RECEIVE_SIZE;
+ if (c == 0x62 && receive_buffer[ss] == 0xb5)
+ {
+ sentence_start = ss;
+ sentence_counter = 2;
+ }
+ receive_index = (receive_index + 1) % RECEIVE_SIZE;
+ }
+}
+
+void GPSUBX_UART::Attach()
+{
+ serial.attach(this, &GPSUBX_UART::Receive, Serial::RxIrq);
+}
+
+void GPSUBX_UART::Decode(char buffer[], int mc, int mi)
+{
+ // POSLLH
+ if (mc == 0x01 && mi == 0x02)
+ {
+ POSLLH posllh;
+ for (int i = 0; i < POSLLH_LEN; i++)
+ {
+ posllh.byte_data[i] = buffer[i+6];
+ }
+ iTOW_POSLLH = posllh.data.iTOW;
+ Longitude = (float)posllh.data.lon * 1e-7f;
+ Latitude = (float)posllh.data.lat * 1e-7f;
+ Height = (float)posllh.data.height / 1000.0f;
+ pc.printf("!%d, %f, %f, %f\r\n", iTOW_POSLLH, Longitude, Latitude, Height);
+ }
+ // TIMEUTC
+ else if (mc == 0x01 && mi == 0x21)
+ {
+ TIMEUTC timeutc;
+ for (int i = 0; i < TIMEUTC_LEN; i++)
+ {
+ timeutc.byte_data[i] = buffer[i+6];
+ }
+ Year = timeutc.data.year;
+ Month = timeutc.data.month;
+ Day = timeutc.data.day;
+ Hours = timeutc.data.hour + TimeZone;
+ if (Hours >= 24)
+ {
+ Hours -= 24;
+ Day += 1;
+ }
+ else if (Hours < 0)
+ {
+ Hours += 24;
+ Day -= 1;
+ }
+ Minutes = timeutc.data.min;
+ Seconds = timeutc.data.sec;
+ pc.printf("&%4d/%2d/%2d %2d:%2d %2d\r\n", Year, Month, Day, Hours, Minutes, Seconds);
+ }
+ // VELNED
+ if (mc == 0x01 && mi == 0x12)
+ {
+ VELNED velned;
+ for (int i = 0; i < VELNED_LEN; i++)
+ {
+ velned.byte_data[i] = buffer[i+6];
+ }
+ iTOW_VELNED = velned.data.iTOW;
+ VelocityNED.x = (float)velned.data.velN / 100.0f;
+ VelocityNED.y = (float)velned.data.velE / 100.0f;
+ VelocityNED.z = (float)velned.data.velD / 100.0f;
+ Speed = (float)velned.data.speed / 100.0f;
+ GroundSpeed = (float)velned.data.gSpeed / 100.0f;
+ Heading = (float)velned.data.heading * 1e-5f;
+ pc.printf("#%d, %f, %f, %f\r\n", iTOW_VELNED, VelocityNED.x, VelocityNED.y, VelocityNED.z);
+ }
+}
+
+Vector3 GPSUBX_UART::ToUniversalUnit()
+{
+ // 東経180度、北緯0度で精度最大
+ float pi_2_theta = Latitude * M_PI / 180.0f;
+ float pi_phi = ((Longitude > 0.0f) ? (Longitude - 180.0f) : (Longitude + 180.0f)) * M_PI / 180.0f;
+ float x = - cosf(pi_2_theta) * cosf(pi_phi);
+ float y = - cosf(pi_2_theta) * sinf(pi_phi);
+ float z = sinf(pi_2_theta);
+ Vector3 v(x, y, z);
+ return v;
+}
+
+Vector3 GPSUBX_UART::ToUniversal()
+{
+ Vector3 v = ToUniversalUnit();
+ return (Radius + Height) * v;
+}
+
+void GPSUBX_UART::CalculateUnit()
+{
+ Vector3 _d = -1.0f * ToUniversalUnit();
+
+ UniversalZeroPosition = -(Radius+Height)*_d;
+ Vector3 _z(0.0f, 0.0f, 1.0f);
+ Vector3 _e = _d * _z;
+ Vector3 _n = _e * _d;
+ UniversalZeroUnitN = _n;
+ UniversalZeroUnitE = _e;
+ UniversalZeroUnitD = _d;
+}
+
+void GPSUBX_UART::Calculate()
+{
+ UniversalPosition = ToUniversal();
+ Vector3 relative = UniversalPosition - UniversalZeroPosition;
+ Vector3 _position(relative % UniversalZeroUnitN, relative % UniversalZeroUnitE, relative % UniversalZeroUnitD);
+ PositionNED = _position;
+}
+
+Vector3 GPSUBX_UART::Calculate(Vector3 position)
+{
+ UniversalPosition = ToUniversal();
+ Vector3 relative = position - UniversalZeroPosition;
+ Vector3 _position(relative % UniversalZeroUnitN, relative % UniversalZeroUnitE, relative % UniversalZeroUnitD);
+ return _position;
+}