GPS module (GYSFDMAXB) 57600 bps
Dependents: HAPS_GPS_Test_0002
Diff: GYSFDMAXB.cpp
- Revision:
- 0:8114a6b113f4
- Child:
- 1:0d9b4ba850d8
diff -r 000000000000 -r 8114a6b113f4 GYSFDMAXB.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/GYSFDMAXB.cpp Tue Apr 06 08:16:08 2021 +0000 @@ -0,0 +1,227 @@ +#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; +}