rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-04-08 00:53:48 +02:00
Code Issues Releases Activity

OP-191: Merge from full-calibration branch. Add the local gravitational field strength to the home location.

Browse Source 
git-svn-id: svn://svn.openpilot.org/OpenPilot/trunk@3055 ebee16cc-31ac-478f-84a7-5cbb03baadba
This commit is contained in:
jonathan 2011-03-21 00:48:32 +00:00 committed by jonathan
parent 8aef515456
commit 160695e546
2 changed files with 22 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

21
flight/Modules/GPS/GPS.c
View File

@ -54,6 +54,7 @@
static void gpsTask(void *parameters); static void gpsTask(void *parameters);
static void setHomeLocation(GPSPositionData * gpsData); static void setHomeLocation(GPSPositionData * gpsData);
static float GravityAccel(float latitude, float longitude, float altitude);
// **************** // ****************
// Private constants // Private constants
@ -333,6 +334,21 @@ static void gpsTask(void *parameters)
} }
} }
/*
* Estimate the acceleration due to gravity for a particular location in LLA
*/
static float GravityAccel(float latitude, float longitude, float altitude)
{
// WGS84 gravity model. The effect of gravity over latitude is strong
// enough to change the estimated accelerometer bias in those apps.
double sinsq = sin((double)latitude);
sinsq *= sinsq;
// Likewise, over the altitude range of a high-altitude balloon, the effect
// due to change in altitude can also affect the model.
return (float)(9.7803267714 * (1 + 0.00193185138639*sinsq) / sqrt(1 - 0.00669437999013*sinsq)
- 3.086e-6*altitude);
}
// **************** // ****************
static void setHomeLocation(GPSPositionData * gpsData) static void setHomeLocation(GPSPositionData * gpsData)
@ -363,6 +379,11 @@ static void setHomeLocation(GPSPositionData * gpsData)
// Compute magnetic flux direction at home location // Compute magnetic flux direction at home location
if (WMM_GetMagVector(LLA[0], LLA[1], LLA[2], gps.Month, gps.Day, gps.Year, &home.Be[0]) >= 0) if (WMM_GetMagVector(LLA[0], LLA[1], LLA[2], gps.Month, gps.Day, gps.Year, &home.Be[0]) >= 0)
{ // calculations appeared to go OK { // calculations appeared to go OK
// Compute local acceleration due to gravity. Vehicles that span a very large
// range of altitude (say, weather balloons) may need to update this during the
// flight.
home.g_e = GravityAccel(LLA[0], LLA[1], LLA[2]);
home.Set = HOMELOCATION_SET_TRUE; home.Set = HOMELOCATION_SET_TRUE;
HomeLocationSet(&home); HomeLocationSet(&home);
} }

1
shared/uavobjectdefinition/homelocation.xml
View File

@ -8,6 +8,7 @@
<field name="ECEF" units="cm" type="int32" elements="3" defaultvalue="0,0,0"/> <field name="ECEF" units="cm" type="int32" elements="3" defaultvalue="0,0,0"/>
<field name="RNE" units="" type="float" elements="9" defaultvalue="0,0,0,0,0,0,0,0,0"/> <field name="RNE" units="" type="float" elements="9" defaultvalue="0,0,0,0,0,0,0,0,0"/>
<field name="Be" units="" type="float" elements="3" defaultvalue="0,0,0"/> <field name="Be" units="" type="float" elements="3" defaultvalue="0,0,0"/>
<field name="g_e" units="m/s^2" type="float" elements="1" defaultvalue="9.81"/>
<access gcs="readwrite" flight="readwrite"/> <access gcs="readwrite" flight="readwrite"/>
<telemetrygcs acked="true" updatemode="onchange" period="0"/> <telemetrygcs acked="true" updatemode="onchange" period="0"/>
<telemetryflight acked="true" updatemode="onchange" period="0"/> <telemetryflight acked="true" updatemode="onchange" period="0"/>