Eigen
Dependencies: Vector3
Dependents: optWingforHAPS_Eigen hexaTest_Eigen
Diff: GPSUBX_UART.cpp
- Revision:
- 0:cf7c726ec8a1
- Child:
- 1:71f5168e48c8
diff -r 000000000000 -r cf7c726ec8a1 GPSUBX_UART.cpp --- /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; +}