File content as of revision 4:708e05b10257:

#include "mbed.h"
#include "GPS_HAPS.hpp"
#include <string.h>
#include <stdlib.h>
#include "Vector3.hpp"
#include <math.h>
#define M_PI 3.14159265358979f

GPS_HAPS::GPS_HAPS(PinName tx, PinName rx, int baud)
    :serial(tx, rx, baud), receive_start(0), receive_index(0), receive_request(0)
{
}

void GPS_HAPS::Receive()
{
    while (serial.readable())
    {
        char c;
        c = serial.getc();
//        pc.putc(c);
        receive_buffer[receive_index] = c;
        if (c == '\r')
        {
            receive_request++;
        }
        receive_index = (receive_index + 1) % RECEIVE_SIZE;
    }   
}

void GPS_HAPS::Loop()
{
    if (receive_request > 0)
    {
        if (receive_buffer[receive_start] == '$')
        {
            for (int i = 0; i < SENTENCE_SIZE; i++)
            {
                char c;
                c = receive_buffer[(receive_start + i) % RECEIVE_SIZE];
                sentence_buffer[i] = c;
                if (c == '\r')
                {
                    receive_start = (receive_start + i + 1) % RECEIVE_SIZE;
                    sentence_buffer[i+1] = 0;
                    break;
                }
            }
            Parse(sentence_buffer);
            receive_request--;
        }
        else
        {
            while (receive_start != receive_index)
            {
                receive_start = (receive_start + 1) % RECEIVE_SIZE;
                if (receive_buffer[receive_start] == '$')
                {
                    break;
                }
            }
        }
    }
}
 
void GPS_HAPS::Parse(char *cmd)
{   
    char ns, ew, tf, status;
    int fq, nst, fix, date;                                     // fix quality, Number of satellites being tracked, 3D fix
    float latitude, longitude, timefix, speed, altitude;
    char s[16];
    
    // Global Positioning System Fix Data
//    $GNGGA,030503.00,3542.92568,N,13945.76087,E,1,05,10.02,225.2,M,39.4,M,,*7C
//    if(strncmp(cmd,"$GPGGA", 6) == 0)
    if(strstr(cmd,"GGA") != NULL)
    {
        sscanf(cmd, "%[^,],%f,%f,%c,%f,%c,%d,%d,%f", s, &timefix, &latitude, &ns, &longitude, &ew, &fq, &nst, &altitude);
        if (abs(latitude) < ERROR_F)
        {
            Latitude = -1.0f;
        }
        else
        {
            float lat = (int)latitude/100;
            Latitude = lat + (latitude-lat*100)/60.0f;
        }
        if (abs(longitude) < ERROR_F)
        {
            Longitude = -1.0f;
        }
        else
        {
            float lon = (int)longitude/100;
            Longitude = lon + (longitude-lon*100)/60.0f;
        }
        N_S = ns;
        E_W = ew;
        Quality = fq;
        Satellites = nst;
        if (abs(altitude) < ERROR_F)
        {
            Altitude = -1.0f;
        }
        {
            Altitude = altitude;
        }
//        pc.printf("%s\n", cmd);
//        pc.printf("##GPGGA Fix taken at: %f, Latitude: %f %c, Longitude: %f %c, Fix quality: %d, Number of sat: %d, Altitude: %f M\r\n", timefix, latitude, ns, longitude, ew, fq, nst, altitude);
    }
    
//    // Satellite status
//     else if(strncmp(cmd,"$GPGSA", 6) == 0) 
//    {
//        sscanf(cmd, "$GPGSA,%c,%d,%d", &tf, &fix, &nst);
//        pc.printf("GPGSA Type fix: %c, 3D fix: %d, number of sat: %d\r\n", tf, fix, nst);
//    }
    
    // Geographic position, Latitude and Longitude
//    else if(strncmp(cmd,"$GPGLL", 6) == 0) 
//    {
//        sscanf(cmd, "$GPGLL,%f,%c,%f,%c,%f", &latitude, &ns, &longitude, &ew, &timefix);
//        pc.printf("GPGLL Latitude: %f %c, Longitude: %f %c, Fix taken at: %f\n", latitude, ns, longitude, ew, timefix);
//    }
    
    // Geographic position, Latitude and Longitude
//    $GNRMC,035344.00,A,3542.91153,N,13945.74153,E,0.239,,070921,,,A,V*15
    else if(strstr(cmd,"RMC") != NULL) 
    {
        sscanf(cmd, "%[^,],%f,%c,%f,%c,%f,%c,%f,,%d", s, &timefix, &status, &latitude, &ns, &longitude, &ew, &speed, &date);
//        pc.printf("%s,%f,%c,%f,%c,%f,%c,%f,,%d", s, timefix, status, latitude, ns, longitude, ew, speed, date);
        if (abs(timefix) > ERROR_F)
        {
            float time = timefix + 90000.00f;
            Hours = int(time) / 10000;
            Minutes = (int(time) % 10000) / 100;
            Seconds = int(time) % 100;
            Milliseconds = (int)(((time - (Hours*10000+Minutes*100+Seconds)))/0.001f);
            Time = Hours*3600000+Minutes*60000+Seconds*1000+Milliseconds;
            if (abs(speed) < ERROR_F)
            {
                Speed = -1.0f;
            }
            else
            {
                Speed = speed * 0.514444f;
            }
            Year = date % 100;
            date /= 100;
            Month = date % 100;
            date /= 100;
            Day = date;
        }
//        pc.printf("%s\n", cmd);
//        pc.printf("##GPRMC Fix taken at: %f, Status: %c, Latitude: %f %c, Longitude: %f %c, Speed: %f, Date: %d\r\n", timefix, status, latitude, ns, longitude, ew, speed, date);
    }
    
    // 
//    $GNVTG,,T,,M,0.444,N,0.822,K,A*31
    else if(strstr(cmd,"VTG") != NULL) 
    {
//        pc.printf("%s\n", cmd);
        Direction = -1.0f;
        if(strstr(cmd, "VTG,,") == NULL){
            sscanf(cmd, "%[^,],%f", s, &Direction);
//            pc.printf("<<%f\r\n", Direction);
     //   pc.printf("its a Vector Track message.\n");
        }
    }
    
//    else if(strncmp(cmd,"$GNGGA", 6) == 0) 
//    {
//        sscanf(cmd, "$GNGGA,%f,%f,%c,%f,%c,%d,%d,%*f,%f", &timefix, &latitude, &ns, &longitude, &ew, &fq, &nst, &altitude);
//        pc.printf("GNGGA Fix taken at: %f, Latitude: %f %c, Longitude: %f %c, Fix quality: %d, Number of sat: %d, Altitude: %f M\n", timefix, latitude, ns, longitude, ew, fq, nst, altitude);
//        parseTime(timefix);
//        pc.printf("Time: %d:%d:%d\n", h_time, m_time, s_time);
//    }
    
//    else if(strncmp(cmd,"$GNGSA", 6) == 0) 
//    {
//        sscanf(cmd, "$GNGSA,%c,%d,%d", &tf, &fix, &nst);
//        pc.printf("GNGSA Type fix: %c, 3D fix: %d, number of sat: %d\r\n", tf, fix, nst);
//    }
    
//    else if(strncmp(cmd,"$GPGSV", 6) == 0) 
//    {
//     //   pc.printf("its a Satellite details message.\n");
//    }
    
//    else if(strncmp(cmd,"$GNGLL", 6) == 0) 
//    {
//        sscanf(cmd, "$GNGLL,%f,%c,%f,%c,%f", &latitude, &ns, &longitude, &ew, &timefix);
//        pc.printf("GNGLL Latitude: %f %c, Longitude: %f %c, Fix taken at: %f\n", latitude, ns, longitude, ew, timefix);
//    }
    
//    else
//    {
//        pc.printf("Unknown message type\n");
//    }
}
 
void GPS_HAPS::Punctuate()
{
}

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

void GPS_HAPS::Update()
{
}

void GPS_HAPS::Initialize()
{
    Satellites = 0;
    while (Satellites <= 4 || Latitude == 0.0f || Longitude == 0.0f || Elevation == 0.0f)
    {
    }
    CalculateUnit();
}

Vector3 GPS_HAPS::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 GPS_HAPS::ToUniversal()
{
    Vector3 v = ToUniversalUnit();
    return (Radius + Elevation) * v;
}

void GPS_HAPS::CalculateUnit()
{
    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 GPS_HAPS::Calculate()
{
    UniversalPosition = ToUniversal();
    Vector3 relative = UniversalPosition - UniversalZeroPosition;
    Vector3 _position(relative % UniversalZeroUnitN, relative % UniversalZeroUnitE, relative % UniversalZeroUnitD);
    Position = _position;
}

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