diff --git a/conf/airframes/jsbsim.xml b/conf/airframes/jsbsim.xml index 40a9910663..73e3f587d7 100644 --- a/conf/airframes/jsbsim.xml +++ b/conf/airframes/jsbsim.xml @@ -206,7 +206,7 @@ - + diff --git a/conf/airframes/microjet_example.xml b/conf/airframes/microjet_example.xml index 9a9f676cc3..e08c9195c7 100644 --- a/conf/airframes/microjet_example.xml +++ b/conf/airframes/microjet_example.xml @@ -188,7 +188,7 @@ - + diff --git a/conf/airframes/twinjet_example.xml b/conf/airframes/twinjet_example.xml index 36eff058d6..b85661fb78 100644 --- a/conf/airframes/twinjet_example.xml +++ b/conf/airframes/twinjet_example.xml @@ -22,7 +22,7 @@ - + diff --git a/conf/autopilot/subsystems/fixedwing/gyro_pitch.makefile b/conf/autopilot/subsystems/fixedwing/gyro_pitch.makefile index 4fd106ef8a..1e792fa408 100644 --- a/conf/autopilot/subsystems/fixedwing/gyro_pitch.makefile +++ b/conf/autopilot/subsystems/fixedwing/gyro_pitch.makefile @@ -1,8 +1,9 @@ $(info Error: Please replace with) -$(info ) +$(info ) $(info ) $(info ) $(info ) +$(info and replace the GYRO section with the appropriate IMU section $(info in your airframe file.) $(error The gyro_pitch subsystem has been removed) diff --git a/conf/autopilot/subsystems/fixedwing/gyro_roll.makefile b/conf/autopilot/subsystems/fixedwing/gyro_roll.makefile index 7697c522a8..359094a4d5 100644 --- a/conf/autopilot/subsystems/fixedwing/gyro_roll.makefile +++ b/conf/autopilot/subsystems/fixedwing/gyro_roll.makefile @@ -1,7 +1,8 @@ $(info Error: Please replace with) -$(info ) +$(info ) $(info ) $(info ) +$(info and replace the GYRO section with the appropriate IMU section $(info in your airframe file.) $(error The gyro_pitch subsystem has been removed) diff --git a/conf/autopilot/subsystems/fixedwing/imu_analog.makefile b/conf/autopilot/subsystems/fixedwing/imu_analog.makefile index 9cfc7e1d90..1bec3d08d3 100644 --- a/conf/autopilot/subsystems/fixedwing/imu_analog.makefile +++ b/conf/autopilot/subsystems/fixedwing/imu_analog.makefile @@ -3,6 +3,9 @@ # # Analog IMU connected to MCU ADC ports # +# Only add the configure and define lines for the sensors you actually use. +# E.g. to replace the old gyro_pitch subsystem only add GYRO_P and GYRO_Q +# # # # @@ -49,11 +52,36 @@ ifeq ($(ARCH), lpc21) imu_CFLAGS += -DIMU_TYPE_H=\"subsystems/imu/imu_analog.h\" -DUSE_IMU imu_CFLAGS += -DADC -imu_CFLAGS += -DUSE_$(GYRO_P) -DUSE_$(GYRO_Q) -DUSE_$(GYRO_R) -imu_CFLAGS += -DUSE_$(ACCEL_X) -DUSE_$(ACCEL_Y) -DUSE_$(ACCEL_Z) +imu_CFLAGS += -DADC_CHANNEL_GYRO_NB_SAMPLES=$(ADC_GYRO_NB_SAMPLES) -imu_CFLAGS += -DADC_CHANNEL_GYRO_P=$(GYRO_P) -DADC_CHANNEL_GYRO_Q=$(GYRO_Q) -DADC_CHANNEL_GYRO_R=$(GYRO_R) -imu_CFLAGS += -DADC_CHANNEL_ACCEL_X=$(ACCEL_X) -DADC_CHANNEL_ACCEL_Y=$(ACCEL_Y) -DADC_CHANNEL_ACCEL_Z=$(ACCEL_Z) +ifeq ($(ADC_ACCEL_NB_SAMPLES),) +ADC_ACCEL_NB_SAMPLES = $(ADC_GYRO_NB_SAMPLES) +endif +imu_CFLAGS += -DADC_CHANNEL_ACCEL_NB_SAMPLES=$(ADC_ACCEL_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_R),) +imu_CFLAGS += -DADC_CHANNEL_GYRO_R=$(GYRO_R) -DUSE_$(GYRO_R) +endif + +ifneq ($(ACCEL_X),) +imu_CFLAGS += -DADC_CHANNEL_ACCEL_X=$(ACCEL_X) -DUSE_$(ACCEL_X) +endif + +ifneq ($(ACCEL_Y),) +imu_CFLAGS += -DADC_CHANNEL_ACCEL_Y=$(ACCEL_Y) -DUSE_$(ACCEL_Y) +endif + +ifneq ($(ACCEL_Z),) +imu_CFLAGS += -DADC_CHANNEL_ACCEL_Z=$(ACCEL_Z) -DUSE_$(ACCEL_Z) +endif imu_srcs += $(SRC_SUBSYSTEMS)/imu.c imu_srcs += $(SRC_SUBSYSTEMS)/imu/imu_analog.c diff --git a/conf/autopilot/subsystems/fixedwing/imu_analog_gyro.makefile b/conf/autopilot/subsystems/fixedwing/imu_analog_gyro.makefile deleted file mode 100644 index 4514777982..0000000000 --- a/conf/autopilot/subsystems/fixedwing/imu_analog_gyro.makefile +++ /dev/null @@ -1,61 +0,0 @@ -# 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/imu/imu_analog.c b/sw/airborne/subsystems/imu/imu_analog.c index ba9f968d22..2bcb9caadb 100644 --- a/sw/airborne/subsystems/imu/imu_analog.c +++ b/sw/airborne/subsystems/imu/imu_analog.c @@ -21,7 +21,6 @@ #include "imu_analog.h" #include "mcu_periph/adc.h" -#include "mcu_periph/uart.h" volatile bool_t analog_imu_available; int imu_overrun; @@ -33,12 +32,24 @@ void imu_impl_init(void) { analog_imu_available = FALSE; imu_overrun = 0; +#ifdef ADC_CHANNEL_GYRO_P adc_buf_channel(ADC_CHANNEL_GYRO_P, &analog_imu_adc_buf[0], ADC_CHANNEL_GYRO_NB_SAMPLES); +#endif +#ifdef ADC_CHANNEL_GYRO_Q adc_buf_channel(ADC_CHANNEL_GYRO_Q, &analog_imu_adc_buf[1], ADC_CHANNEL_GYRO_NB_SAMPLES); +#endif +#ifdef ADC_CHANNEL_GYRO_R adc_buf_channel(ADC_CHANNEL_GYRO_R, &analog_imu_adc_buf[2], ADC_CHANNEL_GYRO_NB_SAMPLES); +#endif +#ifdef ADC_CHANNEL_ACCEL_X adc_buf_channel(ADC_CHANNEL_ACCEL_X, &analog_imu_adc_buf[3], ADC_CHANNEL_ACCEL_NB_SAMPLES); +#endif +#ifdef ADC_CHANNEL_ACCEL_Y adc_buf_channel(ADC_CHANNEL_ACCEL_Y, &analog_imu_adc_buf[4], ADC_CHANNEL_ACCEL_NB_SAMPLES); +#endif +#ifdef ADC_CHANNEL_ACCEL_Z adc_buf_channel(ADC_CHANNEL_ACCEL_Z, &analog_imu_adc_buf[5], ADC_CHANNEL_ACCEL_NB_SAMPLES); +#endif } @@ -52,12 +63,24 @@ void imu_periodic(void) { last_head = analog_imu_adc_buf[0].head; // Read All Measurements +#ifdef ADC_CHANNEL_GYRO_P imu.gyro_unscaled.p = analog_imu_adc_buf[0].sum / ADC_CHANNEL_GYRO_NB_SAMPLES; +#endif +#ifdef ADC_CHANNEL_GYRO_Q imu.gyro_unscaled.q = analog_imu_adc_buf[1].sum / ADC_CHANNEL_GYRO_NB_SAMPLES; +#endif +#ifdef ADC_CHANNEL_GYRO_R imu.gyro_unscaled.r = analog_imu_adc_buf[2].sum / ADC_CHANNEL_GYRO_NB_SAMPLES; +#endif +#ifdef ADC_CHANNEL_ACCEL_X imu.accel_unscaled.x = analog_imu_adc_buf[3].sum / ADC_CHANNEL_ACCEL_NB_SAMPLES; +#endif +#ifdef ADC_CHANNEL_ACCEL_Y imu.accel_unscaled.y = analog_imu_adc_buf[4].sum / ADC_CHANNEL_ACCEL_NB_SAMPLES; +#endif +#ifdef ADC_CHANNEL_ACCEL_Z imu.accel_unscaled.z = analog_imu_adc_buf[5].sum / ADC_CHANNEL_ACCEL_NB_SAMPLES; +#endif analog_imu_available = TRUE; } diff --git a/sw/airborne/subsystems/imu/imu_analog.h b/sw/airborne/subsystems/imu/imu_analog.h index c7f7270fbf..be9854f43b 100644 --- a/sw/airborne/subsystems/imu/imu_analog.h +++ b/sw/airborne/subsystems/imu/imu_analog.h @@ -22,10 +22,39 @@ #ifndef IMU_ANALOG_H #define IMU_ANALOG_H -#include "subsystems/imu.h" #define NB_ANALOG_IMU_ADC 6 +// if not all gyros are used, override the ImuScaleGyro handler +#if defined ADC_CHANNEL_GYRO_P && defined ADC_CHANNEL_GYRO_Q && ! defined ADC_CHANNEL_GYRO_R + +#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; \ + } + +#elif defined ADC_CHANNEL_GYRO_P && ! defined ADC_CHANNEL_GYRO_Q && ! defined ADC_CHANNEL_GYRO_R + +#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 + +// if we don't have any accelerometers, set an empty ImuScaleAccel handler +#if ! defined ADC_CHANNEL_ACCEL_X && ! defined ADC_CHANNEL_ACCEL_Z && ! defined ADC_CHANNEL_ACCEL_Z +#define ImuScaleAccel(_imu) {} +#endif + +/* + * we include imh.h after the definitions of ImuScale so we can override the default handlers + */ +#include "subsystems/imu.h" + + extern volatile bool_t analog_imu_available; extern int imu_overrun; diff --git a/sw/airborne/subsystems/imu/imu_analog_gyro.c b/sw/airborne/subsystems/imu/imu_analog_gyro.c deleted file mode 100644 index c501fd59d2..0000000000 --- a/sw/airborne/subsystems/imu/imu_analog_gyro.c +++ /dev/null @@ -1,72 +0,0 @@ -/* - * 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 deleted file mode 100644 index 35c1bf149c..0000000000 --- a/sw/airborne/subsystems/imu/imu_analog_gyro.h +++ /dev/null @@ -1,74 +0,0 @@ -/* - * 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 */