mirror of
https://bitbucket.org/librepilot/librepilot.git
synced 2025-03-15 07:29:15 +01:00
Perform the update every cycle. Mag rate of 0.001 seems to work well.
This commit is contained in:
parent
abb0caa6bd
commit
221ae05ef6
@ -465,8 +465,6 @@ static void magOffsetEstimation(MagnetometerData *mag)
|
||||
B2[0] = B1[0]; B2[1] = B1[1]; B2[2] = B1[2];
|
||||
}
|
||||
#else
|
||||
static uint32_t call_count = 0;
|
||||
|
||||
MagBiasData magBias;
|
||||
MagBiasGet(&magBias);
|
||||
|
||||
@ -474,49 +472,47 @@ static void magOffsetEstimation(MagnetometerData *mag)
|
||||
mag->x -= magBias.x;
|
||||
mag->y -= magBias.y;
|
||||
mag->z -= magBias.z;
|
||||
|
||||
if((call_count++ % 5) == 0) {
|
||||
HomeLocationData homeLocation;
|
||||
HomeLocationGet(&homeLocation);
|
||||
|
||||
AttitudeActualData attitude;
|
||||
AttitudeActualGet(&attitude);
|
||||
|
||||
const float Rxy = sqrtf(homeLocation.Be[0]*homeLocation.Be[0] + homeLocation.Be[1]*homeLocation.Be[1]);
|
||||
const float Rz = homeLocation.Be[2];
|
||||
|
||||
const float rate = cal.MagBiasNullingRate;
|
||||
float R[3][3];
|
||||
float B_e[3];
|
||||
float xy[2];
|
||||
float delta[3];
|
||||
|
||||
// Get the rotation matrix
|
||||
Quaternion2R(&attitude.q1, R);
|
||||
|
||||
// Rotate the mag into the NED frame
|
||||
B_e[0] = R[0][0] * mag->x + R[1][0] * mag->y + R[2][0] * mag->z;
|
||||
B_e[1] = R[0][1] * mag->x + R[1][1] * mag->y + R[2][1] * mag->z;
|
||||
B_e[2] = R[0][2] * mag->x + R[1][2] * mag->y + R[2][2] * mag->z;
|
||||
|
||||
float cy = cosf(attitude.Yaw * M_PI / 180.0f);
|
||||
float sy = sinf(attitude.Yaw * M_PI / 180.0f);
|
||||
|
||||
xy[0] = cy * B_e[0] + sy * B_e[1];
|
||||
xy[1] = -sy * B_e[0] + cy * B_e[1];
|
||||
|
||||
float xy_norm = sqrtf(xy[0]*xy[0] + xy[1]*xy[1]);
|
||||
|
||||
delta[0] = -rate * (xy[0] / xy_norm * Rxy - xy[0]);
|
||||
delta[1] = -rate * (xy[1] / xy_norm * Rxy - xy[1]);
|
||||
delta[2] = -rate * (Rz - B_e[2]);
|
||||
|
||||
if (delta[0] == delta[0] && delta[1] == delta[1] && delta[2] == delta[2]) {
|
||||
magBias.x += delta[0];
|
||||
magBias.y += delta[1];
|
||||
magBias.z += delta[2];
|
||||
MagBiasSet(&magBias);
|
||||
}
|
||||
|
||||
HomeLocationData homeLocation;
|
||||
HomeLocationGet(&homeLocation);
|
||||
|
||||
AttitudeActualData attitude;
|
||||
AttitudeActualGet(&attitude);
|
||||
|
||||
const float Rxy = sqrtf(homeLocation.Be[0]*homeLocation.Be[0] + homeLocation.Be[1]*homeLocation.Be[1]);
|
||||
const float Rz = homeLocation.Be[2];
|
||||
|
||||
const float rate = cal.MagBiasNullingRate;
|
||||
float R[3][3];
|
||||
float B_e[3];
|
||||
float xy[2];
|
||||
float delta[3];
|
||||
|
||||
// Get the rotation matrix
|
||||
Quaternion2R(&attitude.q1, R);
|
||||
|
||||
// Rotate the mag into the NED frame
|
||||
B_e[0] = R[0][0] * mag->x + R[1][0] * mag->y + R[2][0] * mag->z;
|
||||
B_e[1] = R[0][1] * mag->x + R[1][1] * mag->y + R[2][1] * mag->z;
|
||||
B_e[2] = R[0][2] * mag->x + R[1][2] * mag->y + R[2][2] * mag->z;
|
||||
|
||||
float cy = cosf(attitude.Yaw * M_PI / 180.0f);
|
||||
float sy = sinf(attitude.Yaw * M_PI / 180.0f);
|
||||
|
||||
xy[0] = cy * B_e[0] + sy * B_e[1];
|
||||
xy[1] = -sy * B_e[0] + cy * B_e[1];
|
||||
|
||||
float xy_norm = sqrtf(xy[0]*xy[0] + xy[1]*xy[1]);
|
||||
|
||||
delta[0] = -rate * (xy[0] / xy_norm * Rxy - xy[0]);
|
||||
delta[1] = -rate * (xy[1] / xy_norm * Rxy - xy[1]);
|
||||
delta[2] = -rate * (Rz - B_e[2]);
|
||||
|
||||
if (delta[0] == delta[0] && delta[1] == delta[1] && delta[2] == delta[2]) {
|
||||
magBias.x += delta[0];
|
||||
magBias.y += delta[1];
|
||||
magBias.z += delta[2];
|
||||
MagBiasSet(&magBias);
|
||||
}
|
||||
#endif
|
||||
}
|
||||
|
Loading…
x
Reference in New Issue
Block a user