Takashi Koyama
GPS module (GYSFDMAXB) 57600 bps
Dependents: HAPS_GPS_Test_0002
Diff: GYSFDMAXB.cpp
- Revision:
- 3:f8045f83d7c1
- Parent:
- 2:9b567c8f5429
- Child:
- 4:8d315f7977b3
diff -r 9b567c8f5429 -r f8045f83d7c1 GYSFDMAXB.cpp
--- a/GYSFDMAXB.cpp Thu Apr 08 10:38:27 2021 +0000
+++ b/GYSFDMAXB.cpp Thu Apr 22 11:51:16 2021 +0000
@@ -1,246 +1,260 @@
-#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;
-}
+//#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), uart_index(0), start_index(0)
+//{
+// for (int i = 0; i < uart_size; i++)
+// {
+// uart_buffer[i] = '\0';
+// for (int j = 0; j < phrase_size; j++)
+// {
+// phrase_buffer[i][j] = '\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 = (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::Update()
+//{
+// for (int i = 0; i < start_size; i++)
+// {
+// if (uart_start[i] != NULL)
+// {
+// strcpy(phrase_buffer[i], uart_start[i]);
+// uart_start[i] = NULL;
+// }
+// }
+// for (int i = 0; i < start_size; i++)
+// {
+// if (phrase_buffer[i][0] != '\0')
+// {
+// char str[128];
+// char* p[16];
+// int p_index = 0;
+// for (int j = 0; j < 16; j++)
+// {
+// p[j] = NULL;
+// }
+// strcpy(str, phrase_buffer[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)
+// {
+// strcpy(gpgga, phrase_buffer[i]);
+// for (int i = 0; i < 128; i++)
+// {
+// gpgga[i] =
+// }
+// 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;
+//
+// if (gpgga[6][0] != '\0')
+// Quality = atoi(p[6]);
+// Satellites = 0;
+// if (gpgga[7][0] != '\0')
+// Satellites = atoi(p[7]);
+// if (gpgga[8][0] != '\0')
+// HDOP = atof(p[8]);
+// if (gpgga[9][0] != '\0')
+// Elevation = atof(p[9]);
+// UnitElevation = p[10][0];
+// if (gpgga[11][0] != '\0')
+// GeoidElevation = atof(gpgga[11]);
+// UnitGeoidElevation = gpgga[10][0];
+//
+// }
+//
+// strcpy(phrase_buffer[i], "");
+// }
+// }
+//}
+//
+//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;
+//}