rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2024-11-30 08: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

33
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,21 +444,20 @@ 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) {
float norm_b1 = sqrtf(B1[0]*B1[0] + B1[1]*B1[1] + B1[2]*B1[2]);
float norm_b2 = sqrtf(B2[0]*B2[0] + B2[1]*B2[1] + B2[2]*B2[2]);
float scale = cal.MagBiasNullingRate * (norm_b2 - norm_b1) / norm_diff;
float b_error[3] = {(B2[0] - B1[0]) * scale, (B2[1] - B1[1]) * scale, (B2[2] - B1[2]) * scale};
magBias.x += b_error[0];
magBias.y += b_error[1];
magBias.z += b_error[2];
MagBiasSet(&magBias);
}
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) {
float norm_b1 = sqrtf(B1[0]*B1[0] + B1[1]*B1[1] + B1[2]*B1[2]);
float norm_b2 = sqrtf(B2[0]*B2[0] + B2[1]*B2[1] + B2[2]*B2[2]);
float scale = cal.MagBiasNullingRate * (norm_b2 - norm_b1) / norm_diff;
float b_error[3] = {(B2[0] - B1[0]) * scale, (B2[1] - B1[1]) * scale, (B2[2] - B1[2]) * scale};
magBias.x += b_error[0];
magBias.y += b_error[1];
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];