From a377bea862f9e229ca623d6b74e4a95b74edeece Mon Sep 17 00:00:00 2001 From: peabody124 Date: Wed, 1 Sep 2010 03:12:06 +0000 Subject: [PATCH] Ground/CoordinateConversions: Found out from Dschin that rotation matrix inverse is rotation matrix transpose. Now should convert correctly from NED to ECEF. I will check tomorrow by frolicking around with a plane. git-svn-id: svn://svn.openpilot.org/OpenPilot/trunk@1494 ebee16cc-31ac-478f-84a7-5cbb03baadba --- .../OpenPilotOSX.xcodeproj/project.pbxproj | 6 ++-- .../src/libs/utils/coordinateconversions.cpp | 30 ++++++++++++++----- ground/src/libs/utils/coordinateconversions.h | 1 + 3 files changed, 27 insertions(+), 10 deletions(-) diff --git a/flight/Project/OpenPilotOSX/OpenPilotOSX.xcodeproj/project.pbxproj b/flight/Project/OpenPilotOSX/OpenPilotOSX.xcodeproj/project.pbxproj index be11cc8e5..7ed93838c 100644 --- a/flight/Project/OpenPilotOSX/OpenPilotOSX.xcodeproj/project.pbxproj +++ b/flight/Project/OpenPilotOSX/OpenPilotOSX.xcodeproj/project.pbxproj @@ -9,7 +9,6 @@ /* Begin PBXFileReference section */ 65003B2D1212499100C183DD /* watchdog.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; path = watchdog.h; sourceTree = ""; }; 65003B2E1212499100C183DD /* watchdog.c */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.c; path = watchdog.c; sourceTree = ""; }; - 65003B30121249CA00C183DD /* pios_wdg.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; name = pios_wdg.h; path = ../STM32F10x/pios_wdg.h; sourceTree = ""; }; 65003B31121249CA00C183DD /* pios_wdg.c */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.c; path = pios_wdg.c; sourceTree = ""; }; 651CF9E5120B5D8300EEFD70 /* pios_usb_hid_desc.c */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.c; path = pios_usb_hid_desc.c; sourceTree = ""; }; 651CF9E6120B5D8300EEFD70 /* pios_usb_hid_istr.c */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.c; path = pios_usb_hid_istr.c; sourceTree = ""; }; @@ -22,6 +21,8 @@ 651CF9F3120B700D00EEFD70 /* usb_conf.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; path = usb_conf.h; sourceTree = ""; }; 65209A1812208B0600453371 /* baroaltitude.xml */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = text.xml; path = baroaltitude.xml; sourceTree = ""; }; 65209A1912208B0600453371 /* gpsposition.xml */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = text.xml; path = gpsposition.xml; sourceTree = ""; }; + 6526645A122DF972006F9A3C /* pios_i2c_priv.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; path = pios_i2c_priv.h; sourceTree = ""; }; + 6526645B122DF972006F9A3C /* pios_wdg.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; path = pios_wdg.h; sourceTree = ""; }; 65322D2F12283CCD0046CD7C /* NMEA.c */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.c; path = NMEA.c; sourceTree = ""; }; 65322D3012283CD60046CD7C /* NMEA.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; path = NMEA.h; sourceTree = ""; }; 65322D3B122841F60046CD7C /* gpstime.xml */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = text.xml; path = gpstime.xml; sourceTree = ""; }; @@ -7197,6 +7198,8 @@ 65E8F03811EFF25C00BBF654 /* inc */ = { isa = PBXGroup; children = ( + 6526645A122DF972006F9A3C /* pios_i2c_priv.h */, + 6526645B122DF972006F9A3C /* pios_wdg.h */, 651CF9EF120B700D00EEFD70 /* pios_usb_hid_desc.h */, 651CF9F0120B700D00EEFD70 /* pios_usb_hid_istr.h */, 651CF9F1120B700D00EEFD70 /* pios_usb_hid_prop.h */, @@ -7230,7 +7233,6 @@ 65E8F05211EFF25C00BBF654 /* pios_usart_priv.h */, 65E8F05311EFF25C00BBF654 /* pios_usb.h */, 65E8F05511EFF25C00BBF654 /* pios_usb_hid.h */, - 65003B30121249CA00C183DD /* pios_wdg.h */, 65E8F05611EFF25C00BBF654 /* stm32f10x_conf.h */, ); name = inc; diff --git a/ground/src/libs/utils/coordinateconversions.cpp b/ground/src/libs/utils/coordinateconversions.cpp index 081beb771..96c7b98da 100644 --- a/ground/src/libs/utils/coordinateconversions.cpp +++ b/ground/src/libs/utils/coordinateconversions.cpp @@ -41,6 +41,24 @@ CoordinateConversions::CoordinateConversions() } +/** + * Get rotation matrix from ECEF to NED for that LLA + * @param[in] LLA Longitude latitude altitude for this location + * @param[out] Rne[3][3] Rotation matrix + */ +void CoordinateConversions::RneFromLLA(double LLA[3], double 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; +} + /** * Convert from LLA coordinates to ECEF coordinates * @param[in] LLA[3] latitude longitude alititude coordinates in @@ -117,19 +135,15 @@ int CoordinateConversions::GetLLA(double BaseECEFcm[3], double NED[3], double po double BaseECEFm[3] = {BaseECEFcm[0] / 100, BaseECEFcm[1] / 100, BaseECEFcm[2] / 100}; double BaseLLA[3]; double ECEF[3]; + double Rne [3][3]; // Get LLA address to compute conversion matrix ECEF2LLA(BaseECEFm, BaseLLA); + RneFromLLA(BaseLLA, Rne); - // TODO: Find/derive correct inverse of Rne using LLA - double RnePrime [3][3] = - {{1,0,0}, - {0,1,0}, - {0,0,1}}; - - /* P = ECEF + CM * NED */ + /* P = ECEF + Rne' * NED */ for(i = 0; i < 3; i++) - ECEF[i] = BaseECEFm[i] + RnePrime[i][0]*NED[0] + RnePrime[i][1]*NED[1] + RnePrime[i][2]*NED[2]; + ECEF[i] = BaseECEFm[i] + Rne[0][i]*NED[0] + Rne[1][i]*NED[1] + Rne[2][i]*NED[2]; ECEF2LLA(ECEF,position); diff --git a/ground/src/libs/utils/coordinateconversions.h b/ground/src/libs/utils/coordinateconversions.h index fa3168931..b3a8e1774 100644 --- a/ground/src/libs/utils/coordinateconversions.h +++ b/ground/src/libs/utils/coordinateconversions.h @@ -44,6 +44,7 @@ public: int GetLLA(double LLA[3], double NED[3], double position[3]); protected: + void RneFromLLA(double LLA[3], double Rne[3][3]); void LLA2ECEF(double LLA[3], double ECEF[3]); int ECEF2LLA(double ECEF[3], double LLA[3]); };