From 6dbad243db7b423389de456f134977eb653ddd3b Mon Sep 17 00:00:00 2001 From: James Cotton Date: Wed, 14 Dec 2011 02:08:14 -0600 Subject: [PATCH] Convert the CoordinateConversion functions to all be float until I determine how to use doubles safely on F4. --- flight/Libraries/CoordinateConversions.c | 28 ++++++++++---------- flight/Libraries/inc/CoordinateConversions.h | 10 +++---- 2 files changed, 19 insertions(+), 19 deletions(-) diff --git a/flight/Libraries/CoordinateConversions.c b/flight/Libraries/CoordinateConversions.c index 8e75a2935..732dcb92d 100644 --- a/flight/Libraries/CoordinateConversions.c +++ b/flight/Libraries/CoordinateConversions.c @@ -36,12 +36,12 @@ #define DEG2RAD (F_PI /180.0f) // ****** convert Lat,Lon,Alt to ECEF ************ -void LLA2ECEF(double LLA[3], double ECEF[3]) +void LLA2ECEF(float LLA[3], float ECEF[3]) { - const double a = 6378137.0; // Equatorial Radius - const double e = 8.1819190842622e-2; // Eccentricity - double sinLat, sinLon, cosLat, cosLon; - double N; + const float a = 6378137.0; // Equatorial Radius + const float e = 8.1819190842622e-2; // Eccentricity + float sinLat, sinLon, cosLat, cosLon; + float N; sinLat = sin(DEG2RAD * LLA[0]); sinLon = sin(DEG2RAD * LLA[1]); @@ -56,7 +56,7 @@ void LLA2ECEF(double LLA[3], double ECEF[3]) } // ****** convert ECEF to Lat,Lon,Alt (ITERATIVE!) ********* -uint16_t ECEF2LLA(double ECEF[3], double LLA[3]) +uint16_t ECEF2LLA(float ECEF[3], float LLA[3]) { /** * LLA parameter is used to prime the iteration. @@ -67,10 +67,10 @@ uint16_t ECEF2LLA(double ECEF[3], double LLA[3]) * Suggestion: [0,0,0] **/ - 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; + const float a = 6378137.0; // Equatorial Radius + const float e = 8.1819190842622e-2; // Eccentricity + float x = ECEF[0], y = ECEF[1], z = ECEF[2]; + float 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-11 // used to be e-14, but we don't need sub micrometer exact calculations @@ -100,7 +100,7 @@ uint16_t ECEF2LLA(double ECEF[3], double LLA[3]) } // ****** find ECEF to NED rotation matrix ******** -void RneFromLLA(double LLA[3], float Rne[3][3]) +void RneFromLLA(float LLA[3], float Rne[3][3]) { float sinLat, sinLon, cosLat, cosLon; @@ -189,9 +189,9 @@ void Quaternion2R(float q[4], float Rbe[3][3]) } // ****** Express LLA in a local NED Base Frame ******** -void LLA2Base(double LLA[3], double BaseECEF[3], float Rne[3][3], float NED[3]) +void LLA2Base(float LLA[3], float BaseECEF[3], float Rne[3][3], float NED[3]) { - double ECEF[3]; + float ECEF[3]; float diff[3]; LLA2ECEF(LLA, ECEF); @@ -206,7 +206,7 @@ void LLA2Base(double LLA[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 ECEF2Base(float ECEF[3], float BaseECEF[3], float Rne[3][3], float NED[3]) { float diff[3]; diff --git a/flight/Libraries/inc/CoordinateConversions.h b/flight/Libraries/inc/CoordinateConversions.h index 7f9013b28..020628d72 100644 --- a/flight/Libraries/inc/CoordinateConversions.h +++ b/flight/Libraries/inc/CoordinateConversions.h @@ -31,12 +31,12 @@ #define COORDINATECONVERSIONS_H_ // ****** convert Lat,Lon,Alt to ECEF ************ -void LLA2ECEF(double LLA[3], double ECEF[3]); +void LLA2ECEF(float LLA[3], float ECEF[3]); // ****** convert ECEF to Lat,Lon,Alt (ITERATIVE!) ********* -uint16_t ECEF2LLA(double ECEF[3], double LLA[3]); +uint16_t ECEF2LLA(float ECEF[3], float LLA[3]); -void RneFromLLA(double LLA[3], float Rne[3][3]); +void RneFromLLA(float LLA[3], float 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(double LLA[3], double BaseECEF[3], float Rne[3][3], float NED[3]); +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(double ECEF[3], double BaseECEF[3], float Rne[3][3], float NED[3]); +void ECEF2Base(float ECEF[3], float 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 ****