From c587ceebfdb75b44b95fe1aa3b60d19dbf1e58fa Mon Sep 17 00:00:00 2001
From: James Cotton <peabody124@gmail.com>
Date: Wed, 25 Jul 2012 11:23:27 -0500
Subject: [PATCH] Make the mag offset nulling convergence rate come from the
 UAVO.  When it is set to zero nulling does not occur to allow us to still
 calibrate the sensors.

---
 flight/Modules/Sensors/sensors.c              | 25 +++++++++++++------
 .../uavobjectdefinition/revocalibration.xml   |  6 ++++-
 2 files changed, 23 insertions(+), 8 deletions(-)

diff --git a/flight/Modules/Sensors/sensors.c b/flight/Modules/Sensors/sensors.c
index 36546de30..f062fd148 100644
--- a/flight/Modules/Sensors/sensors.c
+++ b/flight/Modules/Sensors/sensors.c
@@ -70,14 +70,16 @@
 #define PI_MOD(x) (fmodf(x + F_PI, F_PI * 2) - F_PI)
 // Private types
 
-// Private variables
-static xTaskHandle sensorsTaskHandle;
 
 // Private functions
 static void SensorsTask(void *parameters);
 static void settingsUpdatedCb(UAVObjEvent * objEv);
 static void magOffsetEstimation(MagnetometerData *mag);
 
+// Private variables
+static xTaskHandle sensorsTaskHandle;
+RevoCalibrationData cal;
+
 // These values are initialized by settings but can be updated by the attitude algorithm
 static bool bias_correct_gyro = true;
 
@@ -400,8 +402,10 @@ static void SensorsTask(void *parameters)
 				mag.z = mags[2];
 			}
 			
-			// Correct for mag bias and update
-			magOffsetEstimation(&mag);
+			// Correct for mag bias and update if the rate is non zero
+			if(cal.MagBiasNullingRate > 0)
+				magOffsetEstimation(&mag);
+
 			MagnetometerSet(&mag);
 			mag_update_time = PIOS_DELAY_GetRaw();
 		}
@@ -423,7 +427,6 @@ 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 CONVERGENCE_RATE = 1.0f;
 
 	static unsigned int call_count = 0;
 	static float B2[3] = {0, 0, 0};
@@ -451,7 +454,7 @@ static void magOffsetEstimation(MagnetometerData *mag)
 		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 = CONVERGENCE_RATE * (norm_b2 - norm_b1) / norm_diff;
+			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];
@@ -470,7 +473,6 @@ static void magOffsetEstimation(MagnetometerData *mag)
  * Locally cache some variables from the AtttitudeSettings object
  */
 static void settingsUpdatedCb(UAVObjEvent * objEv) {
-	RevoCalibrationData cal;
 	RevoCalibrationGet(&cal);
 	
 	mag_bias[0] = cal.mag_bias[REVOCALIBRATION_MAG_BIAS_X];
@@ -487,6 +489,15 @@ static void settingsUpdatedCb(UAVObjEvent * objEv) {
 	accel_scale[2] = cal.accel_scale[REVOCALIBRATION_ACCEL_SCALE_Z];
 	// Do not store gyros_bias here as that comes from the state estimator and should be
 	// used from GyroBias directly
+	
+	// Zero out any adaptive tracking
+	MagBiasData magBias;
+	MagBiasGet(&magBias);
+	magBias.x = 0;
+	magBias.y = 0;
+	magBias.z = 0;
+	MagBiasSet(&magBias);
+	
 
 	AttitudeSettingsData attitudeSettings;
 	AttitudeSettingsGet(&attitudeSettings);
diff --git a/shared/uavobjectdefinition/revocalibration.xml b/shared/uavobjectdefinition/revocalibration.xml
index ee5722b6f..481190b5a 100644
--- a/shared/uavobjectdefinition/revocalibration.xml
+++ b/shared/uavobjectdefinition/revocalibration.xml
@@ -1,7 +1,7 @@
 <xml>
     <object name="RevoCalibration" singleinstance="true" settings="true">
         <description>Settings for the INS to control the algorithm and what is updated</description>
-	<field name="BiasCorrectedRaw" units="" type="enum" elements="1" options="FALSE,TRUE" defaultvalue="TRUE"/>
+
 
 	<!-- Sensor noises -->
         <field name="accel_bias" units="m/s" type="float" elementnames="X,Y,Z" defaultvalue="0,0,0"/>
@@ -18,6 +18,10 @@
 	<field name="gps_var" units="m^2" type="float" elementnames="Pos,Vel" defaultvalue="1,1"/>
 	<field name="baro_var" units="m^2" type="float" elements="1" defaultvalue="1"/>
 
+	<!-- These settings are related to how the sensors are post processed -->
+	<field name="BiasCorrectedRaw" units="" type="enum" elements="1" options="FALSE,TRUE" defaultvalue="TRUE"/>
+	<field name="MagBiasNullingRate" units="" type="float" elements="1" defaultvalue="0"/>
+
         <access gcs="readwrite" flight="readwrite"/>
         <telemetrygcs acked="true" updatemode="onchange" period="0"/>
         <telemetryflight acked="true" updatemode="onchange" period="0"/>