From 2f082dbf87a4d4178953386d056b9aff3a282787 Mon Sep 17 00:00:00 2001 From: Corvus Corax Date: Mon, 20 May 2013 11:24:57 +0200 Subject: [PATCH] added filter for magnetometer bias drift compensation --- flight/modules/StateEstimation/filtermag.c | 182 +++++++++++++++++++++ 1 file changed, 182 insertions(+) create mode 100644 flight/modules/StateEstimation/filtermag.c diff --git a/flight/modules/StateEstimation/filtermag.c b/flight/modules/StateEstimation/filtermag.c new file mode 100644 index 000000000..a54e99257 --- /dev/null +++ b/flight/modules/StateEstimation/filtermag.c @@ -0,0 +1,182 @@ +/** + ****************************************************************************** + * @addtogroup OpenPilotModules OpenPilot Modules + * @{ + * @addtogroup State Estimation + * @brief Acquires sensor data and computes state estimate + * @{ + * + * @file filtermag.c + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2013. + * @brief Magnetometer drift compensation, uses previous cycles + * AttitudeState for estimation + * + * @see The GNU Public License (GPL) Version 3 + * + ******************************************************************************/ +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ + +#include "inc/stateestimation.h" +#include +#include +#include + +#include + +// Private constants + +// Private types + +// Private variables + +static HomeLocationData homeLocation; +static RevoCalibrationData revoCalibration; +static float magBias[3] = { 0 }; + +// Private functions + +static int32_t init(void); +static int32_t filter(stateEstimation *state); +static void magOffsetEstimation(float mag[3]); + + +void filterMagInitialize(stateFilter *handle) +{ + handle->init = &init; + handle->filter = &filter; +} + +static int32_t init(void) +{ + magBias[0] = magBias[1] = magBias[2] = 0.0f; + HomeLocationGet(&homeLocation); + RevoCalibrationGet(&revoCalibration); + return 0; +} + +static int32_t filter(stateEstimation *state) +{ + if (ISSET(state->updated, mag_UPDATED)) { + if (revoCalibration.MagBiasNullingRate > 0) { + magOffsetEstimation(state->mag); + } + } + + return 0; +} + +/** + * Perform an update of the @ref MagBias based on + * Magmeter Offset Cancellation: Theory and Implementation, + * revisited William Premerlani, October 14, 2011 + */ +static void magOffsetEstimation(float mag[3]) +{ +#if 0 + // Constants, to possibly go into a UAVO + static const float MIN_NORM_DIFFERENCE = 50; + + static float B2[3] = { 0, 0, 0 }; + + MagBiasData magBias; + MagBiasGet(&magBias); + + // Remove the current estimate of the bias + mag->x -= magBias.x; + mag->y -= magBias.y; + mag->z -= magBias.z; + + // First call + if (B2[0] == 0 && B2[1] == 0 && B2[2] == 0) { + B2[0] = mag->x; + B2[1] = mag->y; + B2[2] = mag->z; + return; + } + + 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]; + } +#else // if 0 + + 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 = revoCalibration.MagBiasNullingRate; + float Rot[3][3]; + float B_e[3]; + float xy[2]; + float delta[3]; + + AttitudeStateData attitude; + AttitudeStateGet(&attitude); + + // Get the rotation matrix + Quaternion2R(&attitude.q1, Rot); + + // Rotate the mag into the NED frame + B_e[0] = Rot[0][0] * mag[0] + Rot[1][0] * mag[1] + Rot[2][0] * mag[2]; + B_e[1] = Rot[0][1] * mag[0] + Rot[1][1] * mag[1] + Rot[2][1] * mag[2]; + B_e[2] = Rot[0][2] * mag[0] + Rot[1][2] * mag[1] + Rot[2][2] * mag[2]; + + float cy = cosf(DEG2RAD(attitude.Yaw)); + float sy = sinf(DEG2RAD(attitude.Yaw)); + + 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 (!isnan(delta[0]) && !isinf(delta[0]) && + !isnan(delta[1]) && !isinf(delta[1]) && + !isnan(delta[2]) && !isinf(delta[2])) { + magBias[0] += delta[0]; + magBias[1] += delta[1]; + magBias[2] += delta[2]; + } + + // Add bias to state estimation + mag[0] += magBias[0]; + mag[1] += magBias[1]; + mag[2] += magBias[2]; + +#endif // if 0 +} + + +/** + * @} + * @} + */