1
0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2024-12-11 19:24:10 +01:00
LibrePilot/flight/Libraries/CoordinateConversions.c

188 lines
6.0 KiB
C

/**
******************************************************************************
*
* @file CoordinateConversions.c
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
* @brief General conversions with different coordinate systems.
* - all angles in deg
* - distances in meters
* - altitude above WGS-84 elipsoid
*
* @see The GNU Public License (GPL) Version 3
*
*****************************************************************************/
/*
* This program is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
* for more details.
*
* You should have received a copy of the GNU General Public License along
* with this program; if not, write to the Free Software Foundation, Inc.,
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/
#include <math.h>
#include <stdint.h>
#include "CoordinateConversions.h"
#define RAD2DEG (180.0/M_PI)
#define DEG2RAD (M_PI/180.0)
// ****** convert Lat,Lon,Alt to ECEF ************
void LLA2ECEF(double LLA[3], double ECEF[3]){
const double a = 6378137.0; // Equatorial Radius
const double e = 8.1819190842622e-2; // Eccentricity
double sinLat, sinLon, cosLat, cosLon;
double N;
sinLat=sin(DEG2RAD*LLA[0]);
sinLon=sin(DEG2RAD*LLA[1]);
cosLat=cos(DEG2RAD*LLA[0]);
cosLon=cos(DEG2RAD*LLA[1]);
N = a / sqrt(1.0 - e*e*sinLat*sinLat); //prime vertical radius of curvature
ECEF[0] = (N+LLA[2])*cosLat*cosLon;
ECEF[1] = (N+LLA[2])*cosLat*sinLon;
ECEF[2] = ((1-e*e)*N + LLA[2]) * sinLat;
}
// ****** convert ECEF to Lat,Lon,Alt (ITERATIVE!) *********
uint16_t ECEF2LLA(double ECEF[3], double LLA[3])
{
const double a = 6378137.0; // Equatorial Radius
const double e = 8.1819190842622e-2; // Eccentricity
double x=ECEF[0], y=ECEF[1], z=ECEF[2];
double Lat, N, NplusH, delta, esLat;
uint16_t iter;
LLA[1] = RAD2DEG*atan2(y,x);
N = a;
NplusH = N;
delta = 1;
Lat = 1;
iter=0;
while (((delta > 1.0e-14)||(delta < -1.0e-14)) && (iter < 100))
{
delta = Lat - atan(z / (sqrt(x*x + y*y)*(1-(N*e*e/NplusH))));
Lat = Lat-delta;
esLat = e*sin(Lat);
N = a / sqrt(1 - esLat*esLat);
NplusH = sqrt(x*x + y*y)/cos(Lat);
iter += 1;
}
LLA[0] = RAD2DEG*Lat;
LLA[2] = NplusH - N;
if (iter==500) return (0);
else return (1);
}
// ****** find ECEF to NED rotation matrix ********
void RneFromLLA(double LLA[3], float Rne[3][3]){
float sinLat, sinLon, cosLat, cosLon;
sinLat=(float)sin(DEG2RAD*LLA[0]);
sinLon=(float)sin(DEG2RAD*LLA[1]);
cosLat=(float)cos(DEG2RAD*LLA[0]);
cosLon=(float)cos(DEG2RAD*LLA[1]);
Rne[0][0] = -sinLat*cosLon; Rne[0][1] = -sinLat*sinLon; Rne[0][2] = cosLat;
Rne[1][0] = -sinLon; Rne[1][1] = cosLon; Rne[1][2] = 0;
Rne[2][0] = -cosLat*cosLon; Rne[2][1] = -cosLat*sinLon; Rne[2][2] = -sinLat;
}
// ****** find roll, pitch, yaw from quaternion ********
void Quaternion2RPY(float q[4], float rpy[3]){
float R13, R11, R12, R23, R33;
float q0s=q[0]*q[0];
float q1s=q[1]*q[1];
float q2s=q[2]*q[2];
float q3s=q[3]*q[3];
R13 = 2*(q[1]*q[3]-q[0]*q[2]);
R11 = q0s+q1s-q2s-q3s;
R12 = 2*(q[1]*q[2]+q[0]*q[3]);
R23 = 2*(q[2]*q[3]+q[0]*q[1]);
R33 = q0s-q1s-q2s+q3s;
rpy[1]=RAD2DEG*asinf(-R13); // pitch always between -pi/2 to pi/2
rpy[2]=RAD2DEG*atan2f(R12,R11);
rpy[0]=RAD2DEG*atan2f(R23,R33);
}
// ****** find quaternion from roll, pitch, yaw ********
void RPY2Quaternion(float rpy[3], float q[4]){
float phi, theta, psi;
float cphi, sphi, ctheta, stheta, cpsi, spsi;
phi=DEG2RAD*rpy[0]/2; theta=DEG2RAD*rpy[1]/2; psi=DEG2RAD*rpy[2]/2;
cphi=cosf(phi); sphi=sinf(phi);
ctheta=cosf(theta); stheta=sinf(theta);
cpsi=cosf(psi); spsi=sinf(psi);
q[0] = cphi*ctheta*cpsi + sphi*stheta*spsi;
q[1] = sphi*ctheta*cpsi - cphi*stheta*spsi;
q[2] = cphi*stheta*cpsi + sphi*ctheta*spsi;
q[3] = cphi*ctheta*spsi - sphi*stheta*cpsi;
if (q[0] < 0){ // q0 always positive for uniqueness
q[0]=-q[0];
q[1]=-q[1];
q[2]=-q[2];
q[3]=-q[3];
}
}
//** Find Rbe, that rotates a vector from earth fixed to body frame, from quaternion **
void Quaternion2R(float q[4], float Rbe[3][3]){
float q0s=q[0]*q[0], q1s=q[1]*q[1], q2s=q[2]*q[2], q3s=q[3]*q[3];
Rbe[0][0]=q0s+q1s-q2s-q3s;
Rbe[0][1]=2*(q[1]*q[2]+q[0]*q[3]);
Rbe[0][2]=2*(q[1]*q[3]-q[0]*q[2]);
Rbe[1][0]=2*(q[1]*q[2]-q[0]*q[3]);
Rbe[1][1]=q0s-q1s+q2s-q3s;
Rbe[1][2]=2*(q[2]*q[3]+q[0]*q[1]);
Rbe[2][0]=2*(q[1]*q[3]+q[0]*q[2]);
Rbe[2][1]=2*(q[2]*q[3]-q[0]*q[1]);
Rbe[2][2]=q0s-q1s-q2s+q3s;
}
// ****** Express LLA in a local NED Base Frame ********
void LLA2Base(double LLA[3], double BaseECEF[3], float Rne[3][3], float NED[3]){
double ECEF[3];
float diff[3];
LLA2ECEF(LLA,ECEF);
diff[0]=(float)(ECEF[0]-BaseECEF[0]);
diff[1]=(float)(ECEF[1]-BaseECEF[1]);
diff[2]=(float)(ECEF[2]-BaseECEF[2]);
NED[0]= Rne[0][0]*diff[0]+Rne[0][1]*diff[1]+Rne[0][2]*diff[2];
NED[1]= Rne[1][0]*diff[0]+Rne[1][1]*diff[1]+Rne[1][2]*diff[2];
NED[2]= Rne[2][0]*diff[0]+Rne[2][1]*diff[1]+Rne[2][2]*diff[2];
}
// ****** Express ECEF in a local NED Base Frame ********
void ECEF2Base(double ECEF[3], double BaseECEF[3], float Rne[3][3], float NED[3]){
float diff[3];
diff[0]=(float)(ECEF[0]-BaseECEF[0]);
diff[1]=(float)(ECEF[1]-BaseECEF[1]);
diff[2]=(float)(ECEF[2]-BaseECEF[2]);
NED[0]= Rne[0][0]*diff[0]+Rne[0][1]*diff[1]+Rne[0][2]*diff[2];
NED[1]= Rne[1][0]*diff[0]+Rne[1][1]*diff[1]+Rne[1][2]*diff[2];
NED[2]= Rne[2][0]*diff[0]+Rne[2][1]*diff[1]+Rne[2][2]*diff[2];
}