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; +//}