diff --git a/conf/airframes/example_twog_analogimu.xml b/conf/airframes/example_twog_analogimu.xml
new file mode 100644
index 0000000000..280f556c55
--- /dev/null
+++ b/conf/airframes/example_twog_analogimu.xml
@@ -0,0 +1,192 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ ap.CFLAGS += -DANALOGIMU_ROTATED
+ ap.CFLAGS += -DANALOGIMU_ZERO_AVERAGE
+
+ ap.CFLAGS += -DGPS_USE_LATLONG
+
+ //ap.CFLAGS += -DWIND_INFO -DSTRONG_WIND
+
+ //sim.CFLAGS += -DWIND_INFO -DSTRONG_WIND
+
+
+
+
+
+
+
diff --git a/conf/autopilot/subsystems/fixedwing/attitude_analogimu.makefile b/conf/autopilot/subsystems/fixedwing/attitude_analogimu.makefile
new file mode 100644
index 0000000000..79068ac459
--- /dev/null
+++ b/conf/autopilot/subsystems/fixedwing/attitude_analogimu.makefile
@@ -0,0 +1,29 @@
+# attitude via analog imu
+
+
+ifeq ($(ARCH), lpc21)
+ap.CFLAGS += -DANALOG_IMU -DADC -DUSE_ADC_0 -DUSE_ADC_1 -DUSE_ADC_2 -DUSE_ADC_3 -DUSE_ADC_4 -DUSE_ADC_5 -DUSE_ADC_6 -DUSE_ADC_7
+
+ap.srcs += $(SRC_FIXEDWING)/subsystems/analogimu/dcm.c $(SRC_FIXEDWING)/subsystems/analogimu/arduimu.c $(SRC_FIXEDWING)/subsystems/analogimu/matrix.c $(SRC_FIXEDWING)/subsystems/analogimu/vector.c
+
+ap.srcs += $(SRC_FIXEDWING)/subsystems/analogimu/analogimu.c $(SRC_FIXEDWING)/subsystems/analogimu/analogimu_util.c
+endif
+
+# since there is currently no SITL sim for the Analog IMU, we use the infrared sim
+
+ifeq ($(TARGET), sim)
+
+sim.CFLAGS += -DIR_ROLL_NEUTRAL_DEFAULT=0
+
+sim.CFLAGS += -DIR_PITCH_NEUTRAL_DEFAULT=0
+
+$(TARGET).CFLAGS += -DUSE_INFRARED
+$(TARGET).srcs += subsystems/sensors/infrared.c
+
+sim.srcs += $(SRC_ARCH)/sim_ir.c
+
+sim.srcs += $(SRC_ARCH)/sim_analogimu.c
+
+endif
+
+jsbsim.srcs += $(SRC_ARCH)/jsbsim_ir.c
diff --git a/conf/settings/tuning_imu.xml b/conf/settings/tuning_imu.xml
new file mode 100644
index 0000000000..50af81acd3
--- /dev/null
+++ b/conf/settings/tuning_imu.xml
@@ -0,0 +1,117 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/sw/airborne/arch/sim/sim_analogimu.c b/sw/airborne/arch/sim/sim_analogimu.c
new file mode 100644
index 0000000000..0551d34890
--- /dev/null
+++ b/sw/airborne/arch/sim/sim_analogimu.c
@@ -0,0 +1,15 @@
+/** \file sim_ir.c
+ * \brief Regroup functions to simulate autopilot/infrared.c
+ *
+ * Infrared soft simulation. OCaml binding.
+ */
+
+
+#include
+#include "subsystems/sensors/infrared.h"
+#include "generated/airframe.h"
+
+#include
+
+float imu_roll_neutral = RadOfDeg(IMU_ROLL_NEUTRAL_DEFAULT);
+float imu_pitch_neutral = RadOfDeg(IMU_PITCH_NEUTRAL_DEFAULT);
diff --git a/sw/airborne/firmwares/fixedwing/main_ap.c b/sw/airborne/firmwares/fixedwing/main_ap.c
index 270aa16383..e02e5a76c4 100644
--- a/sw/airborne/firmwares/fixedwing/main_ap.c
+++ b/sw/airborne/firmwares/fixedwing/main_ap.c
@@ -65,6 +65,10 @@
#endif
+#ifdef ANALOG_IMU
+#include "subsystems/analogimu/analogimu.h"
+#endif
+
#if ! defined CATASTROPHIC_BAT_LEVEL && defined LOW_BATTERY
#warning "LOW_BATTERY deprecated. Renamed into CATASTROPHIC_BAT_LEVEL (in airframe file)"
#define CATASTROPHIC_BAT_LEVEL LOW_BATTERY
@@ -431,6 +435,13 @@ void periodic_task_ap( void ) {
#error "Only 20 and 60 allowed for CONTROL_RATE"
#endif
+#ifdef ANALOG_IMU
+ if (!_20Hz) {
+ estimator_update_state_analog_imu();
+ analog_imu_downlink();
+ }
+#endif // ANALOG_IMU
+
#if CONTROL_RATE == 20
if (!_20Hz)
#endif
@@ -485,6 +496,10 @@ void init_ap( void ) {
#endif
+#ifdef ANALOG_IMU
+ analog_imu_init();
+#endif
+
/************* Links initialization ***************/
#if defined MCU_SPI_LINK
link_mcu_init();
@@ -534,6 +549,11 @@ void init_ap( void ) {
traffic_info_init();
#endif
+#ifdef ANALOG_IMU
+ //wait 10secs for init
+ sys_time_usleep(10000000);
+ analog_imu_offset_set();
+#endif
}
diff --git a/sw/airborne/subsystems/analogimu/analogimu.c b/sw/airborne/subsystems/analogimu/analogimu.c
new file mode 100644
index 0000000000..25d803d610
--- /dev/null
+++ b/sw/airborne/subsystems/analogimu/analogimu.c
@@ -0,0 +1,257 @@
+/*
+ * $Id: analogimu.c $
+ *
+ * Copyright (C) 2010 Oliver Riesener, Christoph Niemann
+ *
+ * This file is part of paparazzi.
+ *
+ * paparazzi 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 2, or (at your option)
+ * any later version.
+ *
+ * paparazzi 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 paparazzi; see the file COPYING. If not, write to
+ * the Free Software Foundation, 59 Temple Place - Suite 330,
+ * Boston, MA 02111-1307, USA.
+ *
+ */
+
+/** \file analogimu.c
+ * \brief Analog IMU Routines
+ *
+ */
+#if ! (defined SITL || defined HITL)
+
+#include "led.h"
+#include "mcu_periph/adc.h"
+#include "mcu_periph/uart.h"
+//#include "downlink.h"
+#include "estimator.h"
+//#include "ap_downlink.h"
+#include "sys_time.h"
+
+#include "dcm.h"
+
+#include "analogimu_util.h"
+
+#include "analogimu.h"
+
+#endif
+
+#define NB_ADC 8
+#define ADC_NB_SAMPLES 16
+
+// variables
+
+uint16_t analog_imu_offset[NB_ADC] = {0,};
+
+static struct adc_buf buf_adc[NB_ADC];
+int adc_average[16] = { 0 };
+
+float imu_roll_neutral = RadOfDeg(IMU_ROLL_NEUTRAL_DEFAULT);
+float imu_pitch_neutral = RadOfDeg(IMU_PITCH_NEUTRAL_DEFAULT);
+
+#if ! (defined SITL || defined HITL)
+
+// functions
+/**
+ * accel2ms2():
+ *
+ * \return accel[ACC_X], accel[ACC_Y], accel[ACC_Z]
+ */
+void accel2ms2( void ) {
+ accel[ACC_X] = (float)(adc_average[ADC_ACCX])/10.19;
+ accel[ACC_Y] = (float)(-adc_average[ADC_ACCY])/10.5;
+ accel[ACC_Z] = (float)(adc_average[ADC_ACCZ])/10.4;//chni: needs to be adjusted for earth gravity
+}
+/**
+ * gyro2rads():
+ *
+ * \return gyro[G_ROLL], gyro[G_PITCH], gyro[G_YAW]
+ */
+void gyro2rads( void ) {
+ /** 150 grad/sec 10Bit, 3,3Volt, 1rad = 2Pi/1024 => Pi/512 */
+ gyro[G_ROLL] = (float)(adc_average[ADC_ROLL]) / 61.3588;
+ gyro[G_PITCH] = (float)(adc_average[ADC_PITCH]) / 57.96;
+ gyro[G_YAW] = (float)(-adc_average[ADC_YAW]) / 60.1;
+}
+
+void analog_imu_init( void ) {
+ adc_buf_channel(ADC_CHANNEL_IMU_GROLL, &buf_adc[0], ADC_NB_SAMPLES);
+ adc_buf_channel(ADC_CHANNEL_IMU_GPITCH, &buf_adc[1], ADC_NB_SAMPLES);
+ adc_buf_channel(ADC_CHANNEL_IMU_GYAW, &buf_adc[2], ADC_NB_SAMPLES);
+ adc_buf_channel(ADC_CHANNEL_IMU_ACCX, &buf_adc[5], ADC_NB_SAMPLES);
+ adc_buf_channel(ADC_CHANNEL_IMU_ACCY, &buf_adc[6], ADC_NB_SAMPLES);
+ adc_buf_channel(ADC_CHANNEL_IMU_ACCZ, &buf_adc[7], ADC_NB_SAMPLES);
+
+#if NB_ADC != 8
+#error "8 ADCs expected !"
+#endif
+
+}
+
+void analog_imu_offset_set( void ) {
+ uint8_t i;
+ for(i = 0; i < NB_ADC - 1; i++) {
+ analog_raw[i] = buf_adc[i].sum / ADC_NB_SAMPLES;
+ analog_imu_offset[i] = analog_raw[i];
+ }
+ analog_imu_offset[7] = analog_raw[7] + 528;// 553; // + Zero of z-acc (without gravity) needs to be adjusted
+}
+/**
+ * analog_imu_update():
+ */
+void analog_imu_update( void ) {
+ uint8_t i;
+ for(i = 0; i < NB_ADC; i++) {
+ analog_raw[i] = buf_adc[i].sum / ADC_NB_SAMPLES;
+ }
+ adc_average[ADC_ROLL] = analog_raw[0] - analog_imu_offset[0];
+ adc_average[ADC_PITCH] = analog_raw[1] - analog_imu_offset[1];
+ adc_average[ADC_YAW] = analog_raw[2] - analog_imu_offset[2];
+ adc_average[ADC_ACCX] = analog_raw[5] - analog_imu_offset[5];
+ adc_average[ADC_ACCY] = analog_raw[6] - analog_imu_offset[6];
+ adc_average[ADC_ACCZ] = analog_raw[7] - analog_imu_offset[7];
+ accel2ms2();
+ gyro2rads();
+}
+
+/** earth accelecation */
+volatile float g = 0.;
+
+// functions
+
+void analog_imu_downlink( void ) {
+ //uint8_t id = 0;
+ //float time = GET_CUR_TIME_FLOAT();
+ //time *= 1000;//secs to msecs
+ //int mx = 0;
+ //int my = 0;
+ //int mz = 0;
+ //DOWNLINK_SEND_HB_FILTER( DefaultChannel,&time, &accel[ACC_X],&accel[ACC_Y],&accel[ACC_Z],&gyro[G_ROLL],&gyro[G_PITCH],&gyro[G_YAW],&heading,&mx,&my,&mz,&euler[EULER_ROLL],&euler[EULER_PITCH],&euler[EULER_YAW], &imu_roll_neutral, &imu_pitch_neutral );
+}
+
+
+/**
+ * matrix transpose is safe to call with c == a if m = n
+ *
+ * \param [out] *c pointer to destination array
+ * \param [in] *a pointer to source array
+ * \param [in] m dimention rows
+ * \param [in] n dimention cols
+ * \return result is in *c, or error if a==c and m != n
+ */
+void matrix_transpose(float *c, float *a, short m, short n)
+{
+ if ( a != c ) { // quick version
+ short i,j;
+ // [i][j] ... [m][n]
+ for ( i=0 ; i -0.01 )
+ {
+ g=0.01;
+ }else{
+ //nothing
+ }
+ //values in radians
+#define NEW
+#ifdef OLD
+ angle[ANG_PITCH] = -asinf( accel[ACC_X] / g );
+ angle[ANG_ROLL] = asinf( accel[ACC_Y] / g );
+ angle[ANG_YAW] = 0.0;
+#endif
+
+#ifdef NEW
+
+ float a1 = accel[ACC_X] / -g;
+
+ if(a1 > 1.0 && a1 >= 0.0){
+ a1 = 1.0;
+ } else if(a1 < -1.0 && a1 < 0.0){
+ a1 = -1.0;
+ }
+
+ angle[ANG_PITCH] = asinf( a1 );
+
+ if(accel[ACC_Z] < 0 && angle[ANG_PITCH] > 0) angle[ANG_PITCH] = + 3.145/2 + (3.145/2 - angle[ANG_PITCH]);
+ else if (accel[ACC_Z] < 0 && angle[ANG_PITCH] < 0) angle[ANG_PITCH] = -3.145/2 - (3.145/2 + angle[ANG_PITCH]);
+
+
+ if( accel[ACC_Z] < 0.01 && accel[ACC_Z] > -0.01 )
+ {
+ accel[ACC_Z]=0.01;
+ }else{
+ //nothing
+ }
+
+
+ angle[ANG_ROLL] = atan2f( accel[ACC_Y], accel[ACC_Z] );
+ //angle[ANG_PITCH] = -atan2f( accel[ACC_X] , accel[ACC_Z] );
+ angle[ANG_YAW] = 0.0;
+#endif
+}
+
+
+void estimator_update_state_analog_imu( void ) {
+#undef ANGLE_FROM_ACCEL
+#ifdef ANGLE_FROM_ACCEL
+ estimator_phi = (float)(atan2f((float)((analog_raw[6]-510)),(float)(-(analog_raw[7]-510))));
+ estimator_theta = (float)(atan2f((float)(-(analog_raw[5]-510)),(float)(-(analog_raw[7]-510))));
+#else
+
+ analog_imu_update();
+
+ Matrix_update();
+ Normalize();
+ Drift_correction();
+ Euler_angles();
+
+ // return euler angles to phi and theta
+ estimator_phi = euler[EULER_ROLL]-imu_roll_neutral;
+ //estimator_phi = angle[ANG_ROLL];
+ estimator_theta = euler[EULER_PITCH]-imu_pitch_neutral;
+ //estimator_theta = angle[ANG_PITCH];
+ estimator_psi = euler[EULER_YAW];
+
+#endif
+}
+#endif
diff --git a/sw/airborne/subsystems/analogimu/analogimu.h b/sw/airborne/subsystems/analogimu/analogimu.h
new file mode 100644
index 0000000000..51d32224b1
--- /dev/null
+++ b/sw/airborne/subsystems/analogimu/analogimu.h
@@ -0,0 +1,67 @@
+/*
+ * $Id: analogimu.h $
+ *
+ * Copyright (C) 2010 Oliver Riesener, Christoph Niemann
+ *
+ * This file is part of paparazzi.
+ *
+ * paparazzi 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 2, or (at your option)
+ * any later version.
+ *
+ * paparazzi 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 paparazzi; see the file COPYING. If not, write to
+ * the Free Software Foundation, 59 Temple Place - Suite 330,
+ * Boston, MA 02111-1307, USA.
+ *
+ */
+
+/** \file analogimu.h
+ * \brief Analog IMU Interface
+ *
+ */
+
+#ifndef _ANALOGIMU_H_
+#define _ANALOGIMU_H_
+// types
+#define uint16_t short int
+
+#include "std.h"
+#include "generated/airframe.h"
+
+extern float imu_roll_neutral;
+extern float imu_pitch_neutral;
+
+
+// ADC0 Slots
+#define ADC_YAW 1
+#define ADC_PITCH 2
+#define ADC_ROLL 3
+#define ADC_OFF 4
+
+// ADC1 Slots
+#define ADC_ACCX (3+8)
+#define ADC_ACCY (4+8)
+#define ADC_ACCZ (5+8)
+#define ADC_VBAT (6+8)
+#define ADC_ALT (7+8)
+
+//functions
+void analog_imu_init( void );
+void analog_imu_update( void );
+void analog_imu_downlink( void );
+void analogconversion( void );
+void matrix_transpose(float *c, float *a, short m, short n);
+void accel2euler( void );
+void accel2ms2( void );
+void estimator_update_state_analog_imu( void );
+void gyro2rads( void );
+void analog_imu_offset_set( void );
+
+#endif // _ANALOGIMU_H_
diff --git a/sw/airborne/subsystems/analogimu/analogimu_util.c b/sw/airborne/subsystems/analogimu/analogimu_util.c
new file mode 100644
index 0000000000..e6b4b5e1fb
--- /dev/null
+++ b/sw/airborne/subsystems/analogimu/analogimu_util.c
@@ -0,0 +1,100 @@
+/*
+ * $Id: analogimu_util.c $
+ *
+ * Copyright (C) 2010 Christoph Niemann
+ *
+ * This file is part of paparazzi.
+ *
+ * paparazzi 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 2, or (at your option)
+ * any later version.
+ *
+ * paparazzi 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 paparazzi; see the file COPYING. If not, write to
+ * the Free Software Foundation, 59 Temple Place - Suite 330,
+ * Boston, MA 02111-1307, USA.
+ *
+ */
+
+/** \file analogimu_util.c
+ * \brief Analog IMU Utilities
+ *
+ */
+
+#include "mcu_periph/adc.h"
+#include "mcu_periph/uart.h"
+
+#include "std.h"
+#include "subsystems/nav.h"
+#include "estimator.h"
+#include "autopilot.h"
+#include "generated/flight_plan.h"
+#include "generated/airframe.h"
+#include "inter_mcu.h"
+
+#include "analogimu_util.h"
+
+
+uint16_t analog_raw[NB_ADC];
+
+// variables
+/** gyro rate in rad */
+volatile float gyro[G_LAST] = {0.};
+/** gyro rate offset in rad */
+volatile float gyro_to_zero[G_LAST] = {0.};
+/** acceleration in ms2 */
+volatile float accel[ACC_LAST] = {0.};
+/** angle in rad */
+volatile float angle[ANG_LAST] = {0.};
+/** magnet heading \todo heading ? */
+volatile float heading;
+
+void analogimu_delay( void ) {
+ volatile int i,j;
+ for (i=0;i<1000;i++)
+ for (j=0;j<1000;j++);
+}
+
+bool_t analog_imu_reset( void ) {
+ #ifndef ANALOGIMU_ZERO_AVERAGE
+ gyro_to_zero[GO_ROLL] = gyro[G_ROLL];
+ gyro_to_zero[GO_PITCH] = gyro[G_PITCH];
+ gyro_to_zero[GO_YAW] = gyro[G_YAW];
+ #else
+ /** temp_value for calculating the average */
+ volatile float gyro_to_zero_temp[G_LAST] = {0.};
+
+ gyro_to_zero_temp[GO_ROLL] = gyro[G_ROLL];
+ gyro_to_zero_temp[GO_PITCH] = gyro[G_PITCH];
+ gyro_to_zero_temp[GO_YAW] = gyro[G_YAW];
+
+ analogimu_delay();
+
+ gyro_to_zero_temp[GO_ROLL] += gyro[G_ROLL];
+ gyro_to_zero_temp[GO_PITCH] += gyro[G_PITCH];
+ gyro_to_zero_temp[GO_YAW] += gyro[G_YAW];
+
+ analogimu_delay();
+
+ gyro_to_zero_temp[GO_ROLL] += gyro[G_ROLL];
+ gyro_to_zero_temp[GO_PITCH] += gyro[G_PITCH];
+ gyro_to_zero_temp[GO_YAW] += gyro[G_YAW];
+
+ gyro_to_zero_temp[GO_ROLL] /= 3;
+ gyro_to_zero_temp[GO_PITCH] /= 3;
+ gyro_to_zero_temp[GO_YAW] /= 3;
+
+ gyro_to_zero[GO_ROLL] = gyro_to_zero_temp[G_ROLL];
+ gyro_to_zero[GO_PITCH] = gyro_to_zero_temp[G_PITCH];
+ gyro_to_zero[GO_YAW] = gyro_to_zero_temp[G_YAW];
+ #endif
+ return FALSE;
+}
+
+
diff --git a/sw/airborne/subsystems/analogimu/analogimu_util.h b/sw/airborne/subsystems/analogimu/analogimu_util.h
new file mode 100644
index 0000000000..8a222fbb30
--- /dev/null
+++ b/sw/airborne/subsystems/analogimu/analogimu_util.h
@@ -0,0 +1,66 @@
+/*
+ * $Id: analogimu_imu.h $
+ *
+ * Copyright (C) 2010 Christoph Niemann
+ *
+ * This file is part of paparazzi.
+ *
+ * paparazzi 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 2, or (at your option)
+ * any later version.
+ *
+ * paparazzi 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 paparazzi; see the file COPYING. If not, write to
+ * the Free Software Foundation, 59 Temple Place - Suite 330,
+ * Boston, MA 02111-1307, USA.
+ *
+ */
+
+/** \file analogimu_util.h
+ * \brief Analog IMU Utilities
+ *
+ */
+
+#ifndef _ANALOGIMU_UTIL_H_
+#define _ANALOGIMU_UTIL_H_
+
+#include "std.h"
+
+// defines
+/** defines for gyro[] indicies */
+enum gyro_idx_t { G_ROLL, G_PITCH, G_YAW, G_LAST };
+/** defines for accel[] indicies */
+enum accel_idx_t { ACC_X, ACC_Y, ACC_Z, ACC_LAST };
+/** defines for stick[] indicies */
+enum stick_idx_t { STICK_ROLL, STICK_PITCH, STICK_YAW, STICK_THRUST,
+ STICK_SWR, STICK_SWL, STICK_CH6, STICK_CH7, STICK_LAST };
+/** defines for angle[] indicies */
+enum angle_idx_t { ANG_ROLL, ANG_PITCH, ANG_YAW, ANG_LAST };
+/** defines for gyro_to_zero[] indicies */
+enum gyro_to_zero_idx_t { GO_ROLL, GO_PITCH, GO_YAW, GO_LAST };
+
+// variables
+extern volatile float gyro[];
+/** acceleration in ms2 */
+extern volatile float accel[];
+/** angle in rad */
+extern volatile float angle[];
+/** magnet heading \todo heading ? */
+extern volatile float heading;
+/** analog_raw[] analog downlink arry */
+extern uint16_t analog_raw[];
+
+extern volatile float gyro_to_zero[];
+
+extern bool_t analog_imu_reset( void );
+
+void analogimu_delay( void );
+
+
+#endif
diff --git a/sw/airborne/subsystems/analogimu/arduimu.c b/sw/airborne/subsystems/analogimu/arduimu.c
new file mode 100644
index 0000000000..1a9b86c6f8
--- /dev/null
+++ b/sw/airborne/subsystems/analogimu/arduimu.c
@@ -0,0 +1,282 @@
+#include
+
+#include "wiring.h"
+#include "vector.h"
+#include "matrix.h"
+#include "read_adc.h"
+#include "arduimu.h"
+
+#include "dcm.h"
+
+// Released under Creative Commons License
+// Code by Jordi Munoz and William Premerlani, Supported by Chris Anderson (Wired) and Nathan Sindle (SparkFun).
+// Version 1.0 for flat board updated by Doug Weibel and Jose Julio
+// Version 1.7 includes support for SCP1000 absolute pressure sensor
+
+// Axis definition: X axis pointing forward, Y axis pointing to the right and Z axis pointing down.
+// Positive pitch : nose up
+// Positive roll : right wing down
+// Positive yaw : clockwise
+
+// olri #include
+// olri #include
+
+//**********************************************************************
+// This section contains USER PARAMETERS !!!
+//
+//**********************************************************************
+
+// *** NOTE! Hardware version - Can be used for v1 (daughterboards) or v2 (flat)
+#define BOARD_VERSION 2 // 1 For V1 and 2 for V2
+
+// Ublox gps is recommended!
+#define GPS_PROTOCOL 1 // 1 - Ublox, 2 - EM406, 3 - NMEA We have only tested with Ublox
+
+// Enable Air Start uses Remove Before Fly flag - connection to pin 6 on ArduPilot
+#define ENABLE_AIR_START 1 // 1 if using airstart/groundstart signaling, 0 if not
+#define GROUNDSTART_PIN 8 // Pin number used for ground start signal (recommend 10 on v1 and 8 on v2 hardware)
+
+/*Min Speed Filter for Yaw drift Correction*/
+#define SPEEDFILT 2 // >1 use min speed filter for yaw drift cancellation, 0=do not use speed filter
+
+/*For debugging propurses*/
+#define PRINT_DEBUG 0 //Will print Debug messages
+
+//OUTPUTMODE=1 will print the corrected data, 0 will print uncorrected data of the gyros (with drift), 2 will print accelerometer only data
+#define OUTPUTMODE 1
+
+#define PRINT_DCM 0 //Will print the whole direction cosine matrix
+#define PRINT_ANALOGS 0 //Will print the analog raw data
+#define PRINT_EULER 0 //Will print the Euler angles Roll, Pitch and Yaw
+#define PRINT_GPS 1 //Will print GPS data
+
+// *** NOTE! To use ArduIMU with ArduPilot you must select binary output messages
+#define PRINT_BINARY 1 //Will print binary message and suppress ASCII messages (above)
+
+// *** NOTE! Performance reporting is only supported for Ublox. Set to 0 for others
+#define PERFORMANCE_REPORTING 1 //Will include performance reports in the binary output ~ 1/2 min
+
+/* Support for optional magnetometer (1 enabled, 0 dissabled) */
+#define USE_MAGNETOMETER 0 // use 1 if you want to make yaw gyro drift corrections using the optional magnetometer
+
+/* Support for optional barometer (1 enabled, 0 dissabled) */
+#define USE_BAROMETER 0 // use 1 if you want to get altitude using the optional absolute pressure sensor
+#define ALT_MIX 50 // For binary messages: GPS or barometric altitude. 0 to 100 = % of barometric. For example 75 gives 25% GPS alt and 75% baro
+
+//**********************************************************************
+// End of user parameters
+//**********************************************************************
+
+#define SOFTWARE_VER "1.7"
+
+// ADC : Voltage reference 3.3v / 10bits(1024 steps) => 3.22mV/ADC step
+// ADXL335 Sensitivity(from datasheet) => 330mV/g, 3.22mV/ADC step => 330/3.22 = 102.48
+// Tested value : 101
+//#define GRAVITY 101 //this equivalent to 1G in the raw data coming from the accelerometer
+//#define GRAVITY 9.81
+
+// olri #define Accel_Scale(x) x*(GRAVITY/9.81)//Scaling the raw data of the accel to actual acceleration in meters for seconds square
+
+// olri #define ToRad(x) (x*0.01745329252) // *pi/180
+// olri #define ToDeg(x) (x*57.2957795131) // *180/pi
+
+// LPR530 & LY530 Sensitivity (from datasheet) => 3.33mV/º/s, 3.22mV/ADC step => 1.03
+// Tested values : 0.96,0.96,0.94
+ // #define Gyro_Gain_X 0.92 //X axis Gyro gain
+ // #define Gyro_Gain_Y 0.92 //Y axis Gyro gain
+ // #define Gyro_Gain_Z 0.94 //Z axis Gyro gain
+ // olri #define Gyro_Scaled_X(x) x*ToRad(Gyro_Gain_X) //Return the scaled ADC raw data of the gyro in radians for second
+ // olri #define Gyro_Scaled_Y(x) x*ToRad(Gyro_Gain_Y) //Return the scaled ADC raw data of the gyro in radians for second
+ // olri #define Gyro_Scaled_Z(x) x*ToRad(Gyro_Gain_Z) //Return the scaled ADC raw data of the gyro in radians for second
+
+//olri #define Kp_ROLLPITCH 0.015
+// olri #define Ki_ROLLPITCH 0.000010
+// olri #define Kp_YAW 1.2
+//#define Kp_YAW 2.5 //High yaw drift correction gain - use with caution!
+// olri #define Ki_YAW 0.00005
+
+/*UBLOX Maximum payload length*/
+#define UBX_MAXPAYLOAD 56
+
+#define ADC_WARM_CYCLES 75
+
+//olri #define FALSE 0
+// olri #define TRUE 1
+
+
+//float G_Dt=0.02; // Integration time (DCM algorithm)
+//chni: changed to 50ms, according to HSB-Data
+float G_Dt=0.05;
+
+long timeNow=0; // Hold the milliseond value for now
+long timer=0; //general purpuse timer
+long timer_old;
+long timer24=0; //Second timer used to print values
+boolean groundstartDone = false; // Used to not repeat ground start
+
+float AN[8]; //array that store the 6 ADC filtered data
+float AN_OFFSET[8]; //Array that stores the Offset of the gyros
+
+float Accel_Vector[3]= {0,0,0}; //Store the acceleration in a vector
+float Gyro_Vector[3]= {0,0,0};//Store the gyros rutn rate in a vector
+float Omega_Vector[3]= {0,0,0}; //Corrected Gyro_Vector data
+float Omega_P[3]= {0,0,0};//Omega Proportional correction
+float Omega_I[3]= {0,0,0};//Omega Integrator
+float Omega[3]= {0,0,0};
+
+//Magnetometer variables
+int magnetom_x;
+int magnetom_y;
+int magnetom_z;
+float MAG_Heading;
+
+// Euler angles
+float roll;
+float pitch;
+float yaw;
+
+float errorRollPitch[3]= {0,0,0};
+float errorYaw[3]= {0,0,0};
+float errorCourse=180;
+float COGX=0; //Course overground X axis
+float COGY=1; //Course overground Y axis
+
+unsigned int cycleCount=0;
+byte gyro_sat=0;
+
+float DCM_Matrix[3][3]= {
+ {
+ 1,0,0 }
+ ,{
+ 0,1,0 }
+ ,{
+ 0,0,1 }
+};
+float Update_Matrix[3][3]={{0,1,2},{3,4,5},{6,7,8}}; //Gyros here
+
+
+float Temporary_Matrix[3][3]={
+ {
+ 0,0,0 }
+ ,{
+ 0,0,0 }
+ ,{
+ 0,0,0 }
+};
+
+//GPS
+
+//GPS stuff
+union long_union {
+ int32_t dword;
+ uint8_t byte[4];
+} longUnion;
+
+union int_union {
+ int16_t word;
+ uint8_t byte[2];
+} intUnion;
+
+/*Flight GPS variables*/
+int gpsFix=1; //This variable store the status of the GPS
+int gpsFixnew=0; //used to flag when new gps data received - used for binary output message flags
+int gps_fix_count = 5; //used to count 5 good fixes at ground startup
+long lat=0; // store the Latitude from the gps to pass to output
+long lon=0; // Store the Longitude from the gps to pass to output
+long alt_MSL=0; //This is the altitude in millimeters
+long iTOW=0; //GPS Millisecond Time of Week
+long alt=0; //Height above Ellipsoid in millimeters
+float speed_3d=0; //Speed (3-D)
+float ground_speed=0;// This is the velocity your "plane" is traveling in meters for second, 1Meters/Second= 3.6Km/H = 1.944 knots
+float ground_course=90;//This is the runaway direction of you "plane" in degrees
+float gc_offset = 0; // Force yaw output to ground course when fresh data available (only implemented for ublox&binary message)
+byte numSV=0; //Number of Sats used.
+float ecefVZ=0; //Vertical Speed in m/s
+unsigned long GPS_timer=0;
+
+#if GPS_PROTOCOL == 1
+// GPS UBLOX
+//byte ck_a=0; // Packet checksum
+//byte ck_b=0;
+byte UBX_step=0;
+byte UBX_class=0;
+byte UBX_id=0;
+byte UBX_payload_length_hi=0;
+byte UBX_payload_length_lo=0;
+byte UBX_payload_counter=0;
+byte UBX_buffer[UBX_MAXPAYLOAD];
+byte UBX_ck_a=0;
+byte UBX_ck_b=0;
+#endif
+
+//ADC variables
+volatile uint8_t MuxSel=0;
+volatile uint8_t analog_reference = DEFAULT;
+volatile uint16_t analog_buffer[8];
+volatile uint8_t analog_count[8];
+
+
+ #if BOARD_VERSION == 1
+ uint8_t sensors[6] = {0,2,1,3,5,4}; // Use these two lines for Hardware v1 (w/ daughterboards)
+ int SENSOR_SIGN[]= {1,-1,1,-1,1,-1,-1,-1,-1}; //Sensor: GYROX, GYROY, GYROZ, ACCELX, ACCELY, ACCELZ
+ #endif
+
+ #if BOARD_VERSION == 2
+ uint8_t sensors[6] = {6,7,3,0,1,2}; // For Hardware v2 flat
+ int SENSOR_SIGN[] = {1,-1,-1,1,-1,1,-1,-1,-1};
+ #endif
+
+ // Performance Monitoring variables
+ // Data collected and reported for ~1/2 minute intervals
+ #if PERFORMANCE_REPORTING == 1
+ int mainLoop_count = 0; //Main loop cycles since last report
+ int G_Dt_max = 0.0; //Max main loop cycle time in milliseconds
+ byte gyro_sat_count = 0;
+ byte adc_constraints = 0;
+ byte renorm_sqrt_count = 0;
+ byte renorm_blowup_count = 0;
+ byte gps_payload_error_count = 0;
+ byte gps_checksum_error_count = 0;
+ byte gps_pos_fix_count = 0;
+ byte gps_nav_fix_count = 0;
+ byte gps_messages_sent = 0;
+ long perf_mon_timer = 0;
+ #endif
+ unsigned int imu_health = 65012;
+
+//**********************************************************************
+// This section contains SCP1000_D11 PARAMETERS !!!
+//**********************************************************************
+#if USE_BAROMETER == 1
+#define SCP_MODE (9) // 9 = high speed mode, 10 = high resolution mode
+#define PRESSURE_ADDR (0x11U) // IIC address of the SCP1000
+// ************ #define START_ALTITUDE (217U) // default initial altitude in m above sea level
+
+// When we have to manage data transfers via IIC directly we need to use the following addresses
+// IIC address of the SCP1000 device forms the Top 7 bits of the address with the R/W bit as the LSB
+#define READ_PRESSURE_ADDR (PRESSURE_ADDR<<1 | 1)
+#define WRITE_PRESSURE_ADDR (PRESSURE_ADDR<<1)
+
+// SCP1000 Register addresses
+#define SNS_ADDR_POPERATION (0x03U) // OPERATON register
+#define SNS_ADDR_PSTATUS (0x07U) // STATUS register
+#define SNS_ADDR_PPRESSURE (0x80U) // DATARD16 Register (pressure)
+#define SNS_ADDR_DATARD8 (0x7FU) // DAYARD8 Register
+#define SNS_ADDR_PTEMP (0x81U) // TEMPOUT Register (temperature)
+
+#ifndef TRUE
+#define TRUE (0x01)
+#endif
+#ifndef FALSE
+#define FALSE (0x00)
+#endif
+
+int temp_unfilt = 0;
+int temperature = 0;
+unsigned long press = 0;
+unsigned long press_filt = 0;
+unsigned long press_gnd = 0;
+long ground_alt = 0; // Ground altitude in millimeters
+long press_alt = 0; // Pressure altitude in millimeters
+
+#endif
diff --git a/sw/airborne/subsystems/analogimu/arduimu.h b/sw/airborne/subsystems/analogimu/arduimu.h
new file mode 100644
index 0000000000..6d1fd31830
--- /dev/null
+++ b/sw/airborne/subsystems/analogimu/arduimu.h
@@ -0,0 +1,130 @@
+#include "wiring.h"
+/* This file was automatically generated. Do not edit! */
+void debug_print(char string[]);
+void startup_air(void);
+void debug_handler(byte message);
+void startup_ground(void);
+//void loop();
+#if USE_BAROMETER == 1
+extern long press_alt;
+extern long ground_alt;
+extern unsigned long press_gnd;
+extern unsigned long press_filt;
+extern unsigned long press;
+extern int temperature;
+extern int temp_unfilt;
+#endif
+extern unsigned int imu_health;
+extern long perf_mon_timer;
+extern byte gps_messages_sent;
+extern byte gps_nav_fix_count;
+extern byte gps_pos_fix_count;
+extern byte gps_checksum_error_count;
+extern byte gps_payload_error_count;
+extern byte renorm_blowup_count;
+extern byte renorm_sqrt_count;
+extern byte adc_constraints;
+extern byte gyro_sat_count;
+extern int G_Dt_max;
+extern int SENSOR_SIGN[];
+extern volatile uint8_t analog_count[8];
+extern volatile uint16_t analog_buffer[8];
+extern volatile uint8_t analog_reference;
+extern volatile uint8_t MuxSel;
+#if GPS_PROTOCOL == 1
+extern byte UBX_ck_b;
+extern byte UBX_ck_a;
+extern byte UBX_buffer[UBX_MAXPAYLOAD];
+extern byte UBX_payload_counter;
+extern byte UBX_payload_length_lo;
+extern byte UBX_payload_length_hi;
+extern byte UBX_id;
+extern byte UBX_class;
+extern byte UBX_step;
+extern byte ck_b;
+extern byte ck_a;
+#endif
+extern unsigned long GPS_timer;
+extern float ecefVZ;
+extern byte numSV;
+extern float gc_offset;
+extern float ground_course;
+extern float ground_speed;
+extern float speed_3d;
+extern long alt;
+extern long iTOW;
+extern long alt_MSL;
+extern long lon;
+extern long lat;
+extern int gps_fix_count;
+extern int gpsFixnew;
+extern int gpsFix;
+extern float Temporary_Matrix[3][3];
+extern float Update_Matrix[3][3];
+extern float DCM_Matrix[3][3];
+extern byte gyro_sat;
+extern unsigned int cycleCount;
+extern float COGY;
+extern float COGX;
+extern float errorCourse;
+extern float errorYaw[3];
+extern float errorRollPitch[3];
+extern float yaw;
+extern float pitch;
+extern float roll;
+extern float MAG_Heading;
+extern int magnetom_z;
+extern int magnetom_y;
+extern int magnetom_x;
+extern float Omega[3];
+extern float Omega_I[3];
+extern float Omega_P[3];
+extern float Omega_Vector[3];
+extern float Gyro_Vector[3];
+extern float Accel_Vector[3];
+extern float AN_OFFSET[8];
+extern float AN[8];
+extern boolean groundstartDone;
+extern long timer24;
+extern long timer_old;
+extern long timer;
+extern long timeNow;
+extern float G_Dt;
+#define FALSE 0
+#define TRUE 1
+//#define GRAVITY 101 //this equivalent to 1G in the raw data coming from the accelerometer
+//chni:
+#define GRAVITY 9.9074 // typical 9.81 sensor depend, hier Z Sensor Wert unserer IMU in Normallage
+
+//#define Kp_ROLLPITCH 0.2
+#define Kp_ROLLPITCH 0.015
+//#define Kp_YAW 1.2
+#define Kp_YAW 1.2 //High yaw drift correction gain - use with caution!
+#define Ki_YAW 0.00005
+#define Ki_ROLLPITCH 0.000010
+#define SPEEDFILT 2 // >1 use min speed filter for yaw drift cancellation, 0=do not use speed filter
+#define ToRad(x) (x*0.01745329252) // *pi/180
+#define ToDeg(x) (x*57.2957795131) // *180/pi
+#define Accel_Scale(x) x*(GRAVITY/9.81) //Scaling the raw data of the accel to actual acceleration in meters for seconds square
+
+#define USE_DEFEKT
+#ifdef USE_DEFEKT
+#define Gyro_Scaled_X(x) x*ToRad(Gyro_Gain_X) //Return the scaled ADC raw data of the gyro in radians for second
+#define Gyro_Scaled_Y(x) x*ToRad(Gyro_Gain_Y) //Return the scaled ADC raw data of the gyro in radians for second
+#define Gyro_Scaled_Z(x) x*ToRad(Gyro_Gain_Z) //Return the scaled ADC raw data of the gyro in radians for second
+#else
+#define Gyro_Scaled_X(x) x*(Gyro_Gain_X) //Return the scaled ADC raw data of the gyro in radians for second
+#define Gyro_Scaled_Y(x) x*(Gyro_Gain_Y) //Return the scaled ADC raw data of the gyro in radians for second
+#define Gyro_Scaled_Z(x) x*(Gyro_Gain_Z) //Return the scaled ADC raw data of the gyro in radians for second
+#endif
+
+#define Gyro_Gain_X 0.92 //X axis Gyro gain
+#define Gyro_Gain_Y 0.92 //Y axis Gyro gain
+#define Gyro_Gain_Z 0.94 //Z axis Gyro gain
+//#define Gyro_Gain_X 1.0 //X axis Gyro gain
+//#define Gyro_Gain_Y 1.0 //Y axis Gyro gain
+//#define Gyro_Gain_Z 1.0 //Z axis Gyro gain
+
+void dcm_init( int i );
+
+extern float adc[11];
diff --git a/sw/airborne/subsystems/analogimu/dcm.c b/sw/airborne/subsystems/analogimu/dcm.c
new file mode 100644
index 0000000000..001acfb778
--- /dev/null
+++ b/sw/airborne/subsystems/analogimu/dcm.c
@@ -0,0 +1,339 @@
+#include
+
+#include "wiring.h"
+#include "vector.h"
+#include "matrix.h"
+#include "read_adc.h"
+#include "arduimu.h"
+
+#ifdef ANALOG_IMU
+#include "analogimu.h"
+#include "analogimu_util.h"
+#endif // ANALOG_IMU
+
+#include "dcm.h"
+/**
+* Geschätzte Winkel in der Eulerdarstellung
+* Estimated angles as Euler
+*/
+float euler[EULER_LAST] = {0.};
+
+/**************************************************/
+void Normalize(void)
+{
+ float error=0;
+ float temporary[3][3];
+ float renorm=0;
+ boolean problem=FALSE;
+
+ error= -Vector_Dot_Product(&DCM_Matrix[0][0],&DCM_Matrix[1][0])*.5; //eq.19
+
+ Vector_Scale(&temporary[0][0], &DCM_Matrix[1][0], error); //eq.19
+ Vector_Scale(&temporary[1][0], &DCM_Matrix[0][0], error); //eq.19
+
+ Vector_Add(&temporary[0][0], &temporary[0][0], &DCM_Matrix[0][0]);//eq.19
+ Vector_Add(&temporary[1][0], &temporary[1][0], &DCM_Matrix[1][0]);//eq.19
+
+ Vector_Cross_Product(&temporary[2][0],&temporary[0][0],&temporary[1][0]); // c= a x b //eq.20
+
+ renorm= Vector_Dot_Product(&temporary[0][0],&temporary[0][0]);
+ if (renorm < 1.5625f && renorm > 0.64f) {
+ renorm= .5 * (3-renorm); //eq.21
+ } else if (renorm < 100.0f && renorm > 0.01f) {
+ renorm= 1. / sqrt(renorm);
+#if PERFORMANCE_REPORTING == 1
+ renorm_sqrt_count++;
+#endif
+#if PRINT_DEBUG != 0
+ Serial.print("???SQT:1,RNM:");
+ Serial.print (renorm);
+ Serial.print (",ERR:");
+ Serial.print (error);
+ Serial.print (",TOW:");
+ Serial.print (iTOW);
+ Serial.println("***");
+#endif
+ } else {
+ problem = TRUE;
+#if PERFORMANCE_REPORTING == 1
+ renorm_blowup_count++;
+#endif
+ #if PRINT_DEBUG != 0
+ Serial.print("???PRB:1,RNM:");
+ Serial.print (renorm);
+ Serial.print (",ERR:");
+ Serial.print (error);
+ Serial.print (",TOW:");
+ Serial.print (iTOW);
+ Serial.println("***");
+#endif
+ }
+ Vector_Scale(&DCM_Matrix[0][0], &temporary[0][0], renorm);
+
+ renorm= Vector_Dot_Product(&temporary[1][0],&temporary[1][0]);
+ if (renorm < 1.5625f && renorm > 0.64f) {
+ renorm= .5 * (3-renorm); //eq.21
+ } else if (renorm < 100.0f && renorm > 0.01f) {
+ renorm= 1. / sqrt(renorm);
+#if PERFORMANCE_REPORTING == 1
+ renorm_sqrt_count++;
+#endif
+#if PRINT_DEBUG != 0
+ Serial.print("???SQT:2,RNM:");
+ Serial.print (renorm);
+ Serial.print (",ERR:");
+ Serial.print (error);
+ Serial.print (",TOW:");
+ Serial.print (iTOW);
+ Serial.println("***");
+#endif
+ } else {
+ problem = TRUE;
+#if PERFORMANCE_REPORTING == 1
+ renorm_blowup_count++;
+#endif
+#if PRINT_DEBUG != 0
+ Serial.print("???PRB:2,RNM:");
+ Serial.print (renorm);
+ Serial.print (",ERR:");
+ Serial.print (error);
+ Serial.print (",TOW:");
+ Serial.print (iTOW);
+ Serial.println("***");
+#endif
+ }
+ Vector_Scale(&DCM_Matrix[1][0], &temporary[1][0], renorm);
+
+ renorm= Vector_Dot_Product(&temporary[2][0],&temporary[2][0]);
+ if (renorm < 1.5625f && renorm > 0.64f) {
+ renorm= .5 * (3-renorm); //eq.21
+ } else if (renorm < 100.0f && renorm > 0.01f) {
+ renorm= 1. / sqrt(renorm);
+#if PERFORMANCE_REPORTING == 1
+ renorm_sqrt_count++;
+#endif
+#if PRINT_DEBUG != 0
+ Serial.print("???SQT:3,RNM:");
+ Serial.print (renorm);
+ Serial.print (",ERR:");
+ Serial.print (error);
+ Serial.print (",TOW:");
+ Serial.print (iTOW);
+ Serial.println("***");
+#endif
+ } else {
+ problem = TRUE;
+#if PERFORMANCE_REPORTING == 1
+ renorm_blowup_count++;
+#endif
+#if PRINT_DEBUG != 0
+ Serial.print("???PRB:3,RNM:");
+ Serial.print (renorm);
+ Serial.print (",TOW:");
+ Serial.print (iTOW);
+ Serial.println("***");
+#endif
+ }
+ Vector_Scale(&DCM_Matrix[2][0], &temporary[2][0], renorm);
+
+ if (problem) { // Our solution is blowing up and we will force back to initial condition. Hope we are not upside down!
+ DCM_Matrix[0][0]= 1.0f;
+ DCM_Matrix[0][1]= 0.0f;
+ DCM_Matrix[0][2]= 0.0f;
+ DCM_Matrix[1][0]= 0.0f;
+ DCM_Matrix[1][1]= 1.0f;
+ DCM_Matrix[1][2]= 0.0f;
+ DCM_Matrix[2][0]= 0.0f;
+ DCM_Matrix[2][1]= 0.0f;
+ DCM_Matrix[2][2]= 1.0f;
+ problem = FALSE;
+ }
+}
+
+/**************************************************/
+ extern short gps_course;
+ extern short gps_gspeed;
+ extern short gps_climb;
+ extern short gps_mode;
+
+void Drift_correction(void)
+{
+ //Compensation the Roll, Pitch and Yaw drift.
+ static float Scaled_Omega_P[3];
+ static float Scaled_Omega_I[3];
+ float Accel_magnitude;
+ float Accel_weight;
+ float Integrator_magnitude;
+ // hwarm
+ ground_course=gps_course/10.-180.;
+ ground_speed=gps_gspeed/100.;
+ float ground_climb=gps_climb/100.;
+ speed_3d= sqrt(ground_speed*ground_speed+ground_climb*ground_climb);
+ //*****Roll and Pitch***************
+
+ // Calculate the magnitude of the accelerometer vector
+ Accel_magnitude = sqrt(Accel_Vector[0]*Accel_Vector[0] + Accel_Vector[1]*Accel_Vector[1] + Accel_Vector[2]*Accel_Vector[2]);
+ Accel_magnitude = Accel_magnitude / GRAVITY; // Scale to gravity.
+ // Dynamic weighting of accelerometer info (reliability filter)
+ // Weight for accelerometer info (<0.5G = 0.0, 1G = 1.0 , >1.5G = 0.0)
+ Accel_weight = constrain(1 - 2*abs(1 - Accel_magnitude),0,1); //
+
+ #if PERFORMANCE_REPORTING == 1
+ tempfloat = ((Accel_weight - 0.5) * 256.0f); //amount added was determined to give imu_health a time constant about twice the time constant of the roll/pitch drift correction
+ imu_health += tempfloat;
+ imu_health = constrain(imu_health,129,65405);
+ #endif
+
+ Vector_Cross_Product(&errorRollPitch[0],&Accel_Vector[0],&DCM_Matrix[2][0]); //adjust the ground of reference
+ Vector_Scale(&Omega_P[0],&errorRollPitch[0],Kp_ROLLPITCH*Accel_weight);
+
+ Vector_Scale(&Scaled_Omega_I[0],&errorRollPitch[0],Ki_ROLLPITCH*Accel_weight);
+ Vector_Add(Omega_I,Omega_I,Scaled_Omega_I);
+
+ //*****YAW***************
+
+ #if USE_MAGNETOMETER==1
+ // We make the gyro YAW drift correction based on compass magnetic heading
+ mag_heading_x = cos(MAG_Heading);
+ mag_heading_y = sin(MAG_Heading);
+ errorCourse=(DCM_Matrix[0][0]*mag_heading_y) - (DCM_Matrix[1][0]*mag_heading_x); //Calculating YAW error
+ Vector_Scale(errorYaw,&DCM_Matrix[2][0],errorCourse); //Applys the yaw correction to the XYZ rotation of the aircraft, depeding the position.
+
+ Vector_Scale(&Scaled_Omega_P[0],&errorYaw[0],Kp_YAW);
+ Vector_Add(Omega_P,Omega_P,Scaled_Omega_P);//Adding Proportional.
+
+ Vector_Scale(&Scaled_Omega_I[0],&errorYaw[0],Ki_YAW);
+ Vector_Add(Omega_I,Omega_I,Scaled_Omega_I);//adding integrator to the Omega_I
+ #else // Use GPS Ground course to correct yaw gyro drift
+ if(gps_mode==3 && ground_speed>= 0.5) //hwarm
+ {
+
+ COGX = cos(ToRad(ground_course));
+ COGY = sin(ToRad(ground_course));
+ errorCourse=(DCM_Matrix[0][0]*COGY) - (DCM_Matrix[1][0]*COGX); //Calculating YAW error
+ Vector_Scale(errorYaw,&DCM_Matrix[2][0],errorCourse); //Applys the yaw correction to the XYZ rotation of the aircraft, depeding the position.
+
+ Vector_Scale(&Scaled_Omega_P[0],&errorYaw[0],Kp_YAW);
+ Vector_Add(Omega_P,Omega_P,Scaled_Omega_P);//Adding Proportional.
+
+ Vector_Scale(&Scaled_Omega_I[0],&errorYaw[0],Ki_YAW);
+ Vector_Add(Omega_I,Omega_I,Scaled_Omega_I);//adding integrator to the Omega_I
+ }
+ #endif
+ // Here we will place a limit on the integrator so that the integrator cannot ever exceed half the saturation limit of the gyros
+ Integrator_magnitude = sqrt(Vector_Dot_Product(Omega_I,Omega_I));
+ if (Integrator_magnitude > ToRad(300)) {
+ Vector_Scale(Omega_I,Omega_I,0.5f*ToRad(300)/Integrator_magnitude);
+#if PRINT_DEBUG != 0
+ Serial.print("!!!INT:1,MAG:");
+ Serial.print (ToDeg(Integrator_magnitude));
+
+ Serial.print (",TOW:");
+ Serial.print (iTOW);
+ Serial.println("***");
+#endif
+ }
+
+
+}
+/**************************************************/
+void Accel_adjust(void)
+{
+ #ifndef ANALOGIMU_ROTATED
+ Accel_Vector[1] += Accel_Scale(speed_3d*Omega[2]); // Centrifugal force on Acc_y = GPS_speed*GyroZ
+ Accel_Vector[2] -= Accel_Scale(speed_3d*Omega[1]); // Centrifugal force on Acc_z = GPS_speed*GyroY
+ #else
+ Accel_Vector[0] -= Accel_Scale(speed_3d*Omega[2]); // Centrifugal force on Acc_x = GPS_speed*GyroZ (ok, wenn x beim rollen nach rechts negativ)
+ Accel_Vector[2] -= Accel_Scale(speed_3d*Omega[0]); // Centrifugal force on Acc_z = GPS_speed*GyroX (ok, wenn nase hoch positiv)
+ #endif
+}
+/**************************************************/
+
+void Matrix_update(void)
+{
+ /* chni: Gyro_Vector[0]=Gyro_Scaled_X(read_adc(3)); //gyro x roll
+ Gyro_Vector[1]=Gyro_Scaled_Y(read_adc(4)); //gyro y pitch
+ Gyro_Vector[2]=Gyro_Scaled_Z(read_adc(5)); //gyro Z yaw
+
+ Accel_Vector[0]=read_adc(0); // acc x
+ Accel_Vector[1]=read_adc(1); // acc y
+ Accel_Vector[2]=read_adc(2); // acc z */
+
+ /* chni: Offset is set dynamic on Ground*/
+ Gyro_Vector[0]= -gyro_to_zero[G_ROLL] + gyro[G_ROLL];
+ Gyro_Vector[1]= -gyro_to_zero[G_PITCH] + gyro[G_PITCH];
+ Gyro_Vector[2]= -gyro_to_zero[G_PITCH] + gyro[G_YAW];
+
+ Accel_Vector[0] = + 0.0 + accel[ACC_X];
+ Accel_Vector[1] = - 0.286 + accel[ACC_Y];
+ Accel_Vector[2] = accel[ACC_Z];
+
+
+
+ Vector_Add(&Omega[0], &Gyro_Vector[0], &Omega_I[0]); //adding proportional term
+ Vector_Add(&Omega_Vector[0], &Omega[0], &Omega_P[0]); //adding Integrator term
+#define USE_GPS
+#ifdef USE_GPS
+ if (gps_mode==3) Accel_adjust(); //Remove centrifugal acceleration.
+#endif
+
+#define OUTPUTMODE 1
+ #if OUTPUTMODE==1 // With corrected data (drift correction)
+ Update_Matrix[0][0]=0;
+ Update_Matrix[0][1]=-G_Dt*Omega_Vector[2];//-z
+ Update_Matrix[0][2]=G_Dt*Omega_Vector[1];//y
+ Update_Matrix[1][0]=G_Dt*Omega_Vector[2];//z
+ Update_Matrix[1][1]=0;
+ Update_Matrix[1][2]=-G_Dt*Omega_Vector[0];//-x
+ Update_Matrix[2][0]=-G_Dt*Omega_Vector[1];//-y
+ Update_Matrix[2][1]=G_Dt*Omega_Vector[0];//x
+ Update_Matrix[2][2]=0;
+ #else // Uncorrected data (no drift correction)
+ Update_Matrix[0][0]=0;
+ Update_Matrix[0][1]=-G_Dt*Gyro_Vector[2];//-z
+ Update_Matrix[0][2]=G_Dt*Gyro_Vector[1];//y
+ Update_Matrix[1][0]=G_Dt*Gyro_Vector[2];//z
+ Update_Matrix[1][1]=0;
+ Update_Matrix[1][2]=-G_Dt*Gyro_Vector[0];
+ Update_Matrix[2][0]=-G_Dt*Gyro_Vector[1];
+ Update_Matrix[2][1]=G_Dt*Gyro_Vector[0];
+ Update_Matrix[2][2]=0;
+ #endif
+
+ Matrix_Multiply(DCM_Matrix,Update_Matrix,Temporary_Matrix); //a*b=c
+
+ for(int x=0; x<3; x++) //Matrix Addition (update)
+ {
+ for(int y=0; y<3; y++)
+ {
+ DCM_Matrix[x][y]+=Temporary_Matrix[x][y];
+ }
+ }
+}
+
+void Euler_angles(void)
+{
+ //#define OUTPUTMODE 2
+ #if (OUTPUTMODE==2) // Only accelerometer info (debugging purposes)
+ euler[EULER_ROLL] = atan2(Accel_Vector[1],Accel_Vector[2]); // atan2(acc_y,acc_z)
+ //euler[EULER_PITCH] = -asin((Accel_Vector[0])/(double)GRAVITY); // asin(acc_x)
+ //todo: chni:ordentlich lösen!
+ euler[EULER_PITCH] = -asin((Accel_Vector[0])/9.81); // asin(acc_x)
+ euler[EULER_YAW] = 0;
+ #else
+ //pitch = -asin(DCM_Matrix[2][0]);
+ //roll = atan2(DCM_Matrix[2][1],DCM_Matrix[2][2]);
+ //yaw = atan2(DCM_Matrix[1][0],DCM_Matrix[0][0]);
+ #ifndef ANALOGIMU_ROTATED
+ euler[EULER_PITCH] = -asin(DCM_Matrix[2][0]);
+ euler[EULER_ROLL] = atan2(DCM_Matrix[2][1],DCM_Matrix[2][2]);
+ #else
+ euler[EULER_ROLL] = -asin(DCM_Matrix[2][0]);
+ euler[EULER_PITCH] = -atan2(DCM_Matrix[2][1],DCM_Matrix[2][2]);
+ #endif
+
+ euler[EULER_YAW] = atan2(DCM_Matrix[1][0],DCM_Matrix[0][0]);
+ euler[EULER_YAW] += M_PI; // Rotating the angle 180deg to fit for PPRZ
+ #endif
+}
+
diff --git a/sw/airborne/subsystems/analogimu/dcm.h b/sw/airborne/subsystems/analogimu/dcm.h
new file mode 100644
index 0000000000..b4c2308a7b
--- /dev/null
+++ b/sw/airborne/subsystems/analogimu/dcm.h
@@ -0,0 +1,15 @@
+void Euler_angles(void);
+void Matrix_update(void);
+void Accel_adjust(void);
+void Drift_correction(void);
+void Normalize(void);
+
+/** defines for euler vector */
+enum euler_idx_t { EULER_ROLL, EULER_PITCH, EULER_YAW, EULER_LAST };
+
+#define M_PI 3.14159265358979323846
+
+// variables
+
+/** output vector with angles in rad */
+extern float euler[EULER_LAST];
diff --git a/sw/airborne/subsystems/analogimu/matrix.c b/sw/airborne/subsystems/analogimu/matrix.c
new file mode 100644
index 0000000000..436e5e554e
--- /dev/null
+++ b/sw/airborne/subsystems/analogimu/matrix.c
@@ -0,0 +1,24 @@
+/**************************************************/
+//Multiply two 3x3 matrixs. This function developed by Jordi can be easily adapted to multiple n*n matrix's. (Pero me da flojera!).
+
+#include "matrix.h"
+
+void Matrix_Multiply(float a[3][3], float b[3][3],float mat[3][3])
+{
+ float op[3];
+ for(int x=0; x<3; x++)
+ {
+ for(int y=0; y<3; y++)
+ {
+ for(int w=0; w<3; w++)
+ {
+ op[w]=a[x][w]*b[w][y];
+ }
+ mat[x][y]=0;
+ mat[x][y]=op[0]+op[1]+op[2];
+
+ }
+ }
+}
+
+
diff --git a/sw/airborne/subsystems/analogimu/matrix.h b/sw/airborne/subsystems/analogimu/matrix.h
new file mode 100644
index 0000000000..0b6f1639dd
--- /dev/null
+++ b/sw/airborne/subsystems/analogimu/matrix.h
@@ -0,0 +1,2 @@
+
+void Matrix_Multiply(float a[3][3],float b[3][3],float mat[3][3]);
diff --git a/sw/airborne/subsystems/analogimu/read_adc.h b/sw/airborne/subsystems/analogimu/read_adc.h
new file mode 100644
index 0000000000..cc1210ac7a
--- /dev/null
+++ b/sw/airborne/subsystems/analogimu/read_adc.h
@@ -0,0 +1,2 @@
+/* This file was automatically generated. Do not edit! */
+float read_adc(char i);
diff --git a/sw/airborne/subsystems/analogimu/vector.c b/sw/airborne/subsystems/analogimu/vector.c
new file mode 100644
index 0000000000..3156312a16
--- /dev/null
+++ b/sw/airborne/subsystems/analogimu/vector.c
@@ -0,0 +1,42 @@
+#include "vector.h"
+
+//Computes the dot product of two vectors
+float Vector_Dot_Product(float vector1[3],float vector2[3])
+{
+ float op=0;
+
+ for(int c=0; c<3; c++)
+ {
+ op+=vector1[c]*vector2[c];
+ }
+
+ return op;
+}
+
+//Computes the cross product of two vectors
+void Vector_Cross_Product(float vectorOut[3], float v1[3],float v2[3])
+{
+ vectorOut[0]= (v1[1]*v2[2]) - (v1[2]*v2[1]);
+ vectorOut[1]= (v1[2]*v2[0]) - (v1[0]*v2[2]);
+ vectorOut[2]= (v1[0]*v2[1]) - (v1[1]*v2[0]);
+}
+
+//Multiply the vector by a scalar.
+void Vector_Scale(float vectorOut[3],float vectorIn[3], float scale2)
+{
+ for(int c=0; c<3; c++)
+ {
+ vectorOut[c]=vectorIn[c]*scale2;
+ }
+}
+
+void Vector_Add(float vectorOut[3],float vectorIn1[3], float vectorIn2[3])
+{
+ for(int c=0; c<3; c++)
+ {
+ vectorOut[c]=vectorIn1[c]+vectorIn2[c];
+ }
+}
+
+
+
diff --git a/sw/airborne/subsystems/analogimu/vector.h b/sw/airborne/subsystems/analogimu/vector.h
new file mode 100644
index 0000000000..b9686543a1
--- /dev/null
+++ b/sw/airborne/subsystems/analogimu/vector.h
@@ -0,0 +1,5 @@
+
+void Vector_Add(float vectorOut[3],float vectorIn1[3],float vectorIn2[3]);
+void Vector_Scale(float vectorOut[3],float vectorIn[3],float scale2);
+void Vector_Cross_Product(float vectorOut[3],float v1[3],float v2[3]);
+float Vector_Dot_Product(float vector1[3],float vector2[3]);
diff --git a/sw/airborne/subsystems/analogimu/wiring.h b/sw/airborne/subsystems/analogimu/wiring.h
new file mode 100644
index 0000000000..73368f4b24
--- /dev/null
+++ b/sw/airborne/subsystems/analogimu/wiring.h
@@ -0,0 +1,135 @@
+
+/*
+ wiring.h - Partial implementation of the Wiring API for the ATmega8.
+ Part of Arduino - http://www.arduino.cc/
+
+ Copyright (c) 2005-2006 David A. Mellis
+
+ This library is free software; you can redistribute it and/or
+ modify it under the terms of the GNU Lesser General Public
+ License as published by the Free Software Foundation; either
+ version 2.1 of the License, or (at your option) any later version.
+
+ This library 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
+ Lesser General Public License for more details.
+
+ You should have received a copy of the GNU Lesser General
+ Public License along with this library; if not, write to the
+ Free Software Foundation, Inc., 59 Temple Place, Suite 330,
+ Boston, MA 02111-1307 USA
+
+ $Id: wiring.h 804 2009-12-18 16:05:52Z dmellis $
+*/
+
+#ifndef Wiring_h
+#define Wiring_h
+
+// olri #include
+// olri #include "binary.h"
+#include
+
+#ifdef __cplusplus
+extern "C"{
+#endif
+
+#define HIGH 0x1
+#define LOW 0x0
+
+#define INPUT 0x0
+#define OUTPUT 0x1
+
+#define true 0x1
+#define false 0x0
+
+#define PI 3.1415926535897932384626433832795
+#define HALF_PI 1.5707963267948966192313216916398
+#define TWO_PI 6.283185307179586476925286766559
+#define DEG_TO_RAD 0.017453292519943295769236907684886
+#define RAD_TO_DEG 57.295779513082320876798154814105
+
+#define SERIAL 0x0
+#define DISPLAY 0x1
+
+#define LSBFIRST 0
+#define MSBFIRST 1
+
+#define CHANGE 1
+#define FALLING 2
+#define RISING 3
+
+#define INTERNAL 3
+#define DEFAULT 1
+#define EXTERNAL 0
+
+// undefine stdlib's abs if encountered
+#ifdef abs
+#undef abs
+#endif
+
+#define min(a,b) ((a)<(b)?(a):(b))
+#define max(a,b) ((a)>(b)?(a):(b))
+#define abs(x) ((x)>0?(x):-(x))
+#define constrain(amt,low,high) ((amt)<(low)?(low):((amt)>(high)?(high):(amt)))
+#define round(x) ((x)>=0?(long)((x)+0.5):(long)((x)-0.5))
+#define radians(deg) ((deg)*DEG_TO_RAD)
+#define degrees(rad) ((rad)*RAD_TO_DEG)
+#define sq(x) ((x)*(x))
+
+#define interrupts() sei()
+#define noInterrupts() cli()
+
+#define clockCyclesPerMicrosecond() ( F_CPU / 1000000L )
+#define clockCyclesToMicroseconds(a) ( (a) / clockCyclesPerMicrosecond() )
+#define microsecondsToClockCycles(a) ( (a) * clockCyclesPerMicrosecond() )
+
+#define lowByte(w) ((uint8_t) ((w) & 0xff))
+#define highByte(w) ((uint8_t) ((w) >> 8))
+
+#define bitRead(value, bit) (((value) >> (bit)) & 0x01)
+#define bitSet(value, bit) ((value) |= (1UL << (bit)))
+#define bitClear(value, bit) ((value) &= ~(1UL << (bit)))
+#define bitWrite(value, bit, bitvalue) (bitvalue ? bitSet(value, bit) : bitClear(value, bit))
+
+typedef unsigned int word;
+
+#define bit(b) (1UL << (b))
+
+//chni: typedef uint8_t boolean;
+typedef uint8_t byte;
+
+void init(void);
+
+void pinMode(uint8_t, uint8_t);
+void digitalWrite(uint8_t, uint8_t);
+int digitalRead(uint8_t);
+int analogRead(uint8_t);
+void analogReference(uint8_t mode);
+void analogWrite(uint8_t, int);
+
+void beginSerial(long);
+void serialWrite(unsigned char);
+int serialAvailable(void);
+int serialRead(void);
+void serialFlush(void);
+
+unsigned long millis(void);
+unsigned long micros(void);
+void delay(unsigned long);
+void delayMicroseconds(unsigned int us);
+unsigned long pulseIn(uint8_t pin, uint8_t state, unsigned long timeout);
+
+void shiftOut(uint8_t dataPin, uint8_t clockPin, uint8_t bitOrder, byte val);
+
+void attachInterrupt(uint8_t, void (*)(void), int mode);
+void detachInterrupt(uint8_t);
+
+void setup(void);
+void loop(void);
+
+#ifdef __cplusplus
+} // extern "C"
+#endif
+
+#endif