Svenska ElektronikForumet

Kod: Markera allt

    |  q   | quaternion           [4x1]
    |  w   | vinkelhastighet      [3x1]
    |  wb  | vinkelhastighetsbias [3x1]
X = | pos  | position             [3x1]
    | vel  | hastighet            [3x1]
    | acc  | acceleration         [3x1]
    | accb | accelerationsbias    [3x1]

Kod: Markera allt

/**********************************************
***************** Navigation routines **********
***********************************************/
float deg2rad(float deg)
{
  return (deg * PI / 180.0);
}

float rad2deg(float rad)
{
  return (rad / PI * 180.0);
}

int GetDistance(float lat1, float lon1, float lat2, float lon2)
    {
        //code for Distance in Meter
        float dlong = (deg2rad(lon2) - deg2rad(lon1));
        float dlat = (deg2rad(lat2) - deg2rad(lat1));
        float a = pow(sin(dlat/2.0), 2) + cos(deg2rad(lat1)) * cos(deg2rad(lat2)) * pow(sin(dlong/2.0), 2);
        float c = 2 * atan2(sqrt(a), sqrt(1-a));
        float dist = 6367000 * c;
        return (dist);
    }

int GetDirection(float lat1, float lon1, float lat2, float lon2)
    {
        //code for Direction in Degrees
        float dlat = deg2rad(lat1) - deg2rad(lat2);
        float dlon = deg2rad(lon1) - deg2rad(lon2);
        float y = sin(dlon) * cos(lat2);
        float x = cos(deg2rad(lat1)) * sin(deg2rad(lat2)) - sin(deg2rad(lat1)) * cos(deg2rad(lat2)) * cos(dlon);
        float direct = rad2deg(atan2(y, x));
        if (direct < 0)
            direct = direct + 360;
        return (direct);
    }