File content as of revision 21:712076ece407:

#include "GPSUBX_UART.hpp"
#include "mbed.h"
#include <math.h>
#define M_PI 3.14159265358979f


GPSUBX_UART::GPSUBX_UART(PinName tx, PinName rx, int baud, int timezone)
    :serial(tx, rx, baud), TimeZone(timezone), receive_index(0)
{
}

void GPSUBX_UART::Checksum(char payload[], int n, char* ck_a, char* ck_b)
{
    int ca = 0;
    int cb = 0;
    for (int i = 0; i < n+4; i++)
    {
        ca += (unsigned char)payload[i+2];
        cb += ca;
    }
    *ck_a = (char)(ca & 0xff);
    *ck_b = (char)(cb & 0xff);
}

void GPSUBX_UART::Receive()
{
    while (serial.readable())
    {
        char c;
        c = serial.getc();
        receive_buffer[receive_index] = c;
        receive_index = (receive_index + 1) % RECEIVE_SIZE;
    }
}


void GPSUBX_UART::Update()
{
    volatile static int sentence_start = 0;
    volatile static int sentence_length = 0;
    volatile static int sentence_counter = 0;
    volatile static int read_index = 0;
    
    volatile static char m_class = 0x00;
    volatile static char m_id = 0x00;
    
    while (read_index != receive_index)
    {
        char c;
        c = receive_buffer[read_index];
        if (sentence_counter >= 2)
        {
            sentence_counter++;
            if (sentence_counter == 3)
            {
                m_class = c;
            }
            else if (sentence_counter == 4)
            {
                m_id = c;
            }
            else if (sentence_counter == 5)
            {
            }
            else if (sentence_counter == 6)
            {
                int sss = (read_index+RECEIVE_SIZE-1)%RECEIVE_SIZE;
                sentence_length = (int)(c << 8 | receive_buffer[sss]);
            }
            else if (sentence_counter >= sentence_length+8)
            {
                for (int i = 0; i < sentence_length+8; i++)
                {
                    sentence_buffer[i] = receive_buffer[(sentence_start+i)%RECEIVE_SIZE];
                }
                char ca, cb;
                Checksum(sentence_buffer, sentence_length, &ca, &cb);
                if (ca == sentence_buffer[sentence_length+6] && cb == sentence_buffer[sentence_length+7])
                {
                    Decode(sentence_buffer, m_class, m_id);
                }
                sentence_start = 0;
                sentence_length = 0;
                sentence_counter = 0;
                m_class = 0x00;
                m_id = 0x00;
            }
        }
        
        int ss = (read_index+RECEIVE_SIZE-1)%RECEIVE_SIZE;
        if (c == 0x62 && receive_buffer[ss] == 0xb5)
        {
            sentence_start = ss;
            sentence_counter = 2;
        }
        read_index = (read_index + 1) % RECEIVE_SIZE;
    }
}

void GPSUBX_UART::Attach()
{
    serial.attach(this, &GPSUBX_UART::Receive, Serial::RxIrq);
}

void GPSUBX_UART::Decode(char buffer[], int mc, int mi)
{
    // POSLLH
    if (mc == 0x01 && mi == 0x02)
    {
        POSLLH posllh;
        for (int i = 0; i < POSLLH_LEN; i++)
        {
            posllh.byte_data[i] = buffer[i+6];
        }
        iTOW_POSLLH = posllh.data.iTOW;
        Longitude = (float)posllh.data.lon * 1e-7f;
        Latitude = (float)posllh.data.lat * 1e-7f;
        Height = (float)posllh.data.height / 1000.0f;
        Hacc = (float)posllh.data.hAcc  / 1000.0f;
//        pc.printf("!%d, %f, %f, %f\r\n", iTOW_POSLLH, Longitude, Latitude, Height);
    }
    // STATUS
    if (mc == 0x01 && mi == 0x03)
    {
        STATUS status;
        for (int i = 0; i < STATUS_LEN; i++)
        {
            status.byte_data[i] = buffer[i+6];
        }
        iTOW_STATUS = status.data.iTOW;
        gpsFix = status.data.gpsFix;
    }
    // TIMEUTC
    else if (mc == 0x01 && mi == 0x21)
    {
        TIMEUTC timeutc;
        for (int i = 0; i < TIMEUTC_LEN; i++)
        {
            timeutc.byte_data[i] = buffer[i+6];
        }
        Year = timeutc.data.year;
        Month = timeutc.data.month;
        Day = timeutc.data.day;
        Hours = (int)timeutc.data.hour + TimeZone;
        if (Hours >= 24)
        {
            Hours -= 24;
            Day += 1; 
        }
        else if (Hours < 0)
        {
            Hours += 24;
            Day -= 1; 
        }
        Minutes = timeutc.data.min;
        Seconds = timeutc.data.sec;
//        pc.printf("&%4d/%2d/%2d %2d:%2d %2d\r\n", Year, Month, Day, Hours, Minutes, Seconds);
    }
    // VELNED
    if (mc == 0x01 && mi == 0x12)
    {
        VELNED velned;
        for (int i = 0; i < VELNED_LEN; i++)
        {
            velned.byte_data[i] = buffer[i+6];
        }
        iTOW_VELNED = velned.data.iTOW;
        VelocityNED.x = (float)velned.data.velN / 100.0f;
        VelocityNED.y = (float)velned.data.velE / 100.0f;
        VelocityNED.z = (float)velned.data.velD / 100.0f;
        Speed = (float)velned.data.speed / 100.0f;
        GroundSpeed = (float)velned.data.gSpeed / 100.0f;
        Heading = (float)velned.data.heading * 1e-5f;
//        pc.printf("#%d, %f, %f, %f\r\n", iTOW_VELNED, VelocityNED.x, VelocityNED.y, VelocityNED.z);
    }
}

Vector3 GPSUBX_UART::ToUniversalUnit()
{
    // 東経180度、北緯0度で精度最大
    float pi_2_theta = Latitude * M_PI / 180.0f;
    float pi_phi = ((Longitude > 0.0f) ?  (Longitude - 180.0f) : (Longitude + 180.0f)) * 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 GPSUBX_UART::ToUniversal()
{
    Vector3 v = ToUniversalUnit();
    return (Radius + Height) * v;
}

void GPSUBX_UART::CalculateUnit()
{
    Vector3 _d = -1.0f * ToUniversalUnit();
    
    UniversalZeroPosition = -(Radius+Height)*_d;
    Vector3 _z(0.0f, 0.0f, 1.0f);
    Vector3 _e = _d * _z;
    Vector3 _n = _e * _d;
    UniversalZeroUnitN = _n / _n.Norm();
    UniversalZeroUnitE = _e / _e.Norm();
    UniversalZeroUnitD = _d / _d.Norm();
    //twelite.printf("%f %f %f \r\n",_n,_e,_d);
}

void GPSUBX_UART::Calculate(float& posN,float& posE,float& posD,float& velN,float& velE,float& velD)
{
    UniversalPosition = ToUniversal();
    Vector3 relative = UniversalPosition - UniversalZeroPosition;
    Vector3 _position(relative % UniversalZeroUnitN, relative % UniversalZeroUnitE, relative % UniversalZeroUnitD);
    PositionNED = _position;
    posN = _position.x;
    posE = _position.y;
    posD = _position.z;
    velN = VelocityNED.x;
    velE = VelocityNED.y;
    velD = VelocityNED.z;
}

Vector3 GPSUBX_UART::Calculate(Vector3 position)
{
    UniversalPosition = ToUniversal();
    Vector3 relative = position - UniversalZeroPosition;
    Vector3 _position(relative % UniversalZeroUnitN, relative % UniversalZeroUnitE, relative % UniversalZeroUnitD);
    return _position;
}