GPS/GNSS UBX library for UART
Dependencies: Vector3
GPSUBX_UART.cpp
- Committer:
- NaotoMorita
- Date:
- 2022-06-21
- Revision:
- 21:712076ece407
- Parent:
- 20:efa07b922ba7
File content as of revision 21:712076ece407:
#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) { // 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; Hacc = (float)posllh.data.hAcc / 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() { 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 / _n.Norm(); UniversalZeroUnitE = _e / _e.Norm(); UniversalZeroUnitD = _d / _d.Norm(); //twelite.printf("%f %f %f \r\n",_n,_e,_d); } void GPSUBX_UART::Calculate(float& posN,float& posE,float& posD,float& velN,float& velE,float& velD) { UniversalPosition = ToUniversal(); Vector3 relative = UniversalPosition - UniversalZeroPosition; Vector3 _position(relative % UniversalZeroUnitN, relative % UniversalZeroUnitE, relative % UniversalZeroUnitD); PositionNED = _position; posN = _position.x; posE = _position.y; posD = _position.z; velN = VelocityNED.x; velE = VelocityNED.y; velD = VelocityNED.z; } Vector3 GPSUBX_UART::Calculate(Vector3 position) { UniversalPosition = ToUniversal(); Vector3 relative = position - UniversalZeroPosition; Vector3 _position(relative % UniversalZeroUnitN, relative % UniversalZeroUnitE, relative % UniversalZeroUnitD); return _position; }