File content as of revision 10:8f574ff249bd:

#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, 57600), 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.attach(this, &GYSFDMAXB::Receive, Serial::RxIrq);
    timer.attach(this, &GYSFDMAXB::Punctuate, 0.1);
}

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()
{
    for (int i = 0; i < start_size; i++)
    {
        if (uart_start[i] != NULL)
        {
            char str[256];
            char* p[64];
            int p_index = 0;
            for (int j = 0; j < 64; 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;
        }
    }
}

void GYSFDMAXB::Initialize()
{
    Satellites = 0;
    while (Satellites <= 4 || Latitude == 0.0f || Longitude == 0.0f || Elevation == 0.0f)
    {
    }
    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;
}