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-06
- Revision:
- 0:8114a6b113f4
- Child:
- 1:0d9b4ba850d8
File content as of revision 0:8114a6b113f4:
#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) { 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]; Latitude = atof(p[3]); N_S = p[4][0]; if (p[5][0] != '\0') Longitude = atof(p[5]); 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::ToUniversal() { float theta = (90.0 + Latitude * ((N_S == 'N') ? -1 : 1)) * M_PI / 180.0; float phi = (Longitude * ((E_W == 'E') ? 1 : -1)) * M_PI / 180.0; float r = Radius + Elevation; float x = r * sin(theta) * cos(phi); float y = r * sin(theta) * sin(phi); float z = r * cos(theta); Vector3 v(x, y, z); return v; } void GYSFDMAXB::CalcurateUnit() { float theta = (90.0 + Latitude * ((N_S == 'N') ? -1 : 1)) * M_PI / 180.0; float phi = (Longitude * ((E_W == 'E') ? 1 : -1)) * M_PI / 180.0; Vector3 _z(sin(theta)*cos(phi), sin(theta)*sin(phi), cos(theta)); Vector3 _y(-cos(theta)*cos(phi), -cos(theta)*sin(phi), sin(theta)); UniversalZeroPosition = (Radius+Elevation)*_z; UniversalZeroUnitY = _y; UniversalZeroUnitZ = _z; UniversalZeroUnitX = _y * _z; } void GYSFDMAXB::Calcurate() { UniversalPosition = ToUniversal(); Vector3 relative = UniversalPosition - UniversalZeroPosition; Vector3 _position(relative % UniversalZeroUnitX, relative % UniversalZeroUnitY, relative % UniversalZeroUnitZ); Position = _position; } Vector3 GYSFDMAXB::Calcurate(Vector3 position) { UniversalPosition = ToUniversal(); Vector3 relative = position - UniversalZeroPosition; Vector3 _position(relative % UniversalZeroUnitX, relative % UniversalZeroUnitY, relative % UniversalZeroUnitZ); return _position; }