File content as of revision 3:f8045f83d7c1:

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