Merge branch 'alt_mag_nulling' into revo

flight/Modules/Sensors/sensors.c

@@ -48,7 +48,9 @@
 
 #include "pios.h"
 #include "attitude.h"
+#include "homelocation.h"
 #include "magnetometer.h"
+#include "magbias.h"
 #include "accels.h"
 #include "gyros.h"
 #include "gyrosbias.h"
@@ -56,14 +58,12 @@
 #include "attitudesettings.h"
 #include "revocalibration.h"
 #include "flightstatus.h"
-#include "gpsposition.h"
-#include "baroaltitude.h"
 #include "CoordinateConversions.h"
 
 #include <pios_board_info.h>
 
 // Private constants
-#define STACK_SIZE_BYTES 700
+#define STACK_SIZE_BYTES 1000
 #define TASK_PRIORITY (tskIDLE_PRIORITY+3)
 #define SENSOR_PERIOD 2
 
@@ -71,15 +71,15 @@
 #define PI_MOD(x) (fmodf(x + F_PI, F_PI * 2) - F_PI)
 // Private types
 
-// Private variables
-static xTaskHandle sensorsTaskHandle;
-static bool gps_updated = false;
-static bool baro_updated = false;
 
 // Private functions
 static void SensorsTask(void *parameters);
 static void settingsUpdatedCb(UAVObjEvent * objEv);
-static void sensorsUpdatedCb(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;
@@ -111,6 +111,7 @@ int32_t SensorsInitialize(void)
 	GyrosBiasInitialize();
 	AccelsInitialize();
 	MagnetometerInitialize();
+	MagBiasInitialize();
 	RevoCalibrationInitialize();
 	AttitudeSettingsInitialize();
 
@@ -206,12 +207,6 @@ static void SensorsTask(void *parameters)
 		}
 	}
 	
-	// If debugging connect callback
-	if(pios_com_aux_id != 0) {
-		BaroAltitudeConnectCallback(&sensorsUpdatedCb);
-		GPSPositionConnectCallback(&sensorsUpdatedCb);
-	}
-	
 	// Main task loop
 	lastSysTime = xTaskGetTickCount();
 	bool error = false;
@@ -407,6 +402,11 @@ static void SensorsTask(void *parameters)
 				mag.y = mags[1];
 				mag.z = mags[2];
 			}
+			
+			// 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();
 		}
@@ -419,21 +419,108 @@ static void SensorsTask(void *parameters)
 }
 
 /**
- * Indicate that these sensors have been updated
+ * Perform an update of the @ref MagBias based on
+ * Magnetometer Offset Cancellation: Theory and Implementation, 
+ * revisited William Premerlani, October 14, 2011
  */
-static void sensorsUpdatedCb(UAVObjEvent * objEv)
+static void magOffsetEstimation(MagnetometerData *mag)
 {
-	if(objEv->obj == GPSPositionHandle())
-		gps_updated = true;
-	if(objEv->obj == BaroAltitudeHandle())
-		baro_updated = true;
+#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
+	MagBiasData magBias;
+	MagBiasGet(&magBias);
+	
+	// Remove the current estimate of the bias
+	mag->x -= magBias.x;
+	mag->y -= magBias.y;
+	mag->z -= magBias.z;
+	
+	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
 }
 
 /**
  * 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];
@@ -450,6 +537,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);

flight/Modules/Sensors/simulated/sensors.c

@@ -63,6 +63,7 @@
 #include "gpsvelocity.h"
 #include "homelocation.h"
 #include "magnetometer.h"
+#include "magbias.h"
 #include "ratedesired.h"
 #include "revocalibration.h"
 
@@ -87,6 +88,8 @@ static void simulateModelAgnostic();
 static void simulateModelQuadcopter();
 static void simulateModelAirplane();
 
+static void magOffsetEstimation(MagnetometerData *mag);
+
 static float accel_bias[3];
 
 static float rand_gauss();
@@ -114,6 +117,7 @@ int32_t SensorsInitialize(void)
 	GPSPositionInitialize();
 	GPSVelocityInitialize();
 	MagnetometerInitialize();
+	MagBiasInitialize();
 	RevoCalibrationInitialize();
 
 	return 0;
@@ -503,6 +507,10 @@ static void simulateModelQuadcopter()
 		mag.x = homeLocation.Be[0] * Rbe[0][0] + homeLocation.Be[1] * Rbe[0][1] + homeLocation.Be[2] * Rbe[0][2];
 		mag.y = homeLocation.Be[0] * Rbe[1][0] + homeLocation.Be[1] * Rbe[1][1] + homeLocation.Be[2] * Rbe[1][2];
 		mag.z = homeLocation.Be[0] * Rbe[2][0] + homeLocation.Be[1] * Rbe[2][1] + homeLocation.Be[2] * Rbe[2][2];
+
+		// Run the offset compensation algorithm from the firmware
+		magOffsetEstimation(&mag);
+
 		MagnetometerSet(&mag);
 		last_mag_time = PIOS_DELAY_GetRaw();
 	}
@@ -779,9 +787,10 @@ static void simulateModelAirplane()
 	static uint32_t last_mag_time = 0;
 	if(PIOS_DELAY_DiffuS(last_mag_time) / 1.0e6 > MAG_PERIOD) {
 		MagnetometerData mag;
-		mag.x = homeLocation.Be[0] * Rbe[0][0] + homeLocation.Be[1] * Rbe[0][1] + homeLocation.Be[2] * Rbe[0][2];
-		mag.y = homeLocation.Be[0] * Rbe[1][0] + homeLocation.Be[1] * Rbe[1][1] + homeLocation.Be[2] * Rbe[1][2];
-		mag.z = homeLocation.Be[0] * Rbe[2][0] + homeLocation.Be[1] * Rbe[2][1] + homeLocation.Be[2] * Rbe[2][2];
+		mag.x = 100+homeLocation.Be[0] * Rbe[0][0] + homeLocation.Be[1] * Rbe[0][1] + homeLocation.Be[2] * Rbe[0][2];
+		mag.y = 100+homeLocation.Be[0] * Rbe[1][0] + homeLocation.Be[1] * Rbe[1][1] + homeLocation.Be[2] * Rbe[1][2];
+		mag.z = 100+homeLocation.Be[0] * Rbe[2][0] + homeLocation.Be[1] * Rbe[2][1] + homeLocation.Be[2] * Rbe[2][2];
+		magOffsetEstimation(&mag);
 		MagnetometerSet(&mag);
 		last_mag_time = PIOS_DELAY_GetRaw();
 	}
@@ -818,6 +827,107 @@ static float rand_gauss (void) {
 		return (v1*sqrt(-2.0 * log(s) / s));
 }
 
+/**
+ * Perform an update of the @ref MagBias based on
+ * Magnetometer Offset Cancellation: Theory and Implementation, 
+ * revisited William Premerlani, October 14, 2011
+ */
+static void magOffsetEstimation(MagnetometerData *mag)
+{
+#if 0
+	RevoCalibrationData cal;
+	RevoCalibrationGet(&cal);
+
+	// 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
+	HomeLocationData homeLocation;
+	HomeLocationGet(&homeLocation);
+	
+	AttitudeActualData attitude;
+	AttitudeActualGet(&attitude);
+	
+	MagBiasData magBias;
+	MagBiasGet(&magBias);
+	
+	// Remove the current estimate of the bias
+	mag->x -= magBias.x;
+	mag->y -= magBias.y;
+	mag->z -= magBias.z;
+	
+	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 = 0.01;
+	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]);
+	
+	magBias.x += delta[0];
+	magBias.y += delta[1];
+	magBias.z += delta[2];
+	MagBiasSet(&magBias);
+#endif
+
+}
+
 /**
   * @}
   * @}

flight/PiOS/STM32F4xx/pios_overo.c

@@ -127,7 +127,7 @@ static void PIOS_OVERO_WriteData(struct pios_overo_dev *overo_dev)
 
 			bytes_added = (overo_dev->tx_out_cb)(overo_dev->tx_out_context, writing_pointer, max_bytes, NULL, &tx_need_yield);
 
-#if OVERO_USES_BLOCKING_WRITE
+#if defined(OVERO_USES_BLOCKING_WRITE)
 			if (tx_need_yield) {
 				vPortYieldFromISR();
 			}
@@ -326,7 +326,7 @@ static void PIOS_OVERO_TxStart(uint32_t overo_id, uint16_t tx_bytes_avail)
 	// DMA TX enable (enable IRQ) ?
 
 	// Load any pending bytes from TX fifo
-	PIOS_OVERO_WriteData(overo_dev);
+	//PIOS_OVERO_WriteData(overo_dev);
 }
 
 static void PIOS_OVERO_RegisterRxCallback(uint32_t overo_id, pios_com_callback rx_in_cb, uint32_t context)

flight/Revolution/UAVObjects.inc

@@ -35,6 +35,7 @@ UAVOBJSRCFILENAMES += gyros
 UAVOBJSRCFILENAMES += gyrosbias
 UAVOBJSRCFILENAMES += accels
 UAVOBJSRCFILENAMES += magnetometer
+UAVOBJSRCFILENAMES += magbias
 UAVOBJSRCFILENAMES += baroaltitude
 UAVOBJSRCFILENAMES += baroairspeed
 UAVOBJSRCFILENAMES += fixedwingpathfollowersettings

ground/openpilotgcs/src/plugins/config/configrevowidget.cpp

@@ -535,6 +535,10 @@ void ConfigRevoWidget::doStartSixPointCalibration()
     revoCalibrationData.mag_bias[RevoCalibration::MAG_BIAS_Y] = 0;
     revoCalibrationData.mag_bias[RevoCalibration::MAG_BIAS_Z] = 0;
 
+    // Disable adaptive mag nulling
+    initialMagCorrectionRate = revoCalibrationData.MagBiasNullingRate;
+    revoCalibrationData.MagBiasNullingRate = 0;
+
     revoCalibration->setData(revoCalibrationData);
 
     Thread::usleep(100000);
@@ -740,6 +744,8 @@ void ConfigRevoWidget::computeScaleBias()
    revoCalibrationData.mag_bias[RevoCalibration::MAG_BIAS_Y] = -sign(S[1]) * b[1];
    revoCalibrationData.mag_bias[RevoCalibration::MAG_BIAS_Z] = -sign(S[2]) * b[2];
 
+   // Restore the previous setting
+   revoCalibrationData.MagBiasNullingRate = initialMagCorrectionRate;
 
 #ifdef SIX_POINT_CAL_ACCEL
    bool good_calibration = true;

ground/openpilotgcs/src/plugins/config/configrevowidget.h

@@ -95,6 +95,7 @@ private:
     UAVObject::Metadata initialGyrosMdata;
     UAVObject::Metadata initialMagMdata;
     UAVObject::Metadata initialBaroMdata;
+    float initialMagCorrectionRate;
 
     int position;
 

ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro

@@ -38,6 +38,7 @@ HEADERS += $$UAVOBJECT_SYNTHETICS/accessorydesired.h \
     $$UAVOBJECT_SYNTHETICS/gyrosbias.h \
     $$UAVOBJECT_SYNTHETICS/accels.h \
     $$UAVOBJECT_SYNTHETICS/magnetometer.h \
+    $$UAVOBJECT_SYNTHETICS/magbias.h \
     $$UAVOBJECT_SYNTHETICS/camerastabsettings.h \
     $$UAVOBJECT_SYNTHETICS/flighttelemetrystats.h \
     $$UAVOBJECT_SYNTHETICS/fixedwingpathfollowersettings.h \
@@ -110,6 +111,7 @@ SOURCES += $$UAVOBJECT_SYNTHETICS/accessorydesired.cpp \
     $$UAVOBJECT_SYNTHETICS/gyros.cpp \
     $$UAVOBJECT_SYNTHETICS/gyrosbias.cpp \
     $$UAVOBJECT_SYNTHETICS/magnetometer.cpp \
+    $$UAVOBJECT_SYNTHETICS/magbias.cpp \
     $$UAVOBJECT_SYNTHETICS/camerastabsettings.cpp \
     $$UAVOBJECT_SYNTHETICS/flighttelemetrystats.cpp \
     $$UAVOBJECT_SYNTHETICS/systemstats.cpp \

ground/openpilotgcs/src/plugins/uavobjects/uavobjecttemplate.m

@@ -75,14 +75,15 @@ startTime=clock;
 while (1)
 	if (feof(fid)); break; end
 
+	try
 	%% Read message header
 	% get sync field (0x3C, 1 byte)
 	sync = fread(fid, 1, 'uint8');
 	if sync ~= correctSyncByte
-        	prebuf = [prebuf(2:end); sync];
-		wrongSyncByte = wrongSyncByte + 1;	
-       		continue
-    	end
+		prebuf = [prebuf(2:end); sync];
+		wrongSyncByte = wrongSyncByte + 1;
+		continue
+	end
     
 	% get msg type (quint8 1 byte ) should be 0x20, ignore the rest?
 	msgType = fread(fid, 1, 'uint8');
@@ -109,7 +110,7 @@ while (1)
 	end
 	
 	%% Read object
-	try
+
 	switch objID
 $(SWITCHCODE)
 		otherwise

shared/uavobjectdefinition/magbias.xml

@@ -0,0 +1,12 @@
+<xml>
+    <object name="MagBias" singleinstance="true" settings="false">
+        <description>The gyro data.</description>
+	<field name="x" units="mGau" type="float" elements="1"/>
+	<field name="y" units="mGau" type="float" elements="1"/>
+	<field name="z" units="mGau" type="float" elements="1"/>
+        <access gcs="readwrite" flight="readwrite"/>
+        <telemetrygcs acked="false" updatemode="manual" period="0"/>
+        <telemetryflight acked="false" updatemode="periodic" period="1000"/>
+        <logging updatemode="manual" period="0"/>
+    </object>
+</xml>

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"/>