File content as of revision 107:46e039e12182:

#include "global.hpp"

void getGPSval()
{
    gpsUpdateFlag = false;
    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;
        gps.Calculate();
        change_refpos();
        pi.x = gps.PositionNED.x;
        pi.y = gps.PositionNED.y;
        pi.z = gps.PositionNED.z;
        gpsUpdateFlag = true;
    }
}

//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)
    {
        autopilot.set_dest(destination.x  - gps.PositionNED.x, destination.y  - gps.PositionNED.y);
        autopilot.set_turn(turn_center.x - gps.PositionNED.x, turn_center.y - gps.PositionNED.y, turn_radius);
        gps.CalculateUnit();
        eskf.setPihat(0.0f, 0.0f, 0.0f);
        //palt0 = palt;
        
    }
    else
    {
        //何もしない
    }
}