GPS module (GYSFDMAXB) 57600 bps
Dependents: HAPS_GPS_Test_0002
GYSFDMAXB.cpp
- Committer:
- cocorlow
- Date:
- 2021-04-08
- Revision:
- 2:9b567c8f5429
- Parent:
- 1:0d9b4ba850d8
- Child:
- 3:f8045f83d7c1
File content as of revision 2:9b567c8f5429:
#include "mbed.h" #include "GYSFDMAXB.hpp" #include <string.h> #include <stdlib.h> #include "Vector3.hpp" #include <cmath> #define M_PI 3.14159265358979 GYSFDMAXB::GYSFDMAXB(PinName tx, PinName rx) :serial(tx, rx), receive_flag(false), uart_index(0) { for (int i = 0; i < uart_size; i++) { uart_buffer[i] = 0; } for (int i = 0; i < start_size; i++) { uart_start[i] = NULL; } serial.baud(57600); serial.attach(this, &GYSFDMAXB::Receive, Serial::RxIrq); } void GYSFDMAXB::Receive() { while (serial.readable()) { char c; c = serial.getc(); if (c == '$') { uart_buffer[uart_index] = c; uart_start[start_index] = &(uart_buffer[uart_index]); uart_index++; } else if (c == '\r') { } else if (c == '\n') { start_index = (start_index + 1) % start_size; uart_buffer[uart_index] = '\0'; receive_flag = true; uart_index++; } else { uart_buffer[uart_index] = c; uart_index++; } } } void GYSFDMAXB::Update() { if (receive_flag) { for (int i = 0; i < start_size; i++) { if (uart_start[i] != NULL) { char str[256]; char* p[16]; int p_index = 0; for (int j = 0; j < 16; j++) { p[j] = NULL; } strcpy(str, uart_start[i]); char checksum = 0; int c_i = 1; while (str[c_i] !='*') { checksum ^= str[c_i]; c_i++; } char data_checksum = 0; char cc; cc = str[c_i+1]; if ('0' <= cc && cc <= '9') data_checksum += (cc-'0')*16; else data_checksum += ((cc-'A')+10)*16; cc = str[c_i+2]; if ('0' <= cc && cc <= '9') data_checksum += (cc-'0'); else data_checksum += ((cc-'A')+10); if (data_checksum != checksum) { continue; } int j = 0; p[p_index] = str; p_index++; while (1) { if (str[j] == ',') { p[p_index] = &(str[j + 1]); p_index++; str[j] = '\0'; } else if (str[j] == '\0') { break; } j++; } /* for (int k = 0; k < p_index; k++) { pc.printf("%s ~ ", p[k]); } pc.printf("\r\n"); */ if (strcmp(p[0], "$GPGGA") == 0) { if (p[6][0] != '\0') Quality = atoi(p[6]); if (p[7][0] != '\0') Satellites = atoi(p[7]); if (p[8][0] != '\0') HDOP = atof(p[8]); if (p[9][0] != '\0') Elevation = atof(p[9]); UnitElevation = p[10][0]; if (p[11][0] != '\0') GeoidElevation = atof(p[11]); UnitGeoidElevation = p[10][0]; } else if (strcmp(p[0], "$GPGLL") == 0) { } else if (strcmp(p[0], "$GPGSA") == 0) { } else if (strcmp(p[0], "$GPGSV") == 0) { } else if (strcmp(p[0], "$GPRMC") == 0) { float _ms_deg_1; int _ms_deg_2; Hours = (p[1][0]-'0')*10+(p[1][1]-'0'); Minutes = (p[1][2]-'0')*10+(p[1][3]-'0'); Seconds = (p[1][4]-'0')*10+(p[1][5]-'0'); Milliseconds = (p[1][7]-'0')*100+(p[1][8]-'0')*10+(p[1][9]-'0'); Status = p[2][0]; if (p[3][0] != '\0') { _ms_deg_1 = atof(p[3]); _ms_deg_2 = (int)_ms_deg_1 / 100; Latitude = _ms_deg_2 + (_ms_deg_1-100.0*_ms_deg_2)/60.0; } N_S = p[4][0]; if (p[5][0] != '\0') { _ms_deg_1 = atof(p[5]); _ms_deg_2 = (int)_ms_deg_1 / 100; Longitude = _ms_deg_2 + (_ms_deg_1-100.0*_ms_deg_2)/60.0; } E_W = p[6][0]; if (p[7][0] != '\0') Speed = atof(p[7]); if (p[8][0] != '\0') Direction = atof(p[8]); Day = (p[9][0]-'0')*10+(p[9][1]-'0'); Month = (p[9][2]-'0')*10+(p[9][3]-'0'); Year = (p[9][4]-'0')*10+(p[9][5]-'0'); if (p[10][0] != '\0') GeomagneticDeclination = atof(p[10]); GeomagneticE_W = p[11][0]; Mode = p[12][0]; } else if (strcmp(p[0], "$GPVTG") == 0) { } else if (strcmp(p[0], "$GPZDA") == 0) { uart_index = 0; } uart_start[i] = NULL; } } receive_flag = false; } } Vector3 GYSFDMAXB::ToUniversalUnit() { // 東経180度、北緯0度で精度最大 float pi_2_theta = (Latitude * ((N_S == 'N') ? 1.0 : -1.0)) * M_PI / 180.0; float pi_phi = ((E_W == 'E') ? Longitude - 180 : 180 - Longitude) * M_PI / 180.0; const float pi_2 = M_PI / 2; float r = Radius + Elevation; float x = - cos(pi_2_theta) * cos(pi_phi); float y = - cos(pi_2_theta) * sin(pi_phi); float z = sin(pi_2_theta); Vector3 v(x, y, z); return v; } Vector3 GYSFDMAXB::ToUniversal() { Vector3 v = ToUniversalUnit(); return (Radius + Elevation) * v; } void GYSFDMAXB::CalcurateUnit() { Vector3 _d = -1.0 * ToUniversalUnit(); UniversalZeroPosition = -(Radius+Elevation)*_d; Vector3 _z(0.0, 0.0, 1.0); Vector3 _e = _d * _z; Vector3 _n = _e * _d; UniversalZeroUnitN = _n; UniversalZeroUnitE = _e; UniversalZeroUnitD = _d; } void GYSFDMAXB::Calcurate() { UniversalPosition = ToUniversal(); Vector3 relative = UniversalPosition - UniversalZeroPosition; Vector3 _position(relative % UniversalZeroUnitN, relative % UniversalZeroUnitE, relative % UniversalZeroUnitD); Position = _position; } Vector3 GYSFDMAXB::Calcurate(Vector3 position) { UniversalPosition = ToUniversal(); Vector3 relative = position - UniversalZeroPosition; Vector3 _position(relative % UniversalZeroUnitN, relative % UniversalZeroUnitE, relative % UniversalZeroUnitD); return _position; }