GPS module (GYSFDMAXB) 57600 bps
Dependents: HAPS_GPS_Test_0001
GYSFDMAXB GPSセンサーGYSFDMAXBのライブラリ 57600 bps
手順 [1] シリアルピンを設定 GYSFDMAXB gps(tx_pin, rx_pin); [2] 零点を設定する(NED座標系における零点) gps.Initialize(); [3] 自動的にデータを受信しては更新していくので、適宜メンバ変数を読み込んで使う
GYSFDMAXB.cpp
- Committer:
- cocorlow
- Date:
- 2021-04-23
- Revision:
- 5:6fc37ae3f262
- Parent:
- 3:f8045f83d7c1
- Child:
- 6:db9f8d2afab7
File content as of revision 5:6fc37ae3f262:
#include "mbed.h" #include "GYSFDMAXB.hpp" #include <string.h> #include <stdlib.h> #include "Vector3.hpp" #include <math.h> #define M_PI 3.14159265358979f GYSFDMAXB::GYSFDMAXB(PinName tx, PinName rx) :serial(tx, rx), receive_flag(false), start_index(0), 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); timer.attach(this, &GYSFDMAXB::Punctuate, 0.01); } 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 = (uart_index + 1) % uart_size; } 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 = (uart_index + 1) % uart_size; } else { uart_buffer[uart_index] = c; uart_index = (uart_index + 1) % uart_size; } } } void GYSFDMAXB::Punctuate() { volatile static int counter = 0; const int delay = 2; if (receive_flag) { counter++; } if (counter >= delay) { counter = 0; Update(); receive_flag = false; } } 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++; } if (strcmp(p[0], "$GPGGA") == 0) { if (p[6][0] != '\0') Quality = atoi(p[6]); Satellites = 0; 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.0f*_ms_deg_2)/60.0f; } 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.0f*_ms_deg_2)/60.0f; } 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; // } } void GYSFDMAXB::Initialize() { Satellites = 0; while (Satellites <= 4 || Latitude == 0.0f || Longitude == 0.0f || Elevation == 0.0f) { Update(); } CalcurateUnit(); } Vector3 GYSFDMAXB::ToUniversalUnit() { // 東経180度、北緯0度で精度最大 float pi_2_theta = (Latitude * ((N_S == 'N') ? 1.0f : -1.0f)) * M_PI / 180.0f; float pi_phi = ((E_W == 'E') ? Longitude - 180.0f : 180.0f - Longitude) * 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 GYSFDMAXB::ToUniversal() { Vector3 v = ToUniversalUnit(); return (Radius + Elevation) * v; } void GYSFDMAXB::CalcurateUnit() { Vector3 _d = -1.0f * ToUniversalUnit(); UniversalZeroPosition = -(Radius+Elevation)*_d; Vector3 _z(0.0f, 0.0f, 1.0f); 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; }