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

Change how we convert LLA to NED. Now it is done with a taylor expansion

Browse Source 
around the home LLA coordinate to avoid the conversion into ECEF coordinates.
This has the benefit of not requiring double precision math and uses less
operations.

Now we should remove the Rne and ECEF fields from HomeLocation as they are
unused
This commit is contained in:
James Cotton 2012-02-29 12:05:54 -06:00
parent e8cc7748af
commit 0eedaa1250
2 changed files with 83 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

51
flight/Modules/Attitude/revolution/attitude.c
View File

@ -86,6 +86,7 @@ static xQueueHandle gpsQueue;
static AttitudeSettingsData attitudeSettings;
static RevoSettingsData revoSettings;
static HomeLocationData homeLocation;
const uint32_t SENSOR_QUEUE_SIZE = 10;
// Private functions
@ -95,6 +96,8 @@ static int32_t updateAttitudeComplimentary(bool first_run);
static int32_t updateAttitudeINSGPS(bool first_run, bool outdoor_mode);
static void settingsUpdatedCb(UAVObjEvent * objEv);
static int32_t getNED(GPSPositionData * gpsPosition, float * NED);
/**
* API for sensor fusion algorithms:
* Configure(xQueueHandle gyro, xQueueHandle accel, xQueueHandle mag, xQueueHandle baro)
@ -138,7 +141,8 @@ int32_t AttitudeInitialize(void)
AttitudeSettingsConnectCallback(&settingsUpdatedCb);
RevoSettingsConnectCallback(&settingsUpdatedCb);
HomeLocationConnectCallback(&settingsUpdatedCb);
return 0;
}
@ -627,15 +631,8 @@ static int32_t updateAttitudeINSGPS(bool first_run, bool outdoor_mode)
vel[1] = gpsPosition.Groundspeed * sinf(gpsPosition.Heading * F_PI / 180.0f);
vel[2] = 0;
// convert from cm back to meters
float LLA[3] = {(float) gpsPosition.Latitude / 1e7f,
(float) gpsPosition.Longitude / 1e7f,
(float) (gpsPosition.GeoidSeparation + gpsPosition.Altitude)};
// put in local NED frame
float ECEF[3] = {(float) (home.ECEF[0] / 100.0f),
(float) (home.ECEF[1] / 100.0f),
(float) (home.ECEF[2] / 100.0f)};
LLA2Base(LLA, ECEF, (float (*)[3]) home.RNE, NED);
// Transform the GPS position into NED coordinates
getNED(&gpsPosition, NED);
// Store this for inspecting offline
NEDPositionData nedPos;
@ -683,10 +680,35 @@ static int32_t updateAttitudeINSGPS(bool first_run, bool outdoor_mode)
return 0;
}
/**
* @brief Convert the GPS LLA position into NED coordinates
* @note this method uses a taylor expansion around the home coordinates
* to convert to NED which allows it to be done with all floating
* calculations
* @param[in] Current GPS coordinates
* @param[out] NED frame coordinates
* @returns 0 for success, -1 for failure
*/
float T[3];
const float DEG2RAD = 3.141592653589793f / 180.0f;
static int32_t getNED(GPSPositionData * gpsPosition, float * NED)
{
float dL[3] = {(gpsPosition->Latitude - homeLocation.Latitude) / 10.0e6 * DEG2RAD,
(gpsPosition->Longitude - homeLocation.Longitude) / 10.0e6 * DEG2RAD,
(gpsPosition->Altitude - homeLocation.Altitude)};
NED[0] = T[0] * dL[0];
NED[1] = T[1] * dL[1];
NED[2] = T[2] * dL[2];
}
static void settingsUpdatedCb(UAVObjEvent * objEv)
{
float lat, lon, alt;
AttitudeSettingsGet(&attitudeSettings);
RevoSettingsGet(&revoSettings);
HomeLocationGet(&homeLocation);
GyrosBiasData gyrosBias;
GyrosBiasGet(&gyrosBias);
@ -694,6 +716,15 @@ static void settingsUpdatedCb(UAVObjEvent * objEv)
gyrosBias.y = attitudeSettings.GyroBias[ATTITUDESETTINGS_GYROBIAS_Y] / 100.0f;
gyrosBias.z = attitudeSettings.GyroBias[ATTITUDESETTINGS_GYROBIAS_Z] / 100.0f;
GyrosBiasSet(&gyrosBias);
// Compute matrix to convert deltaLLA to NED
lat = homeLocation.Latitude / 10.0e6f * DEG2RAD;
lon = homeLocation.Longitude / 10.0e6 * DEG2RAD;
alt = homeLocation.Altitude;
T[0] = alt+6.378137E6f;
T[1] = cosf(lat)*(alt+6.378137E6f);
T[2] = -1.0f;
}
/**
* @}

42
matlab/ins/LLA2NED_symbolic.m Normal file
View File

@ -0,0 +1,42 @@
function NED = LLA2NED_symbolic(lla, home)
DEG2RAD = pi / 180;
a = 6378137.0; % Equatorial Radius
e = 8.1819190842622e-2; % Eccentricity
lat = home(1) / 10e6 * DEG2RAD;
lon = home(2) / 10e6 * DEG2RAD;
alt = home(3);
delta = (lla - home);
delta(1:2) = delta(1:2) / 10e6 * DEG2RAD;
delta = reshape(delta,[],1);
N = sym('a / sqrt(1.0 - e * e * sin(lat) * sin(lat))'); %prime vertical radius of curvature
ECEF = [sym('(N + alt) * cos(lat) * cos(lon)'); ...
sym('(N + alt) * cos(lat) * sin(lon)'); ...
sym('((1 - e * e) * N + alt) * sin(lat)')];
ECEF = subs(ECEF, 'N', N);
ECEF = subs(ECEF, 'e', 0);
ECEF = subs(ECEF, 'a', a);
J = [diff(ECEF,'lat') diff(ECEF,'lon') diff(ECEF,'alt')];
Rne(1,1) = sym('-sin(lat) * cos(lon)');
Rne(1,2) = sym('-sin(lat) * sin(lon)');
Rne(1,3) = sym('cos(lat)');
Rne(2,1) = sym('-sin(lon)');
Rne(2,2) = sym('cos(lon)');
Rne(2,3) = 0;
Rne(3,1) = sym('-cos(lat) * cos(lon)');
Rne(3,2) = sym('-cos(lat) * sin(lon)');
Rne(3,3) = sym('-sin(lat)');
ccode(simplify(Rne * J))
NEDsymb = simplify(Rne * J) * delta;
NED = subs(subs(subs(NEDsymb,'lat',lat),'lon',lon),'alt',alt);
%delta = subs(subs(subs(J * delta,'lat',lat),'lon',lon),'alt',alt)
%Rne = subs(subs(subs(Rne,'lat',lat),'lon',lon),'alt',alt)