diff --git a/conf/autopilot/subsystems/fixedwing/imu_analog_gyro.makefile b/conf/autopilot/subsystems/fixedwing/imu_analog_gyro.makefile new file mode 100644 index 00000000000..45147779829 --- /dev/null +++ b/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: +# +# +# +# +# To use roll and pitch gyros: +# +# +# +# +# +# +# required xml: +#
+# +# +# +# +# +# +# +# +# +# +#
+# + + +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) diff --git a/sw/airborne/subsystems/ahrs/ahrs_infrared.c b/sw/airborne/subsystems/ahrs/ahrs_infrared.c index 48febeef860..506096c7c75 100644 --- a/sw/airborne/subsystems/ahrs/ahrs_infrared.c +++ b/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) { diff --git a/sw/airborne/subsystems/imu/imu_analog_gyro.c b/sw/airborne/subsystems/imu/imu_analog_gyro.c new file mode 100644 index 00000000000..c501fd59d2a --- /dev/null +++ b/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; +} diff --git a/sw/airborne/subsystems/imu/imu_analog_gyro.h b/sw/airborne/subsystems/imu/imu_analog_gyro.h new file mode 100644 index 00000000000..35c1bf149c6 --- /dev/null +++ b/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 */