File content as of revision 1:0d9b4ba850d8:

#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::ToUniversal()
{
    float theta = (90.0 + Latitude * ((N_S == 'N') ? -1 : 1)) * M_PI / 180.0;
    float phi = (Longitude * ((E_W == 'E') ? 1 : -1)) * M_PI / 180.0;
    float r = Radius + Elevation;
    float x = r * sin(theta) * cos(phi);
    float y = r * sin(theta) * sin(phi);
    float z = r * cos(theta);
    Vector3 v(x, y, z);
    return v;
}

void GYSFDMAXB::CalcurateUnit()
{
    float theta = (90.0 + Latitude * ((N_S == 'N') ? -1 : 1)) * M_PI / 180.0;
    float phi = (Longitude * ((E_W == 'E') ? 1 : -1)) * M_PI / 180.0;
    Vector3 _d(-sin(theta)*cos(phi), -sin(theta)*sin(phi), -cos(theta));
    
    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 % UniversalZeroUnitX, relative % UniversalZeroUnitY, relative % UniversalZeroUnitZ);
    Position = _position;
}

Vector3 GYSFDMAXB::Calcurate(Vector3 position)
{
    UniversalPosition = ToUniversal();
    Vector3 relative = position - UniversalZeroPosition;
    Vector3 _position(relative % UniversalZeroUnitX, relative % UniversalZeroUnitY, relative % UniversalZeroUnitZ);
    return _position;
}