diff --git a/flight/AHRS/ahrs.c b/flight/AHRS/ahrs.c
index c697b08f2..36fb7ad1c 100644
--- a/flight/AHRS/ahrs.c
+++ b/flight/AHRS/ahrs.c
@@ -26,79 +26,255 @@
 
 /* OpenPilot Includes */
 #include "ahrs.h"
+#include "pios_opahrs_proto.h"
+#include "ahrs_fsm.h"		/* lfsm_state */
 
 /* Global Variables */
 
 /* Local Variables */
+struct mag_sensor {
+  uint8_t id[4];
+  struct {
+    int16_t axis[3];
+    float   heading;
+  } raw;
+};
+
+struct accel_sensor {
+  struct {
+    uint16_t x;
+    uint16_t y;
+    uint16_t z;
+  } raw;
+};
+
+struct attitude_solution {
+  struct {
+    float q1;
+    float q2;
+    float q3;
+    float q4;
+  } quaternion;
+
+  struct {
+    float roll;
+    float pitch;
+    float yaw;
+  } euler;
+};
+
+static struct mag_sensor        mag_data;
+static struct accel_sensor      accel_data;
+static struct attitude_solution attitude_data = {
+  .quaternion = {
+    .q1 = 1.011,
+    .q2 = 2.022,
+    .q3 = 3.033,
+    .q4 = 0,
+  },
+  .euler = {
+    .roll  = 4.044,
+    .pitch = 5.055,
+    .yaw   = 6.066,
+  },
+};
 
 /* Function Prototypes */
-
+void process_spi_request(void);
 
 /**
 * AHRS Main function
 */
 int main()
 {
-	/* Brings up System using CMSIS functions, enables the LEDs. */
-	PIOS_SYS_Init();
+  /* Brings up System using CMSIS functions, enables the LEDs. */
+  PIOS_SYS_Init();
+  
+  /* Delay system */
+  PIOS_DELAY_Init();
+  
+  /* Communication system */
+  PIOS_COM_Init();
+  
+  /* ADC system */
+  PIOS_ADC_Init();
+  
+  /* Magnetic sensor system */
+  PIOS_I2C_Init();
+  PIOS_HMC5843_Init();
+  
+  /* Setup the Accelerometer FS (Full-Scale) GPIO */
+  PIOS_GPIO_Enable(0);
+  SET_ACCEL_2G;
+  
+  /* Configure the HMC5843 Sensor */
+  PIOS_HMC5843_ConfigTypeDef HMC5843_InitStructure;
+  HMC5843_InitStructure.M_ODR = PIOS_HMC5843_ODR_10;
+  HMC5843_InitStructure.Meas_Conf = PIOS_HMC5843_MEASCONF_NORMAL;
+  HMC5843_InitStructure.Gain = PIOS_HMC5843_GAIN_2;
+  HMC5843_InitStructure.Mode = PIOS_HMC5843_MODE_CONTINUOUS;
+  PIOS_HMC5843_Config(&HMC5843_InitStructure);
+  
+  /* SPI link to master */
+  PIOS_SPI_Init();
+  
+  lfsm_init();
+  
+  // Main loop
+  while (1) {
+    uint8_t loop_ctr;
 
-	/* Delay system */
-	PIOS_DELAY_Init();
+    // Alive signal
+    if (loop_ctr++ > 100) {
+      PIOS_LED_Toggle(LED1);
+      loop_ctr = 0;
+    }
+    
+    // Get 3 ID bytes
+    strcpy ((char *)mag_data.id, "ZZZ");
+    PIOS_HMC5843_ReadID(mag_data.id);
+    
+    // Get magnetic readings
+    PIOS_HMC5843_ReadMag(mag_data.raw.axis);
+    
+    // Calculate the heading
+    mag_data.raw.heading = atan2((double)(mag_data.raw.axis[0]), (double)(-1 * mag_data.raw.axis[1])) * (180 / M_PI);
+    if(mag_data.raw.heading < 0) mag_data.raw.heading += 360;
+    
+    // Test ADC
+    accel_data.raw.x = PIOS_ADC_PinGet(0);
+    accel_data.raw.y = PIOS_ADC_PinGet(1);
+    accel_data.raw.z = PIOS_ADC_PinGet(2);
+    //PIOS_COM_SendFormattedString(PIOS_COM_AUX, "ADC Values: %d,%d,%d,%d,%d,%d\r\n", PIOS_ADC_PinGet(0), PIOS_ADC_PinGet(1), PIOS_ADC_PinGet(2), PIOS_ADC_PinGet(3), PIOS_ADC_PinGet(4), PIOS_ADC_PinGet(5));
 
-	/* Communication system */
-	PIOS_COM_Init();
 
-	/* ADC system */
-	PIOS_ADC_Init();
+    /* Simulate a rotating airframe */
+    attitude_data.quaternion.q1 += .001;
+    attitude_data.quaternion.q2 += .002;
+    attitude_data.quaternion.q3 += .003;
+    attitude_data.quaternion.q4 += 1;
 
-	/* Magnetic sensor system */
-	PIOS_I2C_Init();
-	PIOS_HMC5843_Init();
-
-	/* Setup the Accelerometer FS (Full-Scale) GPIO */
-	PIOS_GPIO_Enable(0);
-	SET_ACCEL_2G;
-
-	/* Configure the HMC5843 Sensor */
-	PIOS_HMC5843_ConfigTypeDef HMC5843_InitStructure;
-	HMC5843_InitStructure.M_ODR = PIOS_HMC5843_ODR_10;
-	HMC5843_InitStructure.Meas_Conf = PIOS_HMC5843_MEASCONF_NORMAL;
-	HMC5843_InitStructure.Gain = PIOS_HMC5843_GAIN_2;
-	HMC5843_InitStructure.Mode = PIOS_HMC5843_MODE_CONTINUOUS;
-	PIOS_HMC5843_Config(&HMC5843_InitStructure);
-
-	uint8_t id[4] = {0};
-	int16_t data[3] = {0};
-	int32_t heading = 0;
-
-	/* SPI link to master */
-	PIOS_SPI_Init();
-
-	// Main loop
-	for(;;) {
-		// Alive signal
-		PIOS_LED_Toggle(LED1);
-
-		// Get 3 ID bytes
-		PIOS_HMC5843_ReadID(id);
-
-		// Get magnetic readings
-		PIOS_HMC5843_ReadMag(data);
-
-		// Calculate the heading
-		heading = atan2((double)(data[0]), (double)(-1 * data[1])) * (180 / M_PI);
-		if(heading < 0) heading += 360;
-
-		// Output Heading data to
-		PIOS_COM_SendFormattedString(PIOS_COM_AUX, "HMC5843 Chip ID: %s  Heading: %d   Raw Mag Values: X=%d Y=%d Z=%d  ", id, heading, data[0], data[1], data[2]);
-
-		// Test ADC
-		PIOS_COM_SendFormattedString(PIOS_COM_AUX, "ADC Values: %d,%d,%d,%d,%d,%d\r\n", PIOS_ADC_PinGet(0), PIOS_ADC_PinGet(1), PIOS_ADC_PinGet(2), PIOS_ADC_PinGet(3), PIOS_ADC_PinGet(4), PIOS_ADC_PinGet(5));
-
-		// Delay until next reading
-		PIOS_DELAY_WaitmS(500);
-	}
-
-	return 0;
+    attitude_data.euler.roll  += .004;
+    if (attitude_data.euler.roll > 360.0) attitude_data.euler.roll -= 360.0;
+    attitude_data.euler.pitch += .005;
+    if (attitude_data.euler.pitch > 360.0) attitude_data.euler.pitch -= 360.0;
+    attitude_data.euler.yaw   += .006;
+    if (attitude_data.euler.yaw > 360.0) attitude_data.euler.yaw -= 360.0;
+    
+    process_spi_request();
+    
+    // Delay until next reading
+    //PIOS_DELAY_WaitmS(50);
+  }
+  
+  return 0;
+}
+
+void dump_spi_message(uint8_t port, const char * prefix, uint8_t * data, uint32_t len)
+{
+
+
+
+
+}
+
+
+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 */
+    //PIOS_COM_SendFormattedString(PIOS_COM_AUX, ".");
+    return;
+  }
+
+  if (user_rx_v1.tail.magic != OPAHRS_MSG_MAGIC_TAIL) {
+    PIOS_COM_SendFormattedString(PIOS_COM_AUX, "x");
+  }
+
+  /* We've got a message to process */
+  //dump_spi_message(PIOS_COM_AUX, "+", (uint8_t *)&user_rx_v1, sizeof(user_rx_v1));
+
+  switch (user_rx_v1.payload.user.t) {
+  case OPAHRS_MSG_V1_REQ_SYNC:
+    opahrs_msg_v1_init_user_tx (&user_tx_v1, OPAHRS_MSG_V1_RSP_SYNC);
+    user_tx_v1.payload.user.v.rsp.sync.i_am_a_bootloader = FALSE;
+    user_tx_v1.payload.user.v.rsp.sync.hw_version = 1;
+    user_tx_v1.payload.user.v.rsp.sync.bl_version = 2;
+    user_tx_v1.payload.user.v.rsp.sync.fw_version = 3;
+    user_tx_v1.payload.user.v.rsp.sync.cookie     = user_rx_v1.payload.user.v.req.sync.cookie;
+    dump_spi_message(PIOS_COM_AUX, "S", (uint8_t *)&user_tx_v1, sizeof(user_tx_v1));
+    lfsm_user_set_tx_v1 (&user_tx_v1);
+    break;
+  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));
+    dump_spi_message(PIOS_COM_AUX, "I", (uint8_t *)&user_tx_v1, sizeof(user_tx_v1));
+    lfsm_user_set_tx_v1 (&user_tx_v1);
+    break;
+  case OPAHRS_MSG_V1_REQ_HEADING:
+    opahrs_msg_v1_init_user_tx (&user_tx_v1, OPAHRS_MSG_V1_RSP_HEADING);
+    user_tx_v1.payload.user.v.rsp.heading.raw_mag.x     = mag_data.raw.axis[0];
+    user_tx_v1.payload.user.v.rsp.heading.raw_mag.y     = mag_data.raw.axis[1];
+    user_tx_v1.payload.user.v.rsp.heading.raw_mag.z     = mag_data.raw.axis[2];
+    user_tx_v1.payload.user.v.rsp.heading.heading       = mag_data.raw.heading;
+    dump_spi_message(PIOS_COM_AUX, "H", (uint8_t *)&user_tx_v1, sizeof(user_tx_v1));
+    lfsm_user_set_tx_v1 (&user_tx_v1);
+    break;
+  case OPAHRS_MSG_V1_REQ_ATTITUDE:
+    opahrs_msg_v1_init_user_tx (&user_tx_v1, OPAHRS_MSG_V1_RSP_ATTITUDE);
+    user_tx_v1.payload.user.v.rsp.attitude.quaternion.q1 = attitude_data.quaternion.q1;
+    user_tx_v1.payload.user.v.rsp.attitude.quaternion.q2 = attitude_data.quaternion.q2;
+    user_tx_v1.payload.user.v.rsp.attitude.quaternion.q3 = attitude_data.quaternion.q3;
+    user_tx_v1.payload.user.v.rsp.attitude.quaternion.q4 = attitude_data.quaternion.q4;
+    user_tx_v1.payload.user.v.rsp.attitude.euler.roll    = attitude_data.euler.roll;
+    user_tx_v1.payload.user.v.rsp.attitude.euler.pitch   = attitude_data.euler.pitch;
+    user_tx_v1.payload.user.v.rsp.attitude.euler.yaw     = attitude_data.euler.yaw;
+    dump_spi_message(PIOS_COM_AUX, "A", (uint8_t *)&user_tx_v1, sizeof(user_tx_v1));
+#if 1
+    /* DEBUG: Overload q4 as a cycle counter since last read. */
+    attitude_data.quaternion.q4 = 0;
+#endif
+    lfsm_user_set_tx_v1 (&user_tx_v1);
+    break;
+  default:
+    break;
+  }
+
+  /* Finished processing the received message, requeue it */
+  memset(&user_rx_v1, 0xAA, sizeof(user_rx_v1));
+  lfsm_user_set_rx_v1 (&user_rx_v1);
+  lfsm_user_done ();
 }