rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2024-12-02 10:24:11 +01:00
Code Issues Releases Activity

Changes to mag nulling from D-Lite. Perform update only when we have a new

Browse Source 
vector sufficiently different from the previous one.
This commit is contained in:
James Cotton 2012-07-26 16:43:24 -05:00
parent 0ed5b84ee9
commit 880d58e4d9
1 changed files with 14 additions and 19 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
flight/Modules/Sensors/sensors.c
View File

@ -425,14 +425,10 @@ static void SensorsTask(void *parameters)
static void magOffsetEstimation(MagnetometerData *mag)
{
// Constants, to possibly go into a UAVO
static const int UPDATE_INTERVAL = 10;
static const float MIN_NORM_DIFFERENCE = 5;
static const float MIN_NORM_DIFFERENCE = 50;
static unsigned int call_count = 0;
static float B2[3] = {0, 0, 0};
call_count++;
MagBiasData magBias;
MagBiasGet(&magBias);
@ -448,7 +444,7 @@ static void magOffsetEstimation(MagnetometerData *mag)
B2[2] = mag->z;
return;
}
if (call_count % UPDATE_INTERVAL == 0) {
float B1[3] = {mag->x, mag->y, mag->z};
float norm_diff = sqrtf(powf(B2[0] - B1[0],2) + powf(B2[1] - B1[1],2) + powf(B2[2] - B1[2],2));
if (norm_diff > MIN_NORM_DIFFERENCE) {
@ -462,7 +458,6 @@ static void magOffsetEstimation(MagnetometerData *mag)
magBias.z += b_error[2];
MagBiasSet(&magBias);
}
// Store this value to compare against next update
B2[0] = B1[0]; B2[1] = B1[1]; B2[2] = B1[2];