rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2024-12-01 09:24:10 +01:00
Code Issues Releases Activity

Changed CoordinateConversion to use doubles for LLA2NED conversion, so coordinates can be represented in ECEF with sufficient resolution

Browse Source
This commit is contained in:
Corvus Corax 2013-06-05 20:40:49 +02:00
parent 5e306250a5
commit 6f6ca2481e
4 changed files with 75 additions and 54 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

92
flight/libraries/CoordinateConversions.c
View File

@ -35,27 +35,27 @@
#define MIN_ALLOWABLE_MAGNITUDE 1e-30f
// ****** convert Lat,Lon,Alt to ECEF ************
void LLA2ECEF(float LLA[3], float ECEF[3])
void LLA2ECEF(float LLA[3], double ECEF[3])
{
const float a = 6378137.0f; // Equatorial Radius
const float e = 8.1819190842622e-2f; // Eccentricity
float sinLat, sinLon, cosLat, cosLon;
float N;
const double a = 6378137.0d; // Equatorial Radius
const double e = 8.1819190842622e-2d; // Eccentricity
double sinLat, sinLon, cosLat, cosLon;
double N;
sinLat = sinf(DEG2RAD(LLA[0]));
sinLon = sinf(DEG2RAD(LLA[1]));
cosLat = cosf(DEG2RAD(LLA[0]));
cosLon = cosf(DEG2RAD(LLA[1]));
sinLat = sin(DEG2RAD(LLA[0]));
sinLon = sin(DEG2RAD(LLA[1]));
cosLat = cos(DEG2RAD(LLA[0]));
cosLon = cos(DEG2RAD(LLA[1]));
N = a / sqrtf(1.0f - e * e * sinLat * sinLat); // prime vertical radius of curvature
N = a / sqrt(1.0d - 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;
ECEF[0] = (N + (double)LLA[2]) * cosLat * cosLon;
ECEF[1] = (N + (double)LLA[2]) * cosLat * sinLon;
ECEF[2] = ((1 - e * e) * N + (double)LLA[2]) * sinLat;
}
// ****** convert ECEF to Lat,Lon,Alt (ITERATIVE!) *********
uint16_t ECEF2LLA(float ECEF[3], float LLA[3])
uint16_t ECEF2LLA(double ECEF[3], float LLA[3])
{
/**
* LLA parameter is used to prime the iteration.
@ -66,48 +66,48 @@ uint16_t ECEF2LLA(float ECEF[3], float LLA[3])
* Suggestion: [0,0,0]
**/
const float a = 6378137.0f; // Equatorial Radius
const float e = 8.1819190842622e-2f; // Eccentricity
float x = ECEF[0], y = ECEF[1], z = ECEF[2];
float Lat, N, NplusH, delta, esLat;
const double a = 6378137.0f; // Equatorial Radius
const double e = 8.1819190842622e-2f; // Eccentricity
double x = ECEF[0], y = ECEF[1], z = ECEF[2];
double Lat, N, NplusH, delta, esLat;
uint16_t iter;
#define MAX_ITER 10 // should not take more than 5 for valid coordinates
#define ACCURACY 1.0e-11f // used to be e-14, but we don't need sub micrometer exact calculations
#define ACCURACY 1.0e-11d // used to be e-14, but we don't need sub micrometer exact calculations
LLA[1] = RAD2DEG(atan2f(y, x));
Lat = DEG2RAD(LLA[0]);
esLat = e * sinf(Lat);
N = a / sqrtf(1 - esLat * esLat);
NplusH = N + LLA[2];
LLA[1] = (float)RAD2DEG_D(atan2(y, x));
Lat = DEG2RAD_D((double)LLA[0]);
esLat = e * sin(Lat);
N = a / sqrt(1 - esLat * esLat);
NplusH = N + (double)LLA[2];
delta = 1;
iter = 0;
while (((delta > ACCURACY) || (delta < -ACCURACY))
&& (iter < MAX_ITER)) {
delta = Lat - atanf(z / (sqrtf(x * x + y * y) * (1 - (N * e * e / NplusH))));
delta = Lat - atan(z / (sqrt(x * x + y * y) * (1 - (N * e * e / NplusH))));
Lat = Lat - delta;
esLat = e * sinf(Lat);
N = a / sqrtf(1 - esLat * esLat);
NplusH = sqrtf(x * x + y * y) / cosf(Lat);
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[0] = RAD2DEG_D(Lat);
LLA[2] = NplusH - N;
return iter < MAX_ITER;
}
// ****** find ECEF to NED rotation matrix ********
void RneFromLLA(float LLA[3], float Rne[3][3])
void RneFromLLA(float LLA[3], double Rne[3][3])
{
float sinLat, sinLon, cosLat, cosLon;
double sinLat, sinLon, cosLat, cosLon;
sinLat = (float)sinf(DEG2RAD(LLA[0]));
sinLon = (float)sinf(DEG2RAD(LLA[1]));
cosLat = (float)cosf(DEG2RAD(LLA[0]));
cosLon = (float)cosf(DEG2RAD(LLA[1]));
sinLat = sin(DEG2RAD(LLA[0]));
sinLon = sin(DEG2RAD(LLA[1]));
cosLat = cos(DEG2RAD(LLA[0]));
cosLon = cos(DEG2RAD(LLA[1]));
Rne[0][0] = -sinLat * cosLon;
Rne[0][1] = -sinLat * sinLon;
@ -188,16 +188,16 @@ void Quaternion2R(float q[4], float Rbe[3][3])
}
// ****** Express LLA in a local NED Base Frame ********
void LLA2Base(float LLA[3], float BaseECEF[3], float Rne[3][3], float NED[3])
void LLA2Base(float LLA[3], double BaseECEF[3], double Rne[3][3], float NED[3])
{
float ECEF[3];
float diff[3];
double ECEF[3];
double 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]);
diff[0] = (ECEF[0] - BaseECEF[0]);
diff[1] = (ECEF[1] - BaseECEF[1]);
diff[2] = (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];
@ -205,13 +205,13 @@ void LLA2Base(float LLA[3], float BaseECEF[3], float Rne[3][3], float NED[3])
}
// ****** Express ECEF in a local NED Base Frame ********
void ECEF2Base(float ECEF[3], float BaseECEF[3], float Rne[3][3], float NED[3])
void ECEF2Base(double ECEF[3], double BaseECEF[3], double Rne[3][3], float NED[3])
{
float diff[3];
double diff[3];
diff[0] = (float)(ECEF[0] - BaseECEF[0]);
diff[1] = (float)(ECEF[1] - BaseECEF[1]);
diff[2] = (float)(ECEF[2] - BaseECEF[2]);
diff[0] = (ECEF[0] - BaseECEF[0]);
diff[1] = (ECEF[1] - BaseECEF[1]);
diff[2] = (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];

10
flight/libraries/inc/CoordinateConversions.h
View File

@ -31,12 +31,12 @@
#define COORDINATECONVERSIONS_H_
// ****** convert Lat,Lon,Alt to ECEF ************
void LLA2ECEF(float LLA[3], float ECEF[3]);
void LLA2ECEF(float LLA[3], double ECEF[3]);
// ****** convert ECEF to Lat,Lon,Alt (ITERATIVE!) *********
uint16_t ECEF2LLA(float ECEF[3], float LLA[3]);
uint16_t ECEF2LLA(double ECEF[3], float LLA[3]);
void RneFromLLA(float LLA[3], float Rne[3][3]);
void RneFromLLA(float LLA[3], double Rne[3][3]);
// ****** find rotation matrix from rotation vector
void Rv2Rot(float Rv[3], float R[3][3]);
@ -51,10 +51,10 @@ void RPY2Quaternion(const float rpy[3], float q[4]);
void Quaternion2R(float q[4], float Rbe[3][3]);
// ****** Express LLA in a local NED Base Frame ********
void LLA2Base(float LLA[3], float BaseECEF[3], float Rne[3][3], float NED[3]);
void LLA2Base(float LLA[3], double BaseECEF[3], double Rne[3][3], float NED[3]);
// ****** Express ECEF in a local NED Base Frame ********
void ECEF2Base(float ECEF[3], float BaseECEF[3], float Rne[3][3], float NED[3]);
void ECEF2Base(double ECEF[3], double BaseECEF[3], double Rne[3][3], float NED[3]);
// ****** convert Rotation Matrix to Quaternion ********
// ****** if R converts from e to b, q is rotation from e to b ****

6
flight/modules/GPS/GPS.c
View File

@ -346,14 +346,14 @@ static void setPositionSensor(GPSPositionSensorData *gpsData)
PositionSensorData pos;
PositionSensorGet(&pos);
float ECEF[3];
float Rne[3][3];
double ECEF[3];
double Rne[3][3];
{
float LLA[3] = { (home.Latitude) / 10e6f, (home.Longitude) / 10e6f, (home.Altitude) };
LLA2ECEF(LLA, ECEF);
RneFromLLA(LLA, Rne);
}
{ float LLA[3] = { (gpsData->Latitude) / 10e6f, (gpsData->Longitude) / 10e6f, gpsData->Altitude + gpsData->GeoidSeparation };
{ float LLA[3] = { (gpsData->Latitude) / 10e6d, (gpsData->Longitude) / 10e6d, gpsData->Altitude + gpsData->GeoidSeparation };
float NED[3];
LLA2Base(LLA, ECEF, Rne, NED);
pos.North = NED[0];

21
flight/pios/inc/pios_math.h
View File

@ -45,10 +45,31 @@
#define M_LNPI_F 1.14472988584940017414342735135f /* ln(pi) */
#define M_EULER_F 0.57721566490153286060651209008f /* Euler constant */
#define M_E_D 2.71828182845904523536028747135d /* e */
#define M_LOG2E_D 1.44269504088896340735992468100d /* log_2 (e) */
#define M_LOG10E_D 0.43429448190325182765112891892d /* log_10 (e) */
#define M_SQRT2_D 1.41421356237309504880168872421d /* sqrt(2) */
#define M_SQRT1_2_D 0.70710678118654752440084436210d /* sqrt(1/2) */
#define M_SQRT3_D 1.73205080756887729352744634151d /* sqrt(3) */
#define M_PI_D 3.14159265358979323846264338328d /* pi */
#define M_PI_2_D 1.57079632679489661923132169164d /* pi/2 */
#define M_PI_4_D 0.78539816339744830961566084582d /* pi/4 */
#define M_SQRTPI_D 1.77245385090551602729816748334d /* sqrt(pi) */
#define M_2_SQRTPI_D 1.12837916709551257389615890312d /* 2/sqrt(pi) */
#define M_1_PI_D 0.31830988618379067153776752675d /* 1/pi */
#define M_2_PI_D 0.63661977236758134307553505349d /* 2/pi */
#define M_LN10_D 2.30258509299404568401799145468d /* ln(10) */
#define M_LN2_D 0.69314718055994530941723212146d /* ln(2) */
#define M_LNPI_D 1.14472988584940017414342735135d /* ln(pi) */
#define M_EULER_D 0.57721566490153286060651209008d /* Euler constant */
// Conversion macro
#define RAD2DEG(rad) ((rad) * (180.0f / M_PI_F))
#define DEG2RAD(deg) ((deg) * (M_PI_F / 180.0f))
#define RAD2DEG_D(rad) ((rad) * (180.0d / M_PI_D))
#define DEG2RAD_D(deg) ((deg) * (M_PI_D / 180.0d))
// Useful math macros
#define MAX(a, b) ((a) > (b) ? (a) : (b))
#define MIN(a, b) ((a) < (b) ? (a) : (b))