File content as of revision 98:bdaa6bbfb1d9:

#include "global.hpp"

void getGPSval()
{
    gps.Update();
    if (gps.iTOW_VELNED != itow_velned)
    {
        itow_velned = gps.iTOW_VELNED;
        vi.x = gps.VelocityNED.x;
        vi.y = gps.VelocityNED.y;
        vi.z = gps.VelocityNED.z;
    }
    if (gps.iTOW_POSLLH != itow_posllh)
    {
        itow_posllh = gps.iTOW_POSLLH;
        change_refpos();
        gps.Calculate();
        pi.x = gps.PositionNED.x;
        pi.y = gps.PositionNED.y;
        pi.z = gps.PositionNED.z;
    }
}

//20m以上の変位で基準位置更新
void change_refpos()
{
    float x_dist = gps.PositionNED.x - pi.x;
    float y_dist = gps.PositionNED.y - pi.y;
    float z_dist = gps.PositionNED.z - pi.z;
    float dist2 = x_dist*x_dist + y_dist*y_dist + z_dist*z_dist;
    if (dist2 >= 400.0f)
    {
        gps.CalculateUnit();
        eskf.setPihat(0.0f, 0.0f, 0.0f);
        palt0 = palt;
    }
    else
    {
        //何もしない
    }
}