mirror of
https://bitbucket.org/librepilot/librepilot.git
synced 2024-11-28 06:24:10 +01:00
LP-368 - Add coordinate conversion functions, perform some cleanup/optimization
This commit is contained in:
Alessio Morale
parent
b356408e16
commit
4e78da41d3
@ -27,25 +27,29 @@
|
||||
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
|
||||
*/
|
||||
|
||||
#include <math.h>
|
||||
#include <stdint.h>
|
||||
#include <pios_math.h>
|
||||
#include "CoordinateConversions.h"
|
||||
#include "inc/CoordinateConversions.h"
|
||||
|
||||
#define MIN_ALLOWABLE_MAGNITUDE 1e-30f
|
||||
|
||||
// Equatorial Radius
|
||||
#define equatorial_radius 6378137.0d
|
||||
|
||||
// Eccentricity
|
||||
#define eccentricity 8.1819190842622e-2d
|
||||
#define equatorial_radius_sq (equatorial_radius * equatorial_radius)
|
||||
#define eccentricity_sq (eccentricity * eccentricity)
|
||||
|
||||
// ****** convert Lat,Lon,Alt to ECEF ************
|
||||
void LLA2ECEF(int32_t LLAi[3], double ECEF[3])
|
||||
void LLA2ECEF(const int32_t LLAi[3], float ECEF[3])
|
||||
{
|
||||
const double a = 6378137.0d; // Equatorial Radius
|
||||
const double e = 8.1819190842622e-2d; // Eccentricity
|
||||
const double e2 = e * e; // Eccentricity squared
|
||||
double sinLat, sinLon, cosLat, cosLon;
|
||||
double N;
|
||||
double LLA[3] = {
|
||||
(double)LLAi[0] * 1e-7d,
|
||||
(double)LLAi[1] * 1e-7d,
|
||||
(double)LLAi[2] * 1e-4d
|
||||
((double)LLAi[0]) * 1e-7d,
|
||||
((double)LLAi[1]) * 1e-7d,
|
||||
((double)LLAi[2]) * 1e-4d
|
||||
};
|
||||
|
||||
sinLat = sin(DEG2RAD_D(LLA[0]));
|
||||
@ -53,60 +57,53 @@ void LLA2ECEF(int32_t LLAi[3], double ECEF[3])
|
||||
cosLat = cos(DEG2RAD_D(LLA[0]));
|
||||
cosLon = cos(DEG2RAD_D(LLA[1]));
|
||||
|
||||
N = a / sqrt(1.0d - e2 * sinLat * sinLat); // prime vertical radius of curvature
|
||||
N = equatorial_radius / sqrt(1.0d - eccentricity_sq * sinLat * sinLat); // prime vertical radius of curvature
|
||||
|
||||
ECEF[0] = (N + LLA[2]) * cosLat * cosLon;
|
||||
ECEF[1] = (N + LLA[2]) * cosLat * sinLon;
|
||||
ECEF[2] = ((1.0d - e2) * N + LLA[2]) * sinLat;
|
||||
ECEF[0] = (float)((N + LLA[2]) * cosLat * cosLon);
|
||||
ECEF[1] = (float)((N + LLA[2]) * cosLat * sinLon);
|
||||
ECEF[2] = (float)(((1.0d - eccentricity_sq) * N + LLA[2]) * sinLat);
|
||||
}
|
||||
|
||||
// ****** convert ECEF to Lat,Lon,Alt (ITERATIVE!) *********
|
||||
uint16_t ECEF2LLA(double ECEF[3], float LLA[3])
|
||||
void ECEF2LLA(const float ECEF[3], int32_t LLA[3])
|
||||
{
|
||||
/**
|
||||
* LLA parameter is used to prime the iteration.
|
||||
* A position within 1 meter of the specified LLA
|
||||
* will be calculated within at most 3 iterations.
|
||||
* If unknown: Call with any valid LLA coordinate
|
||||
* will compute within at most 5 iterations.
|
||||
* Suggestion: [0,0,0]
|
||||
**/
|
||||
/* b = math.sqrt( asq * (1-esq) )
|
||||
bsq = b*b
|
||||
|
||||
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;
|
||||
ep = math.sqrt((asq - bsq)/bsq)
|
||||
p = math.sqrt( math.pow(x,2) + math.pow(y,2) )
|
||||
th = math.atan2(a*z, b*p)
|
||||
|
||||
#define MAX_ITER 10 // should not take more than 5 for valid coordinates
|
||||
#define ACCURACY 1.0e-11d // used to be e-14, but we don't need sub micrometer exact calculations
|
||||
lon = math.atan2(y,x)
|
||||
lat = math.atan2( (z + ep*ep *b * math.pow(math.sin(th),3) ), (p - esq*a*math.pow(math.cos(th),3)) )
|
||||
N = a/( math.sqrt(1-esq*math.pow(math.sin(lat),2)) )
|
||||
alt = p / math.cos(lat) - N*/
|
||||
|
||||
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;
|
||||
const double x = ECEF[0], y = ECEF[1], z = ECEF[2];
|
||||
|
||||
while (((delta > ACCURACY) || (delta < -ACCURACY))
|
||||
&& (iter < MAX_ITER)) {
|
||||
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;
|
||||
}
|
||||
const double b = sqrt(equatorial_radius_sq * (1 - eccentricity_sq));
|
||||
const double bsq = b * b;
|
||||
const double ep = sqrt((equatorial_radius_sq - bsq) / bsq);
|
||||
|
||||
LLA[0] = RAD2DEG_D(Lat);
|
||||
LLA[2] = NplusH - N;
|
||||
const double p = sqrt(x * x + y * y);
|
||||
const double th = atan2(equatorial_radius * z, b * p);
|
||||
|
||||
return iter < MAX_ITER;
|
||||
double lon = atan2(y, x);
|
||||
|
||||
const double lat = atan2(
|
||||
(z + ep * ep * b * pow(sin(th), 3)),
|
||||
(p - eccentricity_sq * equatorial_radius * pow(cos(th), 3))
|
||||
);
|
||||
const double N = equatorial_radius / (sqrt(1 - eccentricity_sq * pow(sin(lat), 2)));
|
||||
const double alt = p / cos(lat) - N;
|
||||
|
||||
LLA[0] = (int32_t)(RAD2DEG_D(lat) * 1e7d);
|
||||
LLA[1] = (int32_t)(RAD2DEG_D(lon) * 1e7d) % ((int32_t)(180 * 1e7d));
|
||||
LLA[2] = (int32_t)(alt * 1e4d);
|
||||
}
|
||||
|
||||
// ****** find ECEF to NED rotation matrix ********
|
||||
void RneFromLLA(int32_t LLAi[3], float Rne[3][3])
|
||||
void RneFromLLA(const int32_t LLAi[3], float Rne[3][3])
|
||||
{
|
||||
float sinLat, sinLon, cosLat, cosLon;
|
||||
|
||||
@ -249,16 +246,30 @@ void Quaternion2zB(const float q[4], float z[3])
|
||||
|
||||
|
||||
// ****** Express LLA in a local NED Base Frame ********
|
||||
void LLA2Base(int32_t LLAi[3], double BaseECEF[3], float Rne[3][3], float NED[3])
|
||||
void LLA2Base(const int32_t LLAi[3], const float BaseECEF[3], float Rne[3][3], float NED[3])
|
||||
{
|
||||
double ECEF[3];
|
||||
float diff[3];
|
||||
float ECEF[3];
|
||||
|
||||
LLA2ECEF(LLAi, ECEF);
|
||||
ECEF2Base(ECEF, BaseECEF, Rne, NED);
|
||||
}
|
||||
|
||||
diff[0] = (float)(ECEF[0] - BaseECEF[0]);
|
||||
diff[1] = (float)(ECEF[1] - BaseECEF[1]);
|
||||
diff[2] = (float)(ECEF[2] - BaseECEF[2]);
|
||||
// ****** Express LLA in a local NED Base Frame ********
|
||||
void Base2LLA(const float NED[3], const float BaseECEF[3], float Rne[3][3], int32_t LLAi[3])
|
||||
{
|
||||
float ECEF[3];
|
||||
|
||||
Base2ECEF(NED, BaseECEF, Rne, ECEF);
|
||||
ECEF2LLA(ECEF, LLAi);
|
||||
}
|
||||
// ****** Express ECEF in a local NED Base Frame ********
|
||||
void ECEF2Base(const float ECEF[3], const float BaseECEF[3], float Rne[3][3], float NED[3])
|
||||
{
|
||||
float diff[3];
|
||||
|
||||
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];
|
||||
@ -266,19 +277,21 @@ void LLA2Base(int32_t LLAi[3], double BaseECEF[3], float Rne[3][3], float NED[3]
|
||||
}
|
||||
|
||||
// ****** Express ECEF in a local NED Base Frame ********
|
||||
void ECEF2Base(double ECEF[3], double BaseECEF[3], float Rne[3][3], float NED[3])
|
||||
void Base2ECEF(const float NED[3], const float BaseECEF[3], float Rne[3][3], float ECEF[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]);
|
||||
diff[0] = Rne[0][0] * NED[0] + Rne[1][0] * NED[1] + Rne[2][0] * NED[2];
|
||||
diff[1] = Rne[0][1] * NED[0] + Rne[1][1] * NED[1] + Rne[2][1] * NED[2];
|
||||
diff[2] = Rne[0][2] * NED[0] + Rne[1][2] * NED[1] + Rne[2][2] * NED[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];
|
||||
|
||||
ECEF[0] = diff[0] + BaseECEF[0];
|
||||
ECEF[1] = diff[1] + BaseECEF[1];
|
||||
ECEF[2] = diff[2] + BaseECEF[2];
|
||||
}
|
||||
|
||||
|
||||
// ****** convert Rotation Matrix to Quaternion ********
|
||||
// ****** if R converts from e to b, q is rotation from e to b ****
|
||||
void R2Quaternion(float R[3][3], float q[4])
|
||||
|
||||
@ -29,14 +29,23 @@
|
||||
|
||||
#ifndef COORDINATECONVERSIONS_H_
|
||||
#define COORDINATECONVERSIONS_H_
|
||||
#include <math.h>
|
||||
|
||||
// ****** convert Lat,Lon,Alt to ECEF ************
|
||||
void LLA2ECEF(int32_t LLAi[3], double ECEF[3]);
|
||||
void LLA2ECEF(const int32_t LLAi[3], float ECEF[3]);
|
||||
|
||||
// ****** convert ECEF to Lat,Lon,Alt (ITERATIVE!) *********
|
||||
uint16_t ECEF2LLA(double ECEF[3], float LLA[3]);
|
||||
// ****** convert ECEF to Lat,Lon,Alt *********
|
||||
void ECEF2LLA(const float ECEF[3], int32_t LLA[3]);
|
||||
|
||||
void RneFromLLA(int32_t LLAi[3], float Rne[3][3]);
|
||||
void RneFromLLA(const int32_t LLAi[3], float Rne[3][3]);
|
||||
|
||||
// ****** Express LLA in a local NED Base Frame and back ********
|
||||
void LLA2Base(const int32_t LLAi[3], const float BaseECEF[3], float Rne[3][3], float NED[3]);
|
||||
void Base2LLA(const float NED[3], const float BaseECEF[3], float Rne[3][3], int32_t LLAi[3]);
|
||||
|
||||
// ****** Express ECEF in a local NED Base Frame and back ********
|
||||
void ECEF2Base(const float ECEF[3], const float BaseECEF[3], float Rne[3][3], float NED[3]);
|
||||
void Base2ECEF(const float NED[3], const float BaseECEF[3], float Rne[3][3], float ECEF[3]);
|
||||
|
||||
// ****** find rotation matrix from rotation vector
|
||||
void Rv2Rot(float Rv[3], float R[3][3]);
|
||||
@ -65,12 +74,6 @@ void Quaternion2yB(const float q[4], float y[3]);
|
||||
void QuaternionC2zB(const float q0, const float q1, const float q2, const float q3, float z[3]);
|
||||
void Quaternion2zB(const float q[4], float z[3]);
|
||||
|
||||
// ****** Express LLA in a local NED Base Frame ********
|
||||
void LLA2Base(int32_t LLAi[3], double BaseECEF[3], float Rne[3][3], float NED[3]);
|
||||
|
||||
// ****** Express ECEF in a local NED Base Frame ********
|
||||
void ECEF2Base(double ECEF[3], double BaseECEF[3], float 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 ****
|
||||
void R2Quaternion(float R[3][3], float q[4]);
|
||||
|
||||
@ -44,7 +44,7 @@
|
||||
struct data {
|
||||
GPSSettingsData settings;
|
||||
HomeLocationData home;
|
||||
double HomeECEF[3];
|
||||
float HomeECEF[3];
|
||||
float HomeRne[3][3];
|
||||
};
|
||||
|
||||
|
||||
@ -32,7 +32,10 @@ endif
|
||||
include $(FLIGHT_ROOT_DIR)/make/firmware-defs.mk
|
||||
|
||||
EXTRAINCDIRS += $(TOPDIR)
|
||||
EXTRAINCDIRS += $(FLIGHT_ROOT_DIR)/flight/libraries/math
|
||||
EXTRAINCDIRS += $(FLIGHTLIB)/math
|
||||
EXTRAINCDIRS += $(FLIGHTLIB)
|
||||
EXTRAINCDIRS += $(PIOS)/inc
|
||||
|
||||
SRC += $(FLIGHTLIB)/CoordinateConversions.c
|
||||
|
||||
include $(FLIGHT_ROOT_DIR)/make/unittest.mk
|
||||
|
||||
82
flight/tests/math/coordinateconversiontest.cpp
Normal file
82
flight/tests/math/coordinateconversiontest.cpp
Normal file
@ -0,0 +1,82 @@
|
||||
#include "gtest/gtest.h"
|
||||
#define __STDC_WANT_DEC_FP__ /* Tell implementation that we want Decimal FP */
|
||||
|
||||
#include <stdio.h> /* printf */
|
||||
#include <stdlib.h> /* abort */
|
||||
#include <string.h> /* memset */
|
||||
|
||||
extern "C" {
|
||||
#include <inc/CoordinateConversions.h>
|
||||
}
|
||||
|
||||
#define epsilon_deg 0.00001f
|
||||
#define epsilon_int_deg ((int32_t)(epsilon_deg * 1e7))
|
||||
#define epsilon_metric 0.2f
|
||||
#define epsilon_int_metric ((int32_t)(epsilon_metric * 1e4))
|
||||
|
||||
// To use a test fixture, derive a class from testing::Test.
|
||||
class CoordinateConversionsTestRaw : public testing::Test {};
|
||||
|
||||
// ****** convert Lat,Lon,Alt to ECEF ************
|
||||
// void LLA2ECEF(const int32_t LLAi[3], float ECEF[3]);
|
||||
|
||||
// ****** convert ECEF to Lat,Lon,Alt *********
|
||||
// void ECEF2LLA(const float ECEF[3], int32_t LLA[3]);
|
||||
|
||||
// void RneFromLLA(const int32_t LLAi[3], float Rne[3][3]);
|
||||
|
||||
// ****** Express LLA in a local NED Base Frame and back ********
|
||||
// void LLA2Base(const int32_t LLAi[3], const float BaseECEF[3], float Rne[3][3], float NED[3]);
|
||||
// void Base2LLA(const float NED[3], const float BaseECEF[3], float Rne[3][3], int32_t LLAi[3]);
|
||||
|
||||
// ****** Express ECEF in a local NED Base Frame and back ********
|
||||
// void ECEF2Base(const float ECEF[3], const float BaseECEF[3], float Rne[3][3], float NED[3]);
|
||||
// void Base2ECEF(const float NED[3], const float BaseECEF[3], float Rne[3][3], float ECEF[3]
|
||||
|
||||
TEST_F(CoordinateConversionsTestRaw, LLA2ECEF) {
|
||||
int32_t LLAi[3] = {
|
||||
419291818,
|
||||
125571688,
|
||||
50 * 1e4
|
||||
};
|
||||
int32_t LLAfromECEF[3];
|
||||
|
||||
float ecef[3];
|
||||
|
||||
LLA2ECEF(LLAi, ecef);
|
||||
ECEF2LLA(ecef, LLAfromECEF);
|
||||
|
||||
EXPECT_NEAR(LLAi[0], LLAfromECEF[0], epsilon_int_deg);
|
||||
EXPECT_NEAR(LLAi[1], LLAfromECEF[1], epsilon_int_deg);
|
||||
EXPECT_NEAR(LLAi[2], LLAfromECEF[2], epsilon_int_metric);
|
||||
}
|
||||
|
||||
|
||||
TEST_F(CoordinateConversionsTestRaw, LLA2NED) {
|
||||
int32_t LLAi[3] = {
|
||||
419291818,
|
||||
125571688,
|
||||
50 * 1e4
|
||||
};
|
||||
int32_t LLAfromNED[3];
|
||||
|
||||
int32_t HomeLLAi[3] = {
|
||||
419291600,
|
||||
125571300,
|
||||
24 * 1e4
|
||||
};
|
||||
|
||||
float Rne[3][3];
|
||||
float baseECEF[3];
|
||||
float NED[3];
|
||||
|
||||
RneFromLLA(HomeLLAi, Rne);
|
||||
LLA2ECEF(HomeLLAi, baseECEF);
|
||||
|
||||
LLA2Base(LLAi, baseECEF, Rne, NED);
|
||||
Base2LLA(NED, baseECEF, Rne, LLAfromNED);
|
||||
|
||||
EXPECT_NEAR(LLAi[0], LLAfromNED[0], epsilon_int_deg);
|
||||
EXPECT_NEAR(LLAi[1], LLAfromNED[1], epsilon_int_deg);
|
||||
EXPECT_NEAR(LLAi[2], LLAfromNED[2], epsilon_int_metric);
|
||||
}
|
||||
Loading…
Reference in New Issue
Block a user