Adds ahrs.h ahrs.c and ahrs.S in sw/airborne/autopilot

Makefile will build it only for AIRCRAFT=Gorazoptere_brushless_ANALOG
Adds IMU setting in the gorazoptere_brushless_ANALOG.xml
(Poine : it build but a lot of work is needed in ahrs.c)

conf/airframes/gorazoptere_brushless_ANALOG.xml

@@ -4,6 +4,18 @@
     <define name="ADC_ROLL_DOT" value="1"/>
     <define name="ADC_PITCH_DOT" value="2"/>
     <define name="ADC_YAW_DOT" value="3"/>
+	<define name="ADC_ROLL_DOT_SIGN" value="+"/>
+    <define name="ADC_PITCH_DOT_SIGN" value="-"/>
+    <define name="ADC_YAW_DOT_SIGN" value="-"/>
+	<define name="ADC_ACCELX" value="2"/>
+    <define name="ADC_ACCELY" value="3"/>
+    <define name="ADC_ACCELZ" value="4"/>
+	<define name="ADC_ACCELX_SIGN" value="-"/>
+    <define name="ADC_ACCELY_SIGN" value="+"/>
+    <define name="ADC_ACCELZ_SIGN" value="-"/>
+	<define name="ADC_ACCELX_ZERO" value="0x24A"/>
+    <define name="ADC_ACCELY_ZERO" value="0x280"/>
+    <define name="ADC_ACCELZ_ZERO" value="0x210"/>
   </section>
   <section name="RATE_LOOP">
     <define name="ROLL_DOT_PGAIN" value="-192."/>

sw/airborne/autopilot/Makefile

@@ -62,6 +62,11 @@ $(TARGET).srcs	= \
 	mainloop.c \
 	cam.c
 
+ifeq ($(AIRCRAFT), Gorazoptere_brushless_ANALOG)
+	$(TARGET).srcs  += ahrs.S 
+	$(TARGET).srcs  += ahrs.c 
+endif
+
 include ../../../conf/Makefile.local
 include ../../../conf/Makefile.avr
 

sw/airborne/autopilot/ahrs.S

@@ -0,0 +1,65 @@
+/* -*- indent-tabs-mode:T; c-basic-offset:8; tab-width:8; -*- vi: set ts=8:
+ * $Id$
+ *
+ * Assembly file to layout the AHRS data.
+ * We want to be sure that things are aligned and packed as
+ * closely as possible, as well as alias several things.  This
+ * is the easiest way to do it.
+ */
+.section .bss
+
+.global X
+X:
+.global quat
+quat:
+.global q0
+q0:
+.space 4
+.global q1
+q1:
+.space 4
+.global q2
+q2:
+.space 4
+.global q3
+q3:
+.space 4
+
+.global	bias
+bias:
+.global	bias_p
+bias_p:
+.space	4
+.global	bias_q
+bias_q:
+.space	4
+.global	bias_r
+bias_r:
+.space	4
+
+
+.global	C
+C:
+.global	Qdot
+Qdot:
+.space	16
+
+
+.global A
+A:
+.global	PCt
+PCt:
+.space	7 * 4
+.global	K
+K:
+.space	7 * 4
+.global E
+E:
+.space	1 * 4
+
+/* And the rest of A */
+.space (4*7 - 7 - 7 - 1) * 4
+
+
+.global end_bss
+end_bss:

sw/airborne/autopilot/ahrs.h

@@ -0,0 +1,52 @@
+/* -*- indent-tabs-mode:T; c-basic-offset:8; tab-width:8; -*- vi: set ts=8:
+ * $Id$
+ *
+ * Fast AHRS object almost ready for use on microcontrollers
+ *
+ * (c) 2003 Trammell Hudson <hudson@rotomotion.com>
+ */
+#ifndef _fast_ahrs_h_
+#define _fast_ahrs_h_
+
+#include <inttypes.h>
+
+
+typedef float		real_t;
+typedef uint8_t		index_t;
+
+
+/*
+ * We have seven variables in our state -- the quaternion attitude
+ * estimate and three gyro bias values.  The state time update equation
+ * comes from the IMU gyro sensor readings:
+ *
+ *	Q_dot		= Wxq(pqr) * Q
+ *	Bias_dot	= 0
+ */
+extern real_t	X[7];
+extern real_t	quat[4];
+extern real_t	q0;
+extern real_t	q1;
+extern real_t	q2;
+extern real_t	q3;
+extern real_t	bias[3];
+extern real_t	bias_p;
+extern real_t	bias_q;
+extern real_t	bias_r;
+
+extern real_t	pqr[3];
+extern int16_t	accel[3];
+
+
+/*
+ * The euler estimate will be updated less frequently than the
+ * quaternion, but some applications are ok with that.
+ */
+extern real_t	euler[3];
+
+
+//exported functions
+extern void ahrs_init( void );
+extern void ahrs_update( void );
+
+#endif

sw/airborne/fly_by_wire/main.c

@@ -22,7 +22,7 @@
  *
  */
 
-//#define LED_DEBUG
+#define LED_DEBUG
 
 #include <inttypes.h>
 #include <avr/io.h>