added a simple imu subsystem for usage with roll (and optionally pitc…

…h) gyros only. Meant to be used together with ahrs_infrared and replacing the old gyro subsystem and code

conf/autopilot/subsystems/fixedwing/imu_analog_gyro.makefile

@@ -0,0 +1,61 @@
+# Hey Emacs, this is a -*- makefile -*-
+
+#
+# Analog roll (and optionally pitch) gyros connected to MCU ADC ports
+#
+# To use a roll gyro only:
+# <subsystem name="imu" type="analog_gyro">
+#   <configure name="GYRO_P" value="ADC_3"/>
+# </subsystem>
+#
+# To use roll and pitch gyros:
+# <subsystem name="imu" type="analog_gyro">
+#   <configure name="GYRO_P" value="ADC_3"/>
+#   <configure name="GYRO_Q" value="ADC_4"/>
+# </subsystem>
+#
+#
+# required xml:
+# <section name="IMU" prefix="IMU_">
+#
+#    <define name="GYRO_P_NEUTRAL" value="512"/>
+#    <define name="GYRO_Q_NEUTRAL" value="512"/>
+#
+#    <define name="GYRO_P_SENS" value="0.017" integer="16"/>
+#    <define name="GYRO_Q_SENS" value="0.017" integer="16"/>
+#
+#    <define name="GYRO_P_SIGN" value="1" />
+#    <define name="GYRO_Q_SIGN" value="1" />
+#
+#  </section>
+#
+
+
+ifeq ($(ARCH), lpc21)
+
+imu_CFLAGS += -DIMU_TYPE_H=\"subsystems/imu/imu_analog_gyro.h\"  -DUSE_IMU
+
+imu_CFLAGS += -DADC -DADC_CHANNEL_GYRO_NB_SAMPLES=$(ADC_GYRO_NB_SAMPLES)
+
+ifneq ($(GYRO_P),)
+imu_CFLAGS += -DADC_CHANNEL_GYRO_P=$(GYRO_P) -DUSE_$(GYRO_P)
+endif
+
+ifneq ($(GYRO_Q),)
+imu_CFLAGS += -DADC_CHANNEL_GYRO_Q=$(GYRO_Q) -DUSE_$(GYRO_Q)
+endif
+
+#ifneq ($(GYRO_P_TEMP),)
+#imu_CFLAGS += -DADC_CHANNEL_GYRO_P_TEMP=$(GYRO_P_TEMP) -DUSE_$(GYRO_P_TEMP)
+#endif
+
+
+imu_srcs += $(SRC_SUBSYSTEMS)/imu.c
+imu_srcs += $(SRC_SUBSYSTEMS)/imu/imu_analog_gyro.c
+
+endif
+
+# Keep CFLAGS/Srcs for imu in separate expression so we can assign it to other targets
+# see: conf/autopilot/subsystems/lisa_passthrough/imu_b2_v1.1.makefile for example
+ap.CFLAGS += $(imu_CFLAGS)
+ap.srcs += $(imu_srcs)

sw/airborne/subsystems/ahrs/ahrs_infrared.c

@@ -22,6 +22,7 @@
 #include "subsystems/ahrs/ahrs_infrared.h"
 
 #include "subsystems/sensors/infrared.h"
+#include "subsystems/imu.h"
 #include "subsystems/gps.h"
 
 #include "estimator.h"
@@ -52,6 +53,12 @@ void ahrs_align(void) {
 }
 
 void ahrs_propagate(void) {
+#ifdef ADC_CHANNEL_GYRO_P
+  ahrs_float.body_rate.p = RATE_FLOAT_OF_BFP(imu.gyro.p);
+#endif
+#ifdef ADC_CHANNEL_GYRO_Q
+  ahrs_float.body_rate.q = RATE_FLOAT_OF_BFP(imu.gyro.q);
+#endif
 }
 
 void ahrs_update_accel(void) {

sw/airborne/subsystems/imu/imu_analog_gyro.c

@@ -0,0 +1,72 @@
+/*
+ * Copyright (C) 2011 The Paparazzi Team
+ *
+ * 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.
+ */
+
+#include BOARD_CONFIG
+#include "imu_analog_gyro.h"
+#include "mcu_periph/adc.h"
+
+
+volatile bool_t imu_analog_gyro_available;
+int imu_overrun;
+
+static struct adc_buf imu_gyro_adc_buf[NB_IMU_GYRO_ADC];
+
+//#if defined ADC_CHANNEL_GYRO_TEMP
+//static struct adc_buf buf_temp;
+//#endif
+
+void imu_impl_init(void) {
+
+  imu_analog_gyro_available = FALSE;
+  imu_overrun = 0;
+
+#ifdef ADC_CHANNEL_GYRO_P
+  adc_buf_channel(ADC_CHANNEL_GYRO_P, &imu_gyro_adc_buf[0], ADC_CHANNEL_GYRO_NB_SAMPLES);
+#endif
+#ifdef ADC_CHANNEL_GYRO_Q
+  adc_buf_channel(ADC_CHANNEL_GYRO_Q, &imu_gyro_adc_buf[1], ADC_CHANNEL_GYRO_NB_SAMPLES);
+#endif
+
+//#ifdef ADC_CHANNEL_GYRO_P_TEMP
+//  adc_buf_channel(ADC_CHANNEL_GYRO_P_TEMP, &buf_temp, ADC_CHANNEL_GYRO_NB_SAMPLES);
+//#endif
+
+}
+
+void imu_periodic(void) {
+  // Actual Nr of ADC measurements per channel per periodic loop
+  static int last_head = 0;
+
+  imu_overrun = imu_gyro_adc_buf[0].head - last_head;
+  if (imu_overrun < 0)
+    imu_overrun += ADC_CHANNEL_GYRO_NB_SAMPLES;
+  last_head = imu_gyro_adc_buf[0].head;
+
+  // Read All Measurements
+#ifdef ADC_CHANNEL_GYRO_P
+  imu.gyro_unscaled.p = imu_gyro_adc_buf[0].sum / ADC_CHANNEL_GYRO_NB_SAMPLES;
+#endif
+#ifdef ADC_CHANNEL_GYRO_Q
+  imu.gyro_unscaled.q = imu_gyro_adc_buf[1].sum / ADC_CHANNEL_GYRO_NB_SAMPLES;
+#endif
+
+  imu_analog_gyro_available = TRUE;
+}

sw/airborne/subsystems/imu/imu_analog_gyro.h

@@ -0,0 +1,74 @@
+/*
+ * Copyright (C) 2011 The Paparazzi Team
+ *
+ * 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.
+ */
+
+#ifndef IMU_ANALOG_GYRO_H
+#define IMU_ANALOG_GYRO_H
+
+
+
+#ifdef ADC_CHANNEL_GYRO_P
+#ifdef ADC_CHANNEL_GYRO_Q
+
+#define NB_IMU_GYRO_ADC 2
+#define IMU_GYRO_R_NEUTRAL 0
+#define ImuScaleGyro(_imu) {                                            \
+    _imu.gyro.p = ((_imu.gyro_unscaled.p - _imu.gyro_neutral.p)*IMU_GYRO_P_SIGN*IMU_GYRO_P_SENS_NUM)/IMU_GYRO_P_SENS_DEN; \
+    _imu.gyro.q = ((_imu.gyro_unscaled.q - _imu.gyro_neutral.q)*IMU_GYRO_Q_SIGN*IMU_GYRO_Q_SENS_NUM)/IMU_GYRO_Q_SENS_DEN; \
+  }
+
+#else  //only roll gyro
+
+#define NB_IMU_GYRO_ADC 1
+#define IMU_GYRO_Q_NEUTRAL 0
+#define IMU_GYRO_R_NEUTRAL 0
+#define ImuScaleGyro(_imu) {                                            \
+    _imu.gyro.p = ((_imu.gyro_unscaled.p - _imu.gyro_neutral.p)*IMU_GYRO_P_SIGN*IMU_GYRO_P_SENS_NUM)/IMU_GYRO_P_SENS_DEN; \
+  }
+
+#endif //ADC_CHANNEL_GYRO_Q
+#else //ADC_CHANNEL_GYRO_P
+#error You need to define at least the roll gyro ADC (GYRO_P) to use this subsystem.
+#endif //ADC_CHANNEL_GYRO_P
+
+// no accelerometers
+#define ImuScaleAccel(_imu) {}
+
+
+/*
+ * we include imh.h after the definitions of ImuScale so we can override the default handlers
+ */
+#include "subsystems/imu.h"
+
+
+extern volatile bool_t imu_analog_gyro_available;
+extern int imu_overrun;
+
+
+#define ImuEvent(_gyro_handler, _accel_handler, _mag_handler) {   \
+    if (imu_analog_gyro_available) {                         \
+      imu_analog_gyro_available = FALSE;                     \
+      _gyro_handler();                            \
+    }                                                   \
+  }
+
+
+
+#endif /* IMU_ANALOG_GYRO_H */