diff --git a/conf/airframes/ENAC/fixed-wing/apogee.xml b/conf/airframes/ENAC/fixed-wing/apogee.xml index 80a729e9e3..f5a379da1a 100644 --- a/conf/airframes/ENAC/fixed-wing/apogee.xml +++ b/conf/airframes/ENAC/fixed-wing/apogee.xml @@ -28,7 +28,7 @@ - + diff --git a/conf/firmwares/subsystems/shared/imu_apogee.makefile b/conf/firmwares/subsystems/shared/imu_apogee.makefile new file mode 100644 index 0000000000..36d120740c --- /dev/null +++ b/conf/firmwares/subsystems/shared/imu_apogee.makefile @@ -0,0 +1,27 @@ +# Hey Emacs, this is a -*- makefile -*- +# +# Apogee IMU +# + +IMU_APOGEE_CFLAGS = -DUSE_IMU +IMU_APOGEE_CFLAGS += -DIMU_TYPE_H=\"boards/apogee/imu_apogee.h\" + +IMU_APOGEE_SRCS = $(SRC_SUBSYSTEMS)/imu.c \ + $(SRC_BOARD)/imu_apogee.c + +IMU_APOGEE_I2C_DEV=i2c1 +IMU_APOGEE_CFLAGS += -DUSE_I2C -DUSE_I2C1 + +IMU_APOGEE_CFLAGS += -DIMU_APOGEE_I2C_DEV=$(IMU_APOGEE_I2C_DEV) +IMU_APOGEE_SRCS += peripherals/mpu60x0_i2c.c + +# with default APOGEE_SMPLRT_DIV (gyro output 100Hz) +# the AHRS_PROPAGATE_FREQUENCY needs to be adjusted accordingly +AHRS_PROPAGATE_FREQUENCY ?= 100 +AHRS_CORRECT_FREQUENCY ?= 100 +ap.CFLAGS += -DAHRS_PROPAGATE_FREQUENCY=$(AHRS_PROPAGATE_FREQUENCY) +ap.CFLAGS += -DAHRS_CORRECT_FREQUENCY=$(AHRS_CORRECT_FREQUENCY) + +ap.CFLAGS += $(IMU_APOGEE_CFLAGS) +ap.srcs += $(IMU_APOGEE_SRCS) + diff --git a/sw/airborne/boards/apogee/imu_apogee.c b/sw/airborne/boards/apogee/imu_apogee.c new file mode 100644 index 0000000000..c21e403260 --- /dev/null +++ b/sw/airborne/boards/apogee/imu_apogee.c @@ -0,0 +1,106 @@ +/* + * Copyright (C) 2013 Gautier Hattenberger + * + * 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 boards/apogee/imu_apogee.c + * + * Driver for the IMU on the Apogee board. + * + * Invensense MPU-6050 + */ + +#include +#include "boards/apogee/imu_apogee.h" +#include "mcu_periph/i2c.h" +#include "led.h" + +// Downlink +#include "mcu_periph/uart.h" +#include "messages.h" +#include "subsystems/datalink/downlink.h" + +#ifndef DOWNLINK_DEVICE +#define DOWNLINK_DEVICE DOWNLINK_AP_DEVICE +#endif + +#if !defined APOGEE_LOWPASS_FILTER && !defined APOGEE_SMPLRT_DIV +#define APOGEE_LOWPASS_FILTER MPU60X0_DLPF_42HZ +#define APOGEE_SMPLRT_DIV 9 +PRINT_CONFIG_MSG("Gyro/Accel output rate is 100Hz") +#endif +PRINT_CONFIG_VAR(APOGEE_SMPLRT_DIV) +PRINT_CONFIG_VAR(APOGEE_LOWPASS_FILTER) + +#ifndef APOGEE_GYRO_RANGE +#define APOGEE_GYRO_RANGE MPU60X0_GYRO_RANGE_1000 +#endif +PRINT_CONFIG_VAR(APOGEE_GYRO_RANGE) + +#ifndef APOGEE_ACCEL_RANGE +#define APOGEE_ACCEL_RANGE MPU60X0_ACCEL_RANGE_4G +#endif +PRINT_CONFIG_VAR(APOGEE_ACCEL_RANGE) + +struct ImuApogee imu_apogee; + +void imu_impl_init(void) +{ + ///////////////////////////////////////////////////////////////////// + // MPU-60X0 + mpu60x0_i2c_init(&imu_apogee.mpu, &(IMU_APOGEE_I2C_DEV), MPU60X0_ADDR_ALT); + // change the default configuration + imu_apogee.mpu.smplrt_div = APOGEE_SMPLRT_DIV; + imu_apogee.mpu.dlpf_cfg = APOGEE_LOWPASS_FILTER; + imu_apogee.mpu.config.gyro_range = APOGEE_GYRO_RANGE; + imu_apogee.mpu.config.accel_range = APOGEE_ACCEL_RANGE; + + imu_apogee.gyr_valid = FALSE; + imu_apogee.acc_valid = FALSE; +} + +void imu_periodic( void ) +{ + // Start reading the latest gyroscope data + mpu60x0_i2c_periodic(&imu_apogee.imu); + + //RunOnceEvery(10,imu_apogee_downlink_raw()); +} + +void imu_apogee_downlink_raw( void ) +{ + DOWNLINK_SEND_IMU_GYRO_RAW(DefaultChannel, DefaultDevice,&imu.gyro_unscaled.p,&imu.gyro_unscaled.q,&imu.gyro_unscaled.r); + DOWNLINK_SEND_IMU_ACCEL_RAW(DefaultChannel, DefaultDevice,&imu.accel_unscaled.x,&imu.accel_unscaled.y,&imu.accel_unscaled.z); +} + + +void imu_umarim_event( void ) +{ + // If the itg3200 I2C transaction has succeeded: convert the data + mpu60x0_i2c_event(&imu_apogee.mpu); + if (imu_apogee.mpu.data_available) { + RATES_COPY(imu.gyro_unscaled, imu_apogee.mpu.data_rates.rates); + VECT3_COPY(imu.accel_unscaled, imu_apogee.mpu.data_accel.vect); + imu_apogee.mpu.data_available = FALSE; + imu_apogee.gyr_valid = TRUE; + imu_apogee.acc_valid = TRUE; + } +} + diff --git a/sw/airborne/boards/apogee/imu_apogee.h b/sw/airborne/boards/apogee/imu_apogee.h new file mode 100644 index 0000000000..9dbf1fa3d3 --- /dev/null +++ b/sw/airborne/boards/apogee/imu_apogee.h @@ -0,0 +1,128 @@ +/* + * Copyright (C) 2011 Gautier Hattenberger + * + * 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 boards/apogee/imu_apogee.h + * + * Driver for the IMU on the Apogee board. + * + * Invensense MPU-6050 + */ + +#ifndef IMU_APOGEE_H +#define IMU_APOGEE_H + +#include "std.h" +#include "generated/airframe.h" +#include "subsystems/imu.h" + +#include "peripherals/mpu60x0_i2c.h" + +// Default configuration +#if !defined IMU_GYRO_P_SIGN & !defined IMU_GYRO_Q_SIGN & !defined IMU_GYRO_R_SIGN +#define IMU_GYRO_P_SIGN 1 +#define IMU_GYRO_Q_SIGN 1 +#define IMU_GYRO_R_SIGN 1 +#endif +#if !defined IMU_ACCEL_X_SIGN & !defined IMU_ACCEL_Y_SIGN & !defined IMU_ACCEL_Z_SIGN +#define IMU_ACCEL_X_SIGN 1 +#define IMU_ACCEL_Y_SIGN 1 +#define IMU_ACCEL_Z_SIGN 1 +#endif + +/** default gyro sensitivy and neutral from the datasheet + * MPU with 1000 deg/s has 32.8 LSB/(deg/s) + * sens = 1/32.8 * pi/180 * 2^INT32_RATE_FRAC + * sens = 1/32.8 * pi/180 * 4096 = 2.17953 + I*/ +#if !defined IMU_GYRO_P_SENS & !defined IMU_GYRO_Q_SENS & !defined IMU_GYRO_R_SENS +// FIXME +#define IMU_GYRO_P_SENS 4.973 +#define IMU_GYRO_P_SENS_NUM 4973 +#define IMU_GYRO_P_SENS_DEN 1000 +#define IMU_GYRO_Q_SENS 4.973 +#define IMU_GYRO_Q_SENS_NUM 4973 +#define IMU_GYRO_Q_SENS_DEN 1000 +#define IMU_GYRO_R_SENS 4.973 +#define IMU_GYRO_R_SENS_NUM 4973 +#define IMU_GYRO_R_SENS_DEN 1000 +#endif +#if !defined IMU_GYRO_P_NEUTRAL & !defined IMU_GYRO_Q_NEUTRAL & !defined IMU_GYRO_R_NEUTRAL +#define IMU_GYRO_P_NEUTRAL 0 +#define IMU_GYRO_Q_NEUTRAL 0 +#define IMU_GYRO_R_NEUTRAL 0 +#endif + + +/** default accel sensitivy from the datasheet + * MPU with 8g has 4.096 LSB/g + * sens = 9.81 [m/s^2] / 4.096 [LSB/g] * 2^INT32_ACCEL_FRAC = 306.263 + */ +#if !defined IMU_ACCEL_X_SENS & !defined IMU_ACCEL_Y_SENS & !defined IMU_ACCEL_Z_SENS +// FIXME +#define IMU_ACCEL_X_SENS 37.91 +#define IMU_ACCEL_X_SENS_NUM 3791 +#define IMU_ACCEL_X_SENS_DEN 100 +#define IMU_ACCEL_Y_SENS 37.91 +#define IMU_ACCEL_Y_SENS_NUM 3791 +#define IMU_ACCEL_Y_SENS_DEN 100 +#define IMU_ACCEL_Z_SENS 39.24 +#define IMU_ACCEL_Z_SENS_NUM 39.24 +#define IMU_ACCEL_Z_SENS_DEN 100 +#endif +#if !defined IMU_ACCEL_X_NEUTRAL & !defined IMU_ACCEL_Y_NEUTRAL & !defined IMU_ACCEL_Z_NEUTRAL +#define IMU_ACCEL_X_NEUTRAL 0 +#define IMU_ACCEL_Y_NEUTRAL 0 +#define IMU_ACCEL_Z_NEUTRAL 0 +#endif + +struct ImuApogee { + volatile bool_t gyr_valid; + volatile bool_t acc_valid; + struct Mpu60x0_I2c mpu; +}; + +extern struct ImuApogee imu_apogee; + + +/* must be defined in order to be IMU code: declared in imu.h +extern void imu_impl_init(void); +extern void imu_periodic(void); +*/ + +/* Own Extra Functions */ +extern void imu_apogee_event( void ); +extern void imu_apogee_downlink_raw( void ); + + +static inline void ImuEvent(void (* _gyro_handler)(void), void (* _accel_handler)(void), void (* _mag_handler)(void) __attribute__((unused))) { + imu_apogee_event(); + if (imu_apogee.gyr_valid) { + imu_apogee.gyr_valid = FALSE; + _gyro_handler(); + } + if (imu_apogee.acc_valid) { + imu_apogee.acc_valid = FALSE; + _accel_handler(); + } +} + +#endif // IMU_APOGEE_H diff --git a/sw/airborne/boards/apogee_0.99.h b/sw/airborne/boards/apogee_0.99.h index 50f5fb237f..b3035292fd 100644 --- a/sw/airborne/boards/apogee_0.99.h +++ b/sw/airborne/boards/apogee_0.99.h @@ -50,37 +50,61 @@ /* Onboard ADCs */ -/* - ADC1 PB0/?ADC13 - ADC2 PB1/?ADC10 - ADC3 PB15/?ADC11 - ADC4 PC4/?ADC15 - BATT PA4/ADC4 -*/ -#define BOARD_ADC_CHANNEL_1 12 -#define BOARD_ADC_CHANNEL_2 10 -#define BOARD_ADC_CHANNEL_3 11 -#define BOARD_ADC_CHANNEL_4 13 //15 -#define BOARD_ADC_CHANNEL_5 14 -// we can only use ADC1,2,3; the last channel is for bat monitoring -#define BOARD_ADC_CHANNEL_6 15 //13 +#define USE_AD_TIM1 1 + +#define BOARD_ADC_CHANNEL_1 8 +#define BOARD_ADC_CHANNEL_2 9 +#define BOARD_ADC_CHANNEL_3 14 +#define BOARD_ADC_CHANNEL_4 4 /* provide defines that can be used to access the ADC_x in the code or airframe file * these directly map to the index number of the 4 adc channels defined above * 4th (index 3) is used for bat monitoring by default */ #define ADC_1 0 +#ifdef USE_ADC_1 +#ifndef ADC_1_GPIO_PORT +#define ADC_1_GPIO_PORT RCC_AHB1ENR_IOPBEN +#endif +#else +#define ADC_1_GPIO_PORT 0 +#endif + #define ADC_2 1 +#ifdef USE_ADC_2 +#ifndef ADC_2_GPIO_PORT +#define ADC_2_GPIO_PORT RCC_AHB1ENR_IOPBEN +#endif +#else +#define ADC_2_GPIO_PORT 0 +#endif + #define ADC_3 2 +#ifdef USE_ADC_3 +#ifndef ADC_3_GPIO_PORT +#define ADC_3_GPIO_PORT RCC_AHB1ENR_IOPCEN +#endif +#else +#define ADC_3_GPIO_PORT 0 +#endif + #define ADC_4 3 -#define ADC_5 4 -#define ADC_6 5 +#ifdef USE_ADC_4 +#ifndef ADC_4_GPIO_PORT +#define ADC_4_GPIO_PORT RCC_AHB1ENR_IOPAEN +#endif +#else +#define ADC_4_GPIO_PORT 0 +#endif + +#define ADC_GPIO_PORT (ADC_1_GPIO_PORT | ADC_2_GPIO_PORT | ADC_3_GPIO_PORT | ADC_4_GPIO_PORT) /* allow to define ADC_CHANNEL_VSUPPLY in the airframe file*/ #ifndef ADC_CHANNEL_VSUPPLY #define ADC_CHANNEL_VSUPPLY ADC_4 #endif + /* I2C mapping (really at this place ?) */ #define GPIO_I2C1_SCL GPIO8 #define GPIO_I2C1_SDA GPIO7 @@ -88,6 +112,32 @@ #define GPIO_I2C2_SDA GPIO11 +/* SPI slave pin declaration */ +//#define SPI_SELECT_SLAVE0_PERIPH RCC_APB2ENR_IOPAEN +//#define SPI_SELECT_SLAVE0_PORT GPIOA +//#define SPI_SELECT_SLAVE0_PIN GPIO15 +// +//#define SPI_SELECT_SLAVE1_PERIPH RCC_APB2ENR_IOPAEN +//#define SPI_SELECT_SLAVE1_PORT GPIOA +//#define SPI_SELECT_SLAVE1_PIN GPIO4 + +#define SPI_SELECT_SLAVE2_PERIPH RCC_AHB1ENR_IOPBEN +#define SPI_SELECT_SLAVE2_PORT GPIOB +#define SPI_SELECT_SLAVE2_PIN GPIO12 + +//#define SPI_SELECT_SLAVE3_PERIPH RCC_APB2ENR_IOPCEN +//#define SPI_SELECT_SLAVE3_PORT GPIOC +//#define SPI_SELECT_SLAVE3_PIN GPIO13 +// +//#define SPI_SELECT_SLAVE4_PERIPH RCC_APB2ENR_IOPCEN +//#define SPI_SELECT_SLAVE4_PORT GPIOC +//#define SPI_SELECT_SLAVE4_PIN GPIO12 +// +//#define SPI_SELECT_SLAVE5_PERIPH RCC_APB2ENR_IOPCEN +//#define SPI_SELECT_SLAVE5_PORT GPIOC +//#define SPI_SELECT_SLAVE5_PIN GPIO4 + + #define BOARD_HAS_BARO 1 #define PWM_1_4_TIMER TIM3 diff --git a/sw/airborne/peripherals/mpu60x0.h b/sw/airborne/peripherals/mpu60x0.h index 2647aee7f0..6f68877b53 100644 --- a/sw/airborne/peripherals/mpu60x0.h +++ b/sw/airborne/peripherals/mpu60x0.h @@ -61,8 +61,8 @@ enum Mpu60x0ConfStatus { struct Mpu60x0Config { uint8_t smplrt_div; ///< Sample rate divider enum Mpu60x0DLPF dlpf_cfg; ///< Digital Low Pass Filter - enum Adxl345GyroRanges gyro_range; ///< deg/s Range - enum Adxl345AccelRanges accel_range; ///< g Range + enum Mpu60x0GyroRanges gyro_range; ///< deg/s Range + enum Mpu60x0AccelRanges accel_range; ///< g Range bool_t i2c_bypass; ///< bypass mpu i2c bool_t drdy_int_enable; ///< Enable Data Ready Interrupt uint8_t clk_sel; ///< Clock select @@ -113,7 +113,7 @@ static inline void mpu60x0_send_config(Mpu60x0ConfigSet mpu_set, void* mpu, stru config->init_status++; break; case MPU60X0_CONF_PWR: - mpu_set(mpu, MPU60X0_REG_PWR_MGMT_1, ((config->config.clk_sel)|(0<<6)); + mpu_set(mpu, MPU60X0_REG_PWR_MGMT_1, ((config->clk_sel)|(0<<6))); config->init_status++; break; case MPU60X0_CONF_DONE: