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

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.

Browse Source 
git-svn-id: svn://svn.openpilot.org/OpenPilot/trunk@1494 ebee16cc-31ac-478f-84a7-5cbb03baadba
This commit is contained in:
peabody124 2010-09-01 03:12:06 +00:00 committed by peabody124
parent b5f6452d7c
commit a377bea862
3 changed files with 27 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
flight/Project/OpenPilotOSX/OpenPilotOSX.xcodeproj/project.pbxproj
View File

@ -9,7 +9,6 @@
/* Begin PBXFileReference section */
65003B2D1212499100C183DD /* watchdog.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; path = watchdog.h; sourceTree = "<group>"; };
65003B2E1212499100C183DD /* watchdog.c */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.c; path = watchdog.c; sourceTree = "<group>"; };
65003B30121249CA00C183DD /* pios_wdg.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; name = pios_wdg.h; path = ../STM32F10x/pios_wdg.h; sourceTree = "<group>"; };
65003B31121249CA00C183DD /* pios_wdg.c */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.c; path = pios_wdg.c; sourceTree = "<group>"; };
651CF9E5120B5D8300EEFD70 /* pios_usb_hid_desc.c */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.c; path = pios_usb_hid_desc.c; sourceTree = "<group>"; };
651CF9E6120B5D8300EEFD70 /* pios_usb_hid_istr.c */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.c; path = pios_usb_hid_istr.c; sourceTree = "<group>"; };
@ -22,6 +21,8 @@
651CF9F3120B700D00EEFD70 /* usb_conf.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; path = usb_conf.h; sourceTree = "<group>"; };
65209A1812208B0600453371 /* baroaltitude.xml */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = text.xml; path = baroaltitude.xml; sourceTree = "<group>"; };
65209A1912208B0600453371 /* gpsposition.xml */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = text.xml; path = gpsposition.xml; sourceTree = "<group>"; };
6526645A122DF972006F9A3C /* pios_i2c_priv.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; path = pios_i2c_priv.h; sourceTree = "<group>"; };
6526645B122DF972006F9A3C /* pios_wdg.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; path = pios_wdg.h; sourceTree = "<group>"; };
65322D2F12283CCD0046CD7C /* NMEA.c */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.c; path = NMEA.c; sourceTree = "<group>"; };
65322D3012283CD60046CD7C /* NMEA.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; path = NMEA.h; sourceTree = "<group>"; };
65322D3B122841F60046CD7C /* gpstime.xml */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = text.xml; path = gpstime.xml; sourceTree = "<group>"; };
@ -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;

30
ground/src/libs/utils/coordinateconversions.cpp
View File

@ -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);

1
ground/src/libs/utils/coordinateconversions.h
View File

@ -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]);
};