LibrePilot/flight/AHRS/ahrs.c

/**
 ******************************************************************************
 * @addtogroup AHRS AHRS Control
 * @brief The AHRS Modules perform
 *
 * @{ 
 * @addtogroup AHRS_Main
 * @brief Main function which does the hardware dependent stuff
 * @{ 
 *
 *
 * @file       ahrs.c
 * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
 * @brief      INSGPS Test Program
 * @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
 */

/* OpenPilot Includes */
#include "ahrs.h"
#include "ahrs_adc.h"
#include "ahrs_timer.h"
#include "pios_opahrs_proto.h"
#include "ahrs_fsm.h"		/* lfsm_state */
#include "insgps.h"
#include "CoordinateConversions.h"

volatile enum algorithms ahrs_algorithm;

// For debugging the raw sensors
//#define DUMP_RAW
//#define DUMP_FRIENDLY
//#define DUMP_EKF

#ifdef DUMP_EKF
#define NUMX 13			// number of states, X is the state vector
#define NUMW 9			// number of plant noise inputs, w is disturbance noise vector
#define NUMV 10			// number of measurements, v is the measurement noise vector
#define NUMU 6			// number of deterministic inputs, U is the input vector
extern float F[NUMX][NUMX], G[NUMX][NUMW], H[NUMV][NUMX];	// linearized system matrices
extern float P[NUMX][NUMX], X[NUMX];	// covariance matrix and state vector
extern float Q[NUMW], R[NUMV];	// input noise and measurement noise variances
extern float K[NUMX][NUMV];	// feedback gain matrix
#endif

/**
 * @addtogroup AHRS_Definitions 
 * @{
 */
// Currently analog acquistion hard coded at 480 Hz
#define ADC_RATE          (4*480)
#define EKF_RATE          (ADC_RATE / adc_oversampling)
#define VDD            3.3	/* supply voltage for ADC */
#define FULL_RANGE     4096	/* 12 bit ADC */
#define ACCEL_RANGE    2	/* adjustable by FS input */
#define ACCEL_GRAVITY  9.81	/* m s^-1 */
#define ACCEL_SENSITIVITY    ( VDD / 5 )
#define ACCEL_SCALE    ( (VDD / FULL_RANGE) / ACCEL_SENSITIVITY * 2 / ACCEL_RANGE * ACCEL_GRAVITY )
#define ACCEL_OFFSET  -2048

#define GYRO_SENSITIVITY   ( 2.0 / 1000 )	/* 2 mV / (deg s^-1) */
#define RAD_PER_DEGREE     ( M_PI / 180 )
#define GYRO_SCALE         ( (VDD / FULL_RANGE) / GYRO_SENSITIVITY * RAD_PER_DEGREE )
#define GYRO_OFFSET       -1675	/* From data sheet, zero accel output is 1.35 v */

#define MAX_IDLE_COUNT          65e3
/**
 * @}
 */

/**
 * @addtogroup AHRS_Local Local Variables 
 * @{
 */
struct mag_sensor {
	uint8_t id[4];
	uint8_t updated;
	struct {
		int16_t axis[3];
	} raw;
};

struct accel_sensor {
	struct {
		uint16_t x;
		uint16_t y;
		uint16_t z;
	} raw;
	struct {
		float x;
		float y;
		float z;
	} filtered;
};

struct gyro_sensor {
	struct {
		uint16_t x;
		uint16_t y;
		uint16_t z;
	} raw;
	struct {
		float x;
		float y;
		float z;
	} filtered;
	struct {
		uint16_t xy;
		uint16_t z;
	} temp;
};

struct attitude_solution {
	struct {
		float q1;
		float q2;
		float q3;
		float q4;
	} quaternion;
};

struct altitude_sensor {
	float altitude;
	bool updated;
};

struct gps_sensor {
	float NED[3];
	float heading;
	float groundspeed;
	float quality;
	bool updated;
};

struct mag_sensor mag_data;
volatile struct accel_sensor accel_data;
volatile struct gyro_sensor gyro_data;
volatile struct altitude_sensor altitude_data;
struct gps_sensor gps_data;
volatile struct attitude_solution attitude_data;

/**
 * @}
 */

/* Function Prototypes */
void process_spi_request(void);
void downsample_data(void);
void calibrate_sensors(void);
void converge_insgps();

volatile uint32_t last_counter_idle_start = 0;
volatile uint32_t last_counter_idle_end = 0;
volatile uint32_t idle_counts;
volatile uint32_t running_counts;
uint32_t counter_val;

/**
 * @addtogroup AHRS_Global_Data AHRS Global Data
 * @{
 * Public data.  Used by both EKF and the sender
 */
//! Accelerometer variance after filter from OP or calibrate_sensors
float accel_var[3] = { 1, 1, 1 };

//! Gyro variance after filter from OP or calibrate sensors
float gyro_var[3] = { 1, 1, 1 };

//! Accelerometer scale after calibration
float accel_scale[3] = { ACCEL_SCALE, ACCEL_SCALE, ACCEL_SCALE };

//! Gyro scale after calibration
float gyro_scale[3] = { GYRO_SCALE, GYRO_SCALE, GYRO_SCALE };

//! Magnetometer variance from OP or calibrate sensors
float mag_var[3] = { 1, 1, 1 };

//! Accelerometer bias from OP or calibrate sensors
int16_t accel_bias[3] = { ACCEL_OFFSET, ACCEL_OFFSET, ACCEL_OFFSET };

//! Gyroscope bias term from OP or calibrate sensors
int16_t gyro_bias[3] = { 0, 0, 0 };

//! Magnetometer bias (direction) from OP or calibrate sensors
int16_t mag_bias[3] = { 0, 0, 0 };

//! Filter coefficients used in decimation.  Limited order so filter can't run between samples
int16_t fir_coeffs[50];
//! Home location in ECEF coordinates
double BaseECEF[3] = { 0, 0, 0 };

//! Rotation matrix from LLA to Rne
float Rne[3][3];
//! Indicates the communications are requesting a calibration
uint8_t calibration_pending = FALSE;
//! The oversampling rate, ekf is 2k / this
static uint8_t adc_oversampling = 25;
/**
 * @}
 */

/**
 * @brief AHRS Main function
 */
int main()
{
	float gyro[3], accel[3], mag[3];
	float vel[3] = { 0, 0, 0 };
	gps_data.quality = -1;

	ahrs_algorithm = INSGPS_Algo;

	/* Brings up System using CMSIS functions, enables the LEDs. */
	PIOS_SYS_Init();

	/* Delay system */
	PIOS_DELAY_Init();

	/* Communication system */
	PIOS_COM_Init();

	/* ADC system */
	AHRS_ADC_Config(adc_oversampling);

	/* Setup the Accelerometer FS (Full-Scale) GPIO */
	PIOS_GPIO_Enable(0);
	SET_ACCEL_2G;
#if defined(PIOS_INCLUDE_HMC5843) && defined(PIOS_INCLUDE_I2C)
	/* Magnetic sensor system */
	PIOS_I2C_Init();
	PIOS_HMC5843_Init();

	// Get 3 ID bytes
	strcpy((char *)mag_data.id, "ZZZ");
	PIOS_HMC5843_ReadID(mag_data.id);
#endif

	/* SPI link to master */
	PIOS_SPI_Init();

	lfsm_init();

	ahrs_state = AHRS_IDLE;

	/* Use simple averaging filter for now */
	for (int i = 0; i < adc_oversampling; i++)
		fir_coeffs[i] = 1;
	fir_coeffs[adc_oversampling] = adc_oversampling;

	if (ahrs_algorithm == INSGPS_Algo) {
		// compute a data point and initialize INS
		downsample_data();
		converge_insgps();
	}
#ifdef DUMP_RAW
	int previous_conversion;
	while (1) {
		int result;
		uint8_t framing[16] =
		    { 0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14,
	  15 };
		while (ahrs_state != AHRS_DATA_READY) ;
		ahrs_state = AHRS_PROCESSING;

		if (total_conversion_blocks != previous_conversion + 1)
			PIOS_LED_On(LED1);	// not keeping up
		else
			PIOS_LED_Off(LED1);
		previous_conversion = total_conversion_blocks;

		downsample_data();
		ahrs_state = AHRS_IDLE;;

		// Dump raw buffer
		result = PIOS_COM_SendBuffer(PIOS_COM_AUX, &framing[0], 16);	// framing header
		result += PIOS_COM_SendBuffer(PIOS_COM_AUX, (uint8_t *) & total_conversion_blocks, sizeof(total_conversion_blocks));	// dump block number
		result +=
		    PIOS_COM_SendBuffer(PIOS_COM_AUX,
					(uint8_t *) & valid_data_buffer[0],
					ADC_OVERSAMPLE *
					ADC_CONTINUOUS_CHANNELS *
					sizeof(valid_data_buffer[0]));
		if (result == 0)
			PIOS_LED_Off(LED1);
		else {
			PIOS_LED_On(LED1);
		}
	}
#endif

	timer_start();

    /******************* Main EKF loop ****************************/
	while (1) {

		// Alive signal
		if ((total_conversion_blocks % 100) == 0)
			PIOS_LED_Toggle(LED1);

		if (calibration_pending) {
			calibrate_sensors();
			calibration_pending = FALSE;
		}
#if defined(PIOS_INCLUDE_HMC5843) && defined(PIOS_INCLUDE_I2C)
		// Get magnetic readings
		if (PIOS_HMC5843_NewDataAvailable()) {
			PIOS_HMC5843_ReadMag(mag_data.raw.axis);
			mag_data.updated = 1;
		}
#endif
		// Delay for valid data

		counter_val = timer_count();
		running_counts = counter_val - last_counter_idle_end;
		last_counter_idle_start = counter_val;

		while (ahrs_state != AHRS_DATA_READY) ;

		counter_val = timer_count();
		idle_counts = counter_val - last_counter_idle_start;
		last_counter_idle_end = counter_val;

		ahrs_state = AHRS_PROCESSING;

		downsample_data();

	/***************** SEND BACK SOME RAW DATA ************************/
		// Hacky - grab one sample from buffer to populate this.  Need to send back
		// all raw data if it's happening
		accel_data.raw.x = valid_data_buffer[0];
		accel_data.raw.y = valid_data_buffer[2];
		accel_data.raw.z = valid_data_buffer[4];

		gyro_data.raw.x = valid_data_buffer[1];
		gyro_data.raw.y = valid_data_buffer[3];
		gyro_data.raw.z = valid_data_buffer[5];

		gyro_data.temp.xy = valid_data_buffer[6];
		gyro_data.temp.z = valid_data_buffer[7];

		if (ahrs_algorithm == INSGPS_Algo) {
	    /******************** INS ALGORITHM **************************/

			// format data for INS algo
			gyro[0] = gyro_data.filtered.x;
			gyro[1] = gyro_data.filtered.y;
			gyro[2] = gyro_data.filtered.z;
			accel[0] = accel_data.filtered.x,
			    accel[1] = accel_data.filtered.y,
			    accel[2] = accel_data.filtered.z,
			    // Note: The magnetometer driver returns registers X,Y,Z from the chip which are 
			    // (left, backward, up).  Remapping to (forward, right, down).
			    mag[0] = -(mag_data.raw.axis[1] - mag_bias[1]);
			mag[1] = -(mag_data.raw.axis[0] - mag_bias[0]);
			mag[2] = -(mag_data.raw.axis[2] - mag_bias[2]);

			INSStatePrediction(gyro, accel,
					   1 / (float)EKF_RATE);
			process_spi_request();
			INSCovariancePrediction(1 / (float)EKF_RATE);

			if (gps_data.updated && gps_data.quality == 1) {
				// Compute velocity from Heading and groundspeed
				vel[0] =
				    gps_data.groundspeed *
				    cos(gps_data.heading * M_PI / 180);
				vel[1] =
				    gps_data.groundspeed *
				    sin(gps_data.heading * M_PI / 180);

				// Completely unprincipled way to make the position variance
				// increase as data quality decreases but keep it bounded
				// Variance becomes 40 m^2 and 40 (m/s)^2 when no gps
				INSSetPosVelVar(0.004);
				if (gps_data.updated) {
					//TOOD: add check for altitude updates
					FullCorrection(mag, gps_data.NED,
						       vel,
						       altitude_data.
						       altitude);
					gps_data.updated = 0;
				} else {
					GpsBaroCorrection(gps_data.NED,
							  vel,
							  altitude_data.
							  altitude);
				}

				gps_data.updated = false;
				mag_data.updated = 0;
			} else if (gps_data.quality != -1
				   && mag_data.updated == 1) {
				MagCorrection(mag);	// only trust mags if outdoors
				mag_data.updated = 0;
			} else {
				// Indoors, update with zero position and velocity and high covariance
				INSSetPosVelVar(0.1);
				vel[0] = 0;
				vel[1] = 0;
				vel[2] = 0;

				VelBaroCorrection(vel,
						  altitude_data.altitude);
//                MagVelBaroCorrection(mag,vel,altitude_data.altitude);  // only trust mags if outdoors
			}

			attitude_data.quaternion.q1 = Nav.q[0];
			attitude_data.quaternion.q2 = Nav.q[1];
			attitude_data.quaternion.q3 = Nav.q[2];
			attitude_data.quaternion.q4 = Nav.q[3];
		} else if (ahrs_algorithm == SIMPLE_Algo) {
			float q[4];
			float rpy[3];
	    /***************** SIMPLE ATTITUDE FROM NORTH AND ACCEL ************/
			/* Very simple computation of the heading and attitude from accel. */
			rpy[2] =
			    atan2((mag_data.raw.axis[0]),
				  (-1 * mag_data.raw.axis[1])) * 180 /
			    M_PI;
			rpy[1] =
			    atan2(accel_data.filtered.x,
				  accel_data.filtered.z) * 180 / M_PI;
			rpy[0] =
			    atan2(accel_data.filtered.y,
				  accel_data.filtered.z) * 180 / M_PI;

			RPY2Quaternion(rpy, q);
			attitude_data.quaternion.q1 = q[0];
			attitude_data.quaternion.q2 = q[1];
			attitude_data.quaternion.q3 = q[2];
			attitude_data.quaternion.q4 = q[3];
			process_spi_request();

		}

		ahrs_state = AHRS_IDLE;

#ifdef DUMP_FRIENDLY
		PIOS_COM_SendFormattedStringNonBlocking(PIOS_COM_AUX, "b: %d\r\n",
							total_conversion_blocks);
		PIOS_COM_SendFormattedStringNonBlocking(PIOS_COM_AUX,"a: %d %d %d\r\n",
							(int16_t) (accel_data.filtered.x * 1000),
							(int16_t) (accel_data.filtered.y * 1000),
							(int16_t) (accel_data.filtered.z * 1000));
		PIOS_COM_SendFormattedStringNonBlocking(PIOS_COM_AUX, "g: %d %d %d\r\n",
							(int16_t) (gyro_data.filtered.x * 1000),
							(int16_t) (gyro_data.filtered.y * 1000),
							(int16_t) (gyro_data.filtered.z * 1000));
		PIOS_COM_SendFormattedStringNonBlocking(PIOS_COM_AUX,"m: %d %d %d\r\n",
							mag_data.raw.axis[0],
							mag_data.raw.axis[1],
							mag_data.raw.axis[2]);
		PIOS_COM_SendFormattedStringNonBlocking(PIOS_COM_AUX,
							"q: %d %d %d %d\r\n",
							(int16_t) (Nav.q[0] * 1000),
							(int16_t) (Nav.q[1] * 1000),
							(int16_t) (Nav.q[2] * 1000),
							(int16_t) (Nav.q[3] * 1000));
#endif
#ifdef DUMP_EKF
		uint8_t framing[16] =
		    { 15, 14, 13, 12, 11, 10, 9, 8, 7, 6, 5, 4, 3, 2, 1,
	  0 };
		extern float F[NUMX][NUMX], G[NUMX][NUMW], H[NUMV][NUMX];	// linearized system matrices
		extern float P[NUMX][NUMX], X[NUMX];	// covariance matrix and state vector
		extern float Q[NUMW], R[NUMV];	// input noise and measurement noise variances
		extern float K[NUMX][NUMV];	// feedback gain matrix

		// Dump raw buffer
		int8_t result;
		result = PIOS_COM_SendBuffer(PIOS_COM_AUX, &framing[0], 16);	// framing header
		result += PIOS_COM_SendBuffer(PIOS_COM_AUX, (uint8_t *) & total_conversion_blocks, sizeof(total_conversion_blocks));	// dump block number
		result +=
		    PIOS_COM_SendBuffer(PIOS_COM_AUX,
					(uint8_t *) & mag_data,
					sizeof(mag_data));
		result +=
		    PIOS_COM_SendBuffer(PIOS_COM_AUX,
					(uint8_t *) & gps_data,
					sizeof(gps_data));
		result +=
		    PIOS_COM_SendBuffer(PIOS_COM_AUX,
					(uint8_t *) & accel_data,
					sizeof(accel_data));
		result +=
		    PIOS_COM_SendBuffer(PIOS_COM_AUX,
					(uint8_t *) & gyro_data,
					sizeof(gyro_data));
		result +=
		    PIOS_COM_SendBuffer(PIOS_COM_AUX, (uint8_t *) & Q,
					sizeof(float) * NUMX * NUMX);
		result +=
		    PIOS_COM_SendBuffer(PIOS_COM_AUX, (uint8_t *) & K,
					sizeof(float) * NUMX * NUMV);
		result +=
		    PIOS_COM_SendBuffer(PIOS_COM_AUX, (uint8_t *) & X,
					sizeof(float) * NUMX * NUMX);

		if (result == 0)
			PIOS_LED_Off(LED1);
		else {
			PIOS_LED_On(LED1);
		}
#endif

	}

	return 0;
}

/**
 * @brief Downsample the analog data
 * @return none
 *
 * Tried to make as much of the filtering fixed point when possible.  Need to account
 * for offset for each sample before the multiplication if filter not a boxcar.  Could
 * precompute fixed offset as sum[fir_coeffs[i]] * ACCEL_OFFSET.  Puts data into global
 * data structures @ref accel_data and @ref gyro_data.
 * 
 * The accel_data values are converted into a coordinate system where X is forwards along
 * the fuselage, Y is along right the wing, and Z is down.
 */
void downsample_data()
{
	int32_t accel_raw[3], gyro_raw[3];
	uint16_t i;

	// Get the Y data.  Third byte in.  Convert to m/s
	accel_raw[0] = 0;
	for (i = 0; i < adc_oversampling; i++)
		accel_raw[0] +=
		    (valid_data_buffer[0 + i * PIOS_ADC_NUM_PINS] +
		     accel_bias[1]) * fir_coeffs[i];
	accel_data.filtered.y =
	    (float)accel_raw[0] / (float)fir_coeffs[adc_oversampling] *
	    accel_scale[1];

	// Get the X data which projects forward/backwards.  Fifth byte in.  Convert to m/s
	accel_raw[1] = 0;
	for (i = 0; i < adc_oversampling; i++)
		accel_raw[1] +=
		    (valid_data_buffer[2 + i * PIOS_ADC_NUM_PINS] +
		     accel_bias[0]) * fir_coeffs[i];
	accel_data.filtered.x =
	    (float)accel_raw[1] / (float)fir_coeffs[adc_oversampling] *
	    accel_scale[0];

	// Get the Z data.  Third byte in.  Convert to m/s
	accel_raw[2] = 0;
	for (i = 0; i < adc_oversampling; i++)
		accel_raw[2] +=
		    (valid_data_buffer[4 + i * PIOS_ADC_NUM_PINS] +
		     accel_bias[2]) * fir_coeffs[i];
	accel_data.filtered.z =
	    -(float)accel_raw[2] / (float)fir_coeffs[adc_oversampling] *
	    accel_scale[2];

	// Get the X gyro data.  Seventh byte in.  Convert to deg/s.
	gyro_raw[0] = 0;
	for (i = 0; i < adc_oversampling; i++)
		gyro_raw[0] +=
		    (valid_data_buffer[1 + i * PIOS_ADC_NUM_PINS] +
		     gyro_bias[0]) * fir_coeffs[i];
	gyro_data.filtered.x =
	    (float)gyro_raw[0] / (float)fir_coeffs[adc_oversampling] *
	    gyro_scale[0];

	// Get the Y gyro data.  Second byte in.  Convert to deg/s.
	gyro_raw[1] = 0;
	for (i = 0; i < adc_oversampling; i++)
		gyro_raw[1] +=
		    (valid_data_buffer[3 + i * PIOS_ADC_NUM_PINS] +
		     gyro_bias[1]) * fir_coeffs[i];
	gyro_data.filtered.y =
	    (float)gyro_raw[1] / (float)fir_coeffs[adc_oversampling] *
	    gyro_scale[1];

	// Get the Z gyro data.  Fifth byte in.  Convert to deg/s.
	gyro_raw[2] = 0;
	for (i = 0; i < adc_oversampling; i++)
		gyro_raw[2] +=
		    (valid_data_buffer[5 + i * PIOS_ADC_NUM_PINS] +
		     gyro_bias[2]) * fir_coeffs[i];
	gyro_data.filtered.z =
	    (float)gyro_raw[2] / (float)fir_coeffs[adc_oversampling] *
	    gyro_scale[2];
}

/**
 * @brief Assumes board is not moving computes biases and variances of sensors
 * @returns None
 * 
 * All data is stored in global structures.  This function should be called from OP when
 * aircraft is in stable state and then the data stored to SD card.
 */
void calibrate_sensors()
{
	int i;
	int16_t mag_raw[3] = { 0, 0, 0 };
	// local biases for noise analysis
	float accel_bias[3], gyro_bias[3], mag_bias[3];

	// run few loops to get mean
	gyro_bias[0] = gyro_bias[1] = gyro_bias[2] = 0;
	accel_bias[0] = accel_bias[1] = accel_bias[2] = 0;
	mag_bias[0] = mag_bias[1] = mag_bias[2] = 0;
	for (i = 0; i < 50; i++) {
		while (ahrs_state != AHRS_DATA_READY) ;
		ahrs_state = AHRS_PROCESSING;
		downsample_data();
		gyro_bias[0] += gyro_data.filtered.x;
		gyro_bias[1] += gyro_data.filtered.y;
		gyro_bias[2] += gyro_data.filtered.z;
		accel_bias[0] += accel_data.filtered.x;
		accel_bias[1] += accel_data.filtered.y;
		accel_bias[2] += accel_data.filtered.z;
#if defined(PIOS_INCLUDE_HMC5843) && defined(PIOS_INCLUDE_I2C)
		PIOS_HMC5843_ReadMag(mag_raw);
#endif
		mag_bias[0] += mag_raw[0];
		mag_bias[1] += mag_raw[1];
		mag_bias[2] += mag_raw[2];

		ahrs_state = AHRS_IDLE;
		process_spi_request();
	}
	gyro_bias[0] /= i;
	gyro_bias[1] /= i;
	gyro_bias[2] /= i;
	accel_bias[0] /= i;
	accel_bias[1] /= i;
	accel_bias[2] /= i;
	mag_bias[0] /= i;
	mag_bias[1] /= i;
	mag_bias[2] /= i;

	// more iterations for variance
	accel_var[0] = accel_var[1] = accel_var[2] = 0;
	gyro_var[0] = gyro_var[1] = gyro_var[2] = 0;
	mag_var[0] = mag_var[1] = mag_var[2] = 0;
	for (i = 0; i < 500; i++) {
		while (ahrs_state != AHRS_DATA_READY) ;
		ahrs_state = AHRS_PROCESSING;
		downsample_data();
		gyro_var[0] +=
		    (gyro_data.filtered.x -
		     gyro_bias[0]) * (gyro_data.filtered.x - gyro_bias[0]);
		gyro_var[1] +=
		    (gyro_data.filtered.y -
		     gyro_bias[1]) * (gyro_data.filtered.y - gyro_bias[1]);
		gyro_var[2] +=
		    (gyro_data.filtered.z -
		     gyro_bias[2]) * (gyro_data.filtered.z - gyro_bias[2]);
		accel_var[0] +=
		    (accel_data.filtered.x -
		     accel_bias[0]) * (accel_data.filtered.x -
				       accel_bias[0]);
		accel_var[1] +=
		    (accel_data.filtered.y -
		     accel_bias[1]) * (accel_data.filtered.y -
				       accel_bias[1]);
		accel_var[2] +=
		    (accel_data.filtered.z -
		     accel_bias[2]) * (accel_data.filtered.z -
				       accel_bias[2]);
#if defined(PIOS_INCLUDE_HMC5843) && defined(PIOS_INCLUDE_I2C)
		PIOS_HMC5843_ReadMag(mag_raw);
#endif
		mag_var[0] +=
		    (mag_raw[0] - mag_bias[0]) * (mag_raw[0] -
						  mag_bias[0]);
		mag_var[1] +=
		    (mag_raw[1] - mag_bias[1]) * (mag_raw[1] -
						  mag_bias[1]);
		mag_var[2] +=
		    (mag_raw[2] - mag_bias[2]) * (mag_raw[2] -
						  mag_bias[2]);
		ahrs_state = AHRS_IDLE;
		process_spi_request();
	}
	gyro_var[0] /= i;
	gyro_var[1] /= i;
	gyro_var[2] /= i;
	accel_var[0] /= i;
	accel_var[1] /= i;
	accel_var[2] /= i;
	mag_var[0] /= i;
	mag_var[1] /= i;
	mag_var[2] /= i;

	float mag_length2 =
	    mag_bias[0] * mag_bias[0] + mag_bias[1] * mag_bias[1] +
	    mag_bias[2] * mag_bias[2];
	mag_var[0] = mag_var[0] / mag_length2;
	mag_var[1] = mag_var[1] / mag_length2;
	mag_var[2] = mag_var[2] / mag_length2;

	if (ahrs_algorithm == INSGPS_Algo)
		converge_insgps();
}

/**
 * @brief Quickly initialize INS assuming stationary and gravity is down
 *
 * Currently this is done iteratively but I'm sure it can be directly computed
 * when I sit down and work it out
 */
void converge_insgps()
{
	float pos[3] = { 0, 0, 0 }, vel[3] = {
	0, 0, 0}, BaroAlt = 0, mag[3], accel[3], temp_gyro[3] = {
	0, 0, 0};
	INSGPSInit();
	INSSetAccelVar(accel_var);
	INSSetGyroBias(temp_gyro);	// set this to zero - crude bias corrected from downsample_data
	INSSetGyroVar(gyro_var);
	INSSetMagVar(mag_var);

	float temp_var[3] = { 10, 10, 10 };
	INSSetGyroVar(temp_var);	// ignore gyro's

	accel[0] = accel_data.filtered.x;
	accel[1] = accel_data.filtered.y;
	accel[2] = accel_data.filtered.z;

	// Iteratively constrain pitch and roll while updating yaw to align magnetic axis.  
	for (int i = 0; i < 50; i++) {
		// This should be done directly but I'm too dumb.
		float rpy[3];
		Quaternion2RPY(Nav.q, rpy);
		rpy[1] =
		    -atan2(accel_data.filtered.x,
			   accel_data.filtered.z) * 180 / M_PI;
		rpy[0] =
		    -atan2(accel_data.filtered.y,
			   accel_data.filtered.z) * 180 / M_PI;
		// Get magnetic readings
#if defined(PIOS_INCLUDE_HMC5843) && defined(PIOS_INCLUDE_I2C)
		PIOS_HMC5843_ReadMag(mag_data.raw.axis);
#endif
		mag[0] = -mag_data.raw.axis[1];
		mag[1] = -mag_data.raw.axis[0];
		mag[2] = -mag_data.raw.axis[2];

		RPY2Quaternion(rpy, Nav.q);
		INSStatePrediction(temp_gyro, accel, 1 / (float)EKF_RATE);
		INSCovariancePrediction(1 / (float)EKF_RATE);
		FullCorrection(mag, pos, vel, BaroAlt);
		process_spi_request();	// again we must keep this hear to prevent SPI connection dropping
	}

	INSSetGyroVar(gyro_var);

}

/**
 * @addtogroup AHRS_SPI SPI Messaging 
 * @{
 * @brief SPI protocol handling requests for data from OP mainboard
 */

static struct opahrs_msg_v1 link_tx_v1;
static struct opahrs_msg_v1 link_rx_v1;
static struct opahrs_msg_v1 user_rx_v1;
static struct opahrs_msg_v1 user_tx_v1;

void process_spi_request(void)
{
	bool msg_to_process = FALSE;

	PIOS_IRQ_Disable();
	/* Figure out if we're in an interesting stable state */
	switch (lfsm_get_state()) {
	case LFSM_STATE_USER_BUSY:
		msg_to_process = TRUE;
		break;
	case LFSM_STATE_INACTIVE:
		/* Queue up a receive buffer */
		lfsm_user_set_rx_v1(&user_rx_v1);
		lfsm_user_done();
		break;
	case LFSM_STATE_STOPPED:
		/* Get things going */
		lfsm_set_link_proto_v1(&link_tx_v1, &link_rx_v1);
		break;
	default:
		/* Not a stable state */
		break;
	}
	PIOS_IRQ_Enable();

	if (!msg_to_process) {
		/* Nothing to do */
		return;
	}

	switch (user_rx_v1.payload.user.t) {
	case OPAHRS_MSG_V1_REQ_RESET:
		PIOS_DELAY_WaitmS(user_rx_v1.payload.user.v.req.reset.
				  reset_delay_in_ms);
		PIOS_SYS_Reset();
		break;
	case OPAHRS_MSG_V1_REQ_SERIAL:
		opahrs_msg_v1_init_user_tx(&user_tx_v1,
					   OPAHRS_MSG_V1_RSP_SERIAL);
		PIOS_SYS_SerialNumberGet((char *)
					 &(user_tx_v1.payload.user.v.rsp.
					   serial.serial_bcd));
		lfsm_user_set_tx_v1(&user_tx_v1);
		break;
	case OPAHRS_MSG_V1_REQ_ALGORITHM:
		opahrs_msg_v1_init_user_tx(&user_tx_v1,
					   OPAHRS_MSG_V1_RSP_ALGORITHM);
		ahrs_algorithm =
		    user_rx_v1.payload.user.v.req.algorithm.algorithm;
		lfsm_user_set_tx_v1(&user_tx_v1);
		break;
	case OPAHRS_MSG_V1_REQ_NORTH:
		opahrs_msg_v1_init_user_tx(&user_tx_v1,
					   OPAHRS_MSG_V1_RSP_NORTH);
		INSSetMagNorth(user_rx_v1.payload.user.v.req.north.Be);
		lfsm_user_set_tx_v1(&user_tx_v1);
		break;
	case OPAHRS_MSG_V1_REQ_CALIBRATION:
		if (user_rx_v1.payload.user.v.req.calibration.
		    measure_var == AHRS_MEASURE) {
			calibration_pending = TRUE;
		} else if (user_rx_v1.payload.user.v.req.calibration.
			   measure_var == AHRS_SET) {
			accel_var[0] =
			    user_rx_v1.payload.user.v.req.calibration.
			    accel_var[0];
			accel_var[1] =
			    user_rx_v1.payload.user.v.req.calibration.
			    accel_var[1];
			accel_var[2] =
			    user_rx_v1.payload.user.v.req.calibration.
			    accel_var[2];
			gyro_bias[0] =
			    user_rx_v1.payload.user.v.req.calibration.
			    gyro_bias[0];
			gyro_bias[1] =
			    user_rx_v1.payload.user.v.req.calibration.
			    gyro_bias[1];
			gyro_bias[2] =
			    user_rx_v1.payload.user.v.req.calibration.
			    gyro_bias[2];
			gyro_var[0] =
			    user_rx_v1.payload.user.v.req.calibration.
			    gyro_var[0];
			gyro_var[1] =
			    user_rx_v1.payload.user.v.req.calibration.
			    gyro_var[1];
			gyro_var[2] =
			    user_rx_v1.payload.user.v.req.calibration.
			    gyro_var[2];
			mag_var[0] =
			    user_rx_v1.payload.user.v.req.calibration.
			    mag_var[0];
			mag_var[1] =
			    user_rx_v1.payload.user.v.req.calibration.
			    mag_var[1];
			mag_var[2] =
			    user_rx_v1.payload.user.v.req.calibration.
			    mag_var[2];
			INSSetAccelVar(accel_var);
			float gyro_bias_ins[3] = { 0, 0, 0 };
			INSSetGyroBias(gyro_bias_ins);	//gyro bias corrects in preprocessing
			INSSetGyroVar(gyro_var);
			INSSetMagVar(mag_var);
		}
		if (user_rx_v1.payload.user.v.req.calibration.
		    measure_var != AHRS_ECHO) {
			/* if echoing don't set anything */
			accel_bias[0] =
			    user_rx_v1.payload.user.v.req.calibration.
			    accel_bias[0];
			accel_bias[1] =
			    user_rx_v1.payload.user.v.req.calibration.
			    accel_bias[1];
			accel_bias[2] =
			    user_rx_v1.payload.user.v.req.calibration.
			    accel_bias[2];
			accel_scale[0] =
			    user_rx_v1.payload.user.v.req.calibration.
			    accel_scale[0];
			accel_scale[1] =
			    user_rx_v1.payload.user.v.req.calibration.
			    accel_scale[1];
			accel_scale[2] =
			    user_rx_v1.payload.user.v.req.calibration.
			    accel_scale[2];
			gyro_scale[0] =
			    user_rx_v1.payload.user.v.req.calibration.
			    gyro_scale[0];
			gyro_scale[1] =
			    user_rx_v1.payload.user.v.req.calibration.
			    gyro_scale[1];
			gyro_scale[2] =
			    user_rx_v1.payload.user.v.req.calibration.
			    gyro_scale[2];
			mag_bias[0] =
			    user_rx_v1.payload.user.v.req.calibration.
			    mag_bias[0];
			mag_bias[1] =
			    user_rx_v1.payload.user.v.req.calibration.
			    mag_bias[1];
			mag_bias[2] =
			    user_rx_v1.payload.user.v.req.calibration.
			    mag_bias[2];
		}
		// echo back the values used
		opahrs_msg_v1_init_user_tx(&user_tx_v1,
					   OPAHRS_MSG_V1_RSP_CALIBRATION);
		user_tx_v1.payload.user.v.rsp.calibration.accel_var[0] =
		    accel_var[0];
		user_tx_v1.payload.user.v.rsp.calibration.accel_var[1] =
		    accel_var[1];
		user_tx_v1.payload.user.v.rsp.calibration.accel_var[2] =
		    accel_var[2];
		user_tx_v1.payload.user.v.rsp.calibration.gyro_bias[0] =
		    gyro_bias[0];
		user_tx_v1.payload.user.v.rsp.calibration.gyro_bias[1] =
		    gyro_bias[1];
		user_tx_v1.payload.user.v.rsp.calibration.gyro_bias[2] =
		    gyro_bias[2];
		user_tx_v1.payload.user.v.rsp.calibration.gyro_var[0] =
		    gyro_var[0];
		user_tx_v1.payload.user.v.rsp.calibration.gyro_var[1] =
		    gyro_var[1];
		user_tx_v1.payload.user.v.rsp.calibration.gyro_var[2] =
		    gyro_var[2];
		user_tx_v1.payload.user.v.rsp.calibration.mag_var[0] =
		    mag_var[0];
		user_tx_v1.payload.user.v.rsp.calibration.mag_var[1] =
		    mag_var[1];
		user_tx_v1.payload.user.v.rsp.calibration.mag_var[2] =
		    mag_var[2];

		lfsm_user_set_tx_v1(&user_tx_v1);
		break;
	case OPAHRS_MSG_V1_REQ_ATTITUDERAW:
		opahrs_msg_v1_init_user_tx(&user_tx_v1,
					   OPAHRS_MSG_V1_RSP_ATTITUDERAW);
		user_tx_v1.payload.user.v.rsp.attituderaw.mags.x =
		    mag_data.raw.axis[0];
		user_tx_v1.payload.user.v.rsp.attituderaw.mags.y =
		    mag_data.raw.axis[1];
		user_tx_v1.payload.user.v.rsp.attituderaw.mags.z =
		    mag_data.raw.axis[2];

		user_tx_v1.payload.user.v.rsp.attituderaw.gyros.x =
		    gyro_data.raw.x;
		user_tx_v1.payload.user.v.rsp.attituderaw.gyros.y =
		    gyro_data.raw.y;
		user_tx_v1.payload.user.v.rsp.attituderaw.gyros.z =
		    gyro_data.raw.z;

		user_tx_v1.payload.user.v.rsp.attituderaw.gyros_filtered.
		    x = gyro_data.filtered.x;
		user_tx_v1.payload.user.v.rsp.attituderaw.gyros_filtered.
		    y = gyro_data.filtered.y;
		user_tx_v1.payload.user.v.rsp.attituderaw.gyros_filtered.
		    z = gyro_data.filtered.z;

		user_tx_v1.payload.user.v.rsp.attituderaw.gyros.xy_temp =
		    gyro_data.temp.xy;
		user_tx_v1.payload.user.v.rsp.attituderaw.gyros.z_temp =
		    gyro_data.temp.z;

		user_tx_v1.payload.user.v.rsp.attituderaw.accels.x =
		    accel_data.raw.x;
		user_tx_v1.payload.user.v.rsp.attituderaw.accels.y =
		    accel_data.raw.y;
		user_tx_v1.payload.user.v.rsp.attituderaw.accels.z =
		    accel_data.raw.z;

		user_tx_v1.payload.user.v.rsp.attituderaw.accels_filtered.
		    x = accel_data.filtered.x;
		user_tx_v1.payload.user.v.rsp.attituderaw.accels_filtered.
		    y = accel_data.filtered.y;
		user_tx_v1.payload.user.v.rsp.attituderaw.accels_filtered.
		    z = accel_data.filtered.z;

		lfsm_user_set_tx_v1(&user_tx_v1);
		break;
	case OPAHRS_MSG_V1_REQ_UPDATE:
		// process incoming data
		opahrs_msg_v1_init_user_tx(&user_tx_v1,
					   OPAHRS_MSG_V1_RSP_UPDATE);
		if (user_rx_v1.payload.user.v.req.update.barometer.updated) {
			altitude_data.altitude =
			    user_rx_v1.payload.user.v.req.update.barometer.
			    altitude;
			altitude_data.updated =
			    user_rx_v1.payload.user.v.req.update.barometer.
			    updated;
		}
		if (user_rx_v1.payload.user.v.req.update.gps.updated) {
			gps_data.updated = true;
			gps_data.NED[0] =
			    user_rx_v1.payload.user.v.req.update.gps.
			    NED[0];
			gps_data.NED[1] =
			    user_rx_v1.payload.user.v.req.update.gps.
			    NED[1];
			gps_data.NED[2] =
			    user_rx_v1.payload.user.v.req.update.gps.
			    NED[2];
			gps_data.heading =
			    user_rx_v1.payload.user.v.req.update.gps.
			    heading;
			gps_data.groundspeed =
			    user_rx_v1.payload.user.v.req.update.gps.
			    groundspeed;
			gps_data.quality =
			    user_rx_v1.payload.user.v.req.update.gps.
			    quality;
		}
		// send out attitude/position estimate
		user_tx_v1.payload.user.v.rsp.update.quaternion.q1 =
		    attitude_data.quaternion.q1;
		user_tx_v1.payload.user.v.rsp.update.quaternion.q2 =
		    attitude_data.quaternion.q2;
		user_tx_v1.payload.user.v.rsp.update.quaternion.q3 =
		    attitude_data.quaternion.q3;
		user_tx_v1.payload.user.v.rsp.update.quaternion.q4 =
		    attitude_data.quaternion.q4;

		// TODO: separate this from INSGPS
		user_tx_v1.payload.user.v.rsp.update.NED[0] = Nav.Pos[0];
		user_tx_v1.payload.user.v.rsp.update.NED[1] = Nav.Pos[1];
		user_tx_v1.payload.user.v.rsp.update.NED[2] = Nav.Pos[2];
		user_tx_v1.payload.user.v.rsp.update.Vel[0] = Nav.Vel[0];
		user_tx_v1.payload.user.v.rsp.update.Vel[1] = Nav.Vel[1];
		user_tx_v1.payload.user.v.rsp.update.Vel[2] = Nav.Vel[2];

		// compute the idle fraction
		user_tx_v1.payload.user.v.rsp.update.load =
		    ((float)running_counts /
		     (float)(idle_counts + running_counts)) * 100;
		user_tx_v1.payload.user.v.rsp.update.idle_time =
		    idle_counts / (TIMER_RATE / 10000);
		user_tx_v1.payload.user.v.rsp.update.run_time =
		    running_counts / (TIMER_RATE / 10000);
		user_tx_v1.payload.user.v.rsp.update.dropped_updates =
		    ekf_too_slow;
		lfsm_user_set_tx_v1(&user_tx_v1);
		break;
	default:
		break;
	}

	/* Finished processing the received message, requeue it */
	lfsm_user_set_rx_v1(&user_rx_v1);
	lfsm_user_done();
}

/**
 * @}
 */