Takashi Koyama
GPS module (GYSFDMAXB) 57600 bps
Dependents: HAPS_GPS_Test_0002
Diff: GYSFDMAXB.cpp
- Revision:
- 0:8114a6b113f4
- Child:
- 1:0d9b4ba850d8
--- /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;
+}