GPS/GNSS UBX library for UART
Dependencies: Vector3
GPSUBX_UART.cpp
- Committer:
- NaotoMorita
- Date:
- 2021-12-01
- Revision:
- 19:ae1a294cb30b
- Parent:
- 15:e77382079cd9
File content as of revision 19:ae1a294cb30b:
#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) :serial(tx, rx, baud), TimeZone(timezone), receive_index(0) { } 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() { while (serial.readable()) { char c; c = serial.getc(); receive_buffer[receive_index] = c; receive_index = (receive_index + 1) % RECEIVE_SIZE; } } void GPSUBX_UART::Update() { volatile static int sentence_start = 0; volatile static int sentence_length = 0; volatile static int sentence_counter = 0; volatile static int read_index = 0; volatile static char m_class = 0x00; volatile static char m_id = 0x00; while (read_index != receive_index) { char c; c = receive_buffer[read_index]; if (sentence_counter >= 2) { sentence_counter++; if (sentence_counter == 3) { m_class = c; } else if (sentence_counter == 4) { m_id = c; } else if (sentence_counter == 5) { } else if (sentence_counter == 6) { int sss = (read_index+RECEIVE_SIZE-1)%RECEIVE_SIZE; sentence_length = (int)(c << 8 | receive_buffer[sss]); } 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]; } char ca, cb; Checksum(sentence_buffer, sentence_length, &ca, &cb); if (ca == sentence_buffer[sentence_length+6] && cb == sentence_buffer[sentence_length+7]) { Decode(sentence_buffer, m_class, m_id); } sentence_start = 0; sentence_length = 0; sentence_counter = 0; m_class = 0x00; m_id = 0x00; } } int ss = (read_index+RECEIVE_SIZE-1)%RECEIVE_SIZE; if (c == 0x62 && receive_buffer[ss] == 0xb5) { sentence_start = ss; sentence_counter = 2; } read_index = (read_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) { // POSECEF if (mc == 0x01 && mi == 0x01) { POSECEF posecef; for (int i = 0; i < POSECEF_LEN; i++) { posecef.byte_data[i] = buffer[i+6]; } iTOW_POSECEF = posecef.data.iTOW; PositionECEF.x = (float)posecef.data.ecefX/100.0f; PositionECEF.y = (float)posecef.data.ecefY/100.0f; PositionECEF.z = (float)posecef.data.ecefZ/100.0f; // pc.printf("!%d, %f, %f, %f\r\n", iTOW_POSLLH, Longitude, Latitude, Height); } // 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); } // STATUS if (mc == 0x01 && mi == 0x03) { STATUS status; for (int i = 0; i < STATUS_LEN; i++) { status.byte_data[i] = buffer[i+6]; } iTOW_STATUS = status.data.iTOW; gpsFix = status.data.gpsFix; } // 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 = (int)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() { UniversalZeroPosition = PositionECEF; UniversalLatitude = Latitude; UniversalLongitude = Longitude; float lat = Latitude * M_PI / 180.0f; float lon = Longitude * M_PI / 180.0f; UniversalZeroUnit1.x = -sinf(lat)*cosf(lon); UniversalZeroUnit1.y = -sinf(lat)*sinf(lon); UniversalZeroUnit1.z = cosf(lat); UniversalZeroUnit2.x = -sinf(lon); UniversalZeroUnit2.y = cosf(lon); UniversalZeroUnit2.z = 0.0f; UniversalZeroUnit3.x = -cosf(lat)*cosf(lon); UniversalZeroUnit3.y = -cosf(lat)*sinf(lon); UniversalZeroUnit3.z = -sinf(lat); //twelite.printf("%f %f %f \r\n",_n,_e,_d); } void GPSUBX_UART::Calculate() { Vector3 relative = PositionECEF - UniversalZeroPosition; Vector3 _position(relative % UniversalZeroUnit1, relative % UniversalZeroUnit2, relative % UniversalZeroUnit3); PositionNED = _position; }