[apogee] add apogee imu based on mpu60x0

conf/airframes/ENAC/fixed-wing/apogee.xml

@@ -28,7 +28,7 @@
     <subsystem name="telemetry" type="transparent"/>
 
     <!-- Actuators are automatically chosen according to board-->
-    <subsystem name="imu"     type="aspirin_v1.0"/>
+    <subsystem name="imu"     type="apogee"/>
     <subsystem name="ahrs"    type="float_dcm">
       <define name="USE_HIGH_ACCEL_FLAG"/>
     </subsystem>

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)
+

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 <math.h>
+#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;
+  }
+}
+

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

sw/airborne/boards/apogee_0.99.h

@@ -50,37 +50,61 @@
 
 
 /* Onboard ADCs */
-/*
+#define USE_AD_TIM1 1
-   ADC1 PB0/?ADC13
+
-   ADC2 PB1/?ADC10
+#define BOARD_ADC_CHANNEL_1 8
-   ADC3 PB15/?ADC11
+#define BOARD_ADC_CHANNEL_2 9
-   ADC4 PC4/?ADC15
+#define BOARD_ADC_CHANNEL_3 14
-   BATT PA4/ADC4
+#define BOARD_ADC_CHANNEL_4 4
-*/
-#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
 
 /* 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
+#ifdef USE_ADC_4
-#define ADC_6 5
+#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

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 Mpu60x0GyroRanges gyro_range;    ///< deg/s Range
-  enum Adxl345AccelRanges accel_range;  ///< g 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: