File content as of revision 4:8d315f7977b3:

#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), start_index(0), 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);
    timer.attach(this, &GYSFDMAXB::Punctuate, 0.01);
}

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::Punctuate()
{
    volatile static int counter = 0;
    const int delay = 2;
    if (receive_flag)
    {
        counter++;
    }
    if (counter >= delay)
    {
        counter = 0;
        Update();
        receive_flag = false;
    }
}

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++;
                }
                
                
                if (strcmp(p[0], "$GPGGA") == 0)
                {
                    if (p[6][0] != '\0')
                        Quality = atoi(p[6]);
                    Satellites = 0;
                    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.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;
                }
                
                uart_start[i] = NULL;
            }
        }
//        receive_flag = false;
//    }
}

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