[krooz] add imu driver for KroozSD board from Sergei

conf/firmwares/rotorcraft.makefile

@@ -201,6 +201,10 @@ ap.CFLAGS += -DUSE_SPI1
 ap.srcs += peripherals/mcp355x.c
 ap.srcs += $(SRC_BOARD)/baro_board.c
 
+# krooz baro
+else ifeq ($(BOARD), krooz)
+ap.srcs += $(SRC_BOARD)/baro_board.c
+
 # apogee baro
 else ifeq ($(BOARD), apogee)
 ap.CFLAGS += -DUSE_I2C1

conf/firmwares/subsystems/shared/imu_krooz_sd.makefile

@@ -0,0 +1,29 @@
+# Hey Emacs, this is a -*- makefile -*-
+#
+# KroozSD IMU
+#
+
+IMU_KROOZ_CFLAGS  = -DUSE_IMU
+IMU_KROOZ_CFLAGS += -DIMU_TYPE_H=\"boards/krooz/imu_krooz.h\"
+
+IMU_KROOZ_SRCS    = $(SRC_SUBSYSTEMS)/imu.c             \
+										 $(SRC_BOARD)/imu_krooz.c             \
+										 $(SRC_ARCH)/subsystems/imu/imu_krooz_sd_arch.c
+
+IMU_KROOZ_I2C_DEV=i2c2
+IMU_KROOZ_CFLAGS += -DUSE_I2C -DUSE_I2C2 -DI2C2_CLOCK_SPEED=400000
+
+IMU_KROOZ_CFLAGS += -DIMU_KROOZ_I2C_DEV=$(IMU_KROOZ_I2C_DEV)
+IMU_KROOZ_CFLAGS += -DMS5611_I2C_DEV=$(IMU_KROOZ_I2C_DEV) -DMS5611_SLAVE_ADDR=0xEC
+IMU_KROOZ_SRCS += peripherals/mpu60x0.c
+IMU_KROOZ_SRCS += peripherals/mpu60x0_i2c.c
+IMU_KROOZ_SRCS += peripherals/hmc58xx.c
+IMU_KROOZ_SRCS += modules/sensors/baro_ms5611_i2c.c
+
+AHRS_PROPAGATE_FREQUENCY ?= 512
+AHRS_CORRECT_FREQUENCY ?= 512
+ap.CFLAGS += -DAHRS_PROPAGATE_FREQUENCY=$(AHRS_PROPAGATE_FREQUENCY)
+ap.CFLAGS += -DAHRS_CORRECT_FREQUENCY=$(AHRS_CORRECT_FREQUENCY)
+
+ap.CFLAGS += $(IMU_KROOZ_CFLAGS)
+ap.srcs   += $(IMU_KROOZ_SRCS)

sw/airborne/arch/stm32/subsystems/imu/imu_krooz_sd_arch.c

@@ -0,0 +1,37 @@
+#include "subsystems/imu.h"
+
+#include <libopencm3/stm32/f4/rcc.h>
+#include <libopencm3/stm32/f4/gpio.h>
+#include <libopencm3/stm32/f4/nvic.h>
+#include <libopencm3/stm32/exti.h>
+
+#include "subsystems/imu/imu_krooz_sd_arch.h"
+
+void imu_krooz_sd_arch_init(void) {
+  rcc_peripheral_enable_clock(&RCC_APB2ENR, RCC_APB2ENR_SYSCFGEN);
+  rcc_peripheral_enable_clock(&RCC_AHB1ENR, RCC_AHB1ENR_IOPBEN);
+  rcc_peripheral_enable_clock(&RCC_AHB1ENR, RCC_AHB1ENR_IOPCEN);
+  gpio_mode_setup(GPIOB, GPIO_MODE_INPUT, GPIO_PUPD_NONE, GPIO5);
+  gpio_mode_setup(GPIOC, GPIO_MODE_INPUT, GPIO_PUPD_NONE, GPIO6);
+
+  nvic_enable_irq(NVIC_EXTI9_5_IRQ);
+  exti_select_source(EXTI5, GPIOB);
+  exti_select_source(EXTI6, GPIOC);
+  exti_set_trigger(EXTI5, EXTI_TRIGGER_RISING);
+  exti_set_trigger(EXTI6, EXTI_TRIGGER_FALLING);
+  exti_enable_request(EXTI5);
+  exti_enable_request(EXTI6);
+  nvic_set_priority(NVIC_EXTI9_5_IRQ, 0x0F);
+}
+
+void exti9_5_isr(void) {
+  /* clear EXTI */
+  if(EXTI_PR & EXTI6) {
+    exti_reset_request(EXTI6);
+    hmc58xx_read(&imu_krooz.hmc);
+  }
+  if(EXTI_PR & EXTI5) {
+    exti_reset_request(EXTI5);
+    mpu60x0_i2c_read(&imu_krooz.mpu);
+  }
+}

sw/airborne/arch/stm32/subsystems/imu/imu_krooz_sd_arch.h

@@ -0,0 +1,8 @@
+#ifndef IMU_KROOZ_SD_ARCH_H
+#define IMU_KROOZ_SD_ARCH_H
+
+#include "subsystems/imu.h"
+
+void imu_krooz_sd_arch_init(void);
+
+#endif /* IMU_KROOZ_SD_ARCH_H */

sw/airborne/boards/krooz/baro_board.c

@@ -0,0 +1,32 @@
+
+#include "subsystems/sensors/baro.h"
+#include "baro_board.h"
+/*
+#include "subsystems/datalink/downlink.h"
+#include "mcu_periph/uart.h"
+#include "mcu_periph/sys_time.h"
+*/
+
+struct Baro baro;
+
+void baro_init(void) {
+  baro_ms5611_init();
+}
+
+void baro_periodic(void) {
+    static uint8_t cnt;
+    switch(cnt) {
+      case 0:
+      baro_ms5611_periodic();
+      cnt++;
+      break;
+      case 1:
+      baro_ms5611_d1();
+      cnt++;
+      break;
+      case 2:
+      baro_ms5611_d2();
+      cnt = 0;
+      break;
+    }
+}

sw/airborne/boards/krooz/baro_board.h

@@ -0,0 +1,30 @@
+
+/*
+ * board specific fonctions for the KroozSD board
+ *
+ */
+
+#ifndef BOARDS_KROOZ_BARO_H
+#define BOARDS_KROOZ_BARO_H
+
+#include "std.h"
+#include "mcu_periph/i2c.h"
+#include "modules/sensors/baro_ms5611_i2c.h"
+#include "math/pprz_algebra_int.h"
+
+//#include "led.h"
+
+static inline void baro_event(void (*b_abs_handler)(void), void (*b_diff_handler)(void))
+{
+  baro_ms5611_event();
+  if(baro_ms5611_valid) {
+    baro.status = BS_RUNNING;
+    baro.absolute = (int32_t)baroms;
+    b_abs_handler();
+    baro_ms5611_valid = FALSE;
+  }
+}
+
+#define BaroEvent(_b_abs_handler, _b_diff_handler) baro_event(_b_abs_handler,_b_diff_handler)
+
+#endif /* BOARDS_KROOZ_SD_BARO_H */

sw/airborne/boards/krooz/imu_krooz.c

@@ -0,0 +1,160 @@
+/*
+ * Copyright (C) 2013 Sergey Krukowski <softsr@yahoo.de>
+ *
+ * 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/krooz/imu_krooz.c
+ *
+ * Driver for the IMU on the KroozSD board.
+ *
+ * Invensense MPU-6050
+ * Honeywell HMC-5883
+ */
+
+#include <math.h>
+#include "boards/krooz/imu_krooz.h"
+#include "subsystems/imu/imu_krooz_sd_arch.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 KROOZ_LOWPASS_FILTER && !defined  KROOZ_SMPLRT_DIV
+#define KROOZ_LOWPASS_FILTER MPU60X0_DLPF_256HZ
+#define KROOZ_SMPLRT_DIV 1
+#endif
+PRINT_CONFIG_VAR(KROOZ_SMPLRT_DIV)
+PRINT_CONFIG_VAR(KROOZ_LOWPASS_FILTER)
+
+#ifndef KROOZ_GYRO_RANGE
+#define KROOZ_GYRO_RANGE MPU60X0_GYRO_RANGE_250
+#endif
+PRINT_CONFIG_VAR(KROOZ_GYRO_RANGE)
+
+#ifndef KROOZ_ACCEL_RANGE
+#define KROOZ_ACCEL_RANGE MPU60X0_ACCEL_RANGE_2G
+#endif
+PRINT_CONFIG_VAR(KROOZ_ACCEL_RANGE)
+
+struct ImuKrooz imu_krooz;
+
+
+#if KROOZ_USE_MEDIAN_FILTER
+#include "filters/median_filter.h"
+struct MedianFilter3Int median_gyro, median_accel, median_mag;
+#endif
+
+void imu_impl_init( void )
+{
+  /////////////////////////////////////////////////////////////////////
+  // MPU-60X0
+  mpu60x0_i2c_init(&imu_krooz.mpu, &(IMU_KROOZ_I2C_DEV), MPU60X0_ADDR);
+  // change the default configuration
+  imu_krooz.mpu.config.smplrt_div = KROOZ_SMPLRT_DIV;
+  imu_krooz.mpu.config.dlpf_cfg = KROOZ_LOWPASS_FILTER;
+  imu_krooz.mpu.config.gyro_range = KROOZ_GYRO_RANGE;
+  imu_krooz.mpu.config.accel_range = KROOZ_ACCEL_RANGE;
+  imu_krooz.mpu.config.drdy_int_enable = TRUE;
+
+  hmc58xx_init(&imu_krooz.hmc, &(IMU_KROOZ_I2C_DEV), HMC58XX_ADDR);
+
+#if KROOZ_USE_MEDIAN_FILTER
+  // Init median filters
+  InitMedianFilterRatesInt(median_gyro);
+  InitMedianFilterVect3Int(median_accel);
+  InitMedianFilterVect3Int(median_mag);
+#endif
+
+  RATES_ASSIGN(imu_krooz.rates_sum, 0, 0, 0);
+  VECT3_ASSIGN(imu_krooz.accel_sum, 0, 0, 0);
+  imu_krooz.meas_nb = 0;
+
+  imu_krooz.gyr_valid = FALSE;
+  imu_krooz.acc_valid = FALSE;
+  imu_krooz.mag_valid = FALSE;
+
+  imu_krooz_sd_arch_init();
+}
+
+void imu_periodic( void )
+{
+  // Start reading the latest gyroscope data
+  if (!imu_krooz.mpu.config.initialized)
+    mpu60x0_i2c_start_configure(&imu_krooz.mpu);
+
+  if (!imu_krooz.hmc.initialized)
+    hmc58xx_start_configure(&imu_krooz.hmc);
+
+  if (imu_krooz.meas_nb) {
+    RATES_ASSIGN(imu.gyro_unscaled, imu_krooz.rates_sum.q / imu_krooz.meas_nb, imu_krooz.rates_sum.p / imu_krooz.meas_nb, imu_krooz.rates_sum.r / imu_krooz.meas_nb);
+#if KROOZ_USE_MEDIAN_FILTER
+    UpdateMedianFilterRatesInt(median_gyro, imu.gyro_unscaled);
+#endif
+    VECT3_ASSIGN(imu.accel_unscaled, imu_krooz.accel_sum.y / imu_krooz.meas_nb, imu_krooz.accel_sum.x / imu_krooz.meas_nb, imu_krooz.accel_sum.z / imu_krooz.meas_nb);
+#if KROOZ_USE_MEDIAN_FILTER
+    UpdateMedianFilterVect3Int(median_accel, imu.accel_unscaled);
+#endif
+    RATES_ASSIGN(imu_krooz.rates_sum, 0, 0, 0);
+    VECT3_ASSIGN(imu_krooz.accel_sum, 0, 0, 0);
+    imu_krooz.meas_nb = 0;
+
+    imu_krooz.gyr_valid = TRUE;
+    imu_krooz.acc_valid = TRUE;
+  }
+
+  //RunOnceEvery(10,imu_krooz_downlink_raw());
+}
+
+void imu_krooz_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_krooz_event( void )
+{
+  // If the MPU6050 I2C transaction has succeeded: convert the data
+  mpu60x0_i2c_event(&imu_krooz.mpu);
+  if (imu_krooz.mpu.data_available) {
+    RATES_ADD(imu_krooz.rates_sum, imu_krooz.mpu.data_rates.rates);
+    VECT3_ADD(imu_krooz.accel_sum, imu_krooz.mpu.data_accel.vect);
+    imu_krooz.meas_nb++;
+    imu_krooz.mpu.data_available = FALSE;
+  }
+
+  // If the HMC5883 I2C transaction has succeeded: convert the data
+  hmc58xx_event(&imu_krooz.hmc);
+  if (imu_krooz.hmc.data_available) {
+    VECT3_COPY(imu.mag_unscaled, imu_krooz.hmc.data.vect);
+#if KROOZ_USE_MEDIAN_FILTER
+    UpdateMedianFilterVect3Int(median_mag, imu.mag_unscaled);
+#endif
+    imu_krooz.hmc.data_available = FALSE;
+    imu_krooz.mag_valid = TRUE;
+  }
+}

sw/airborne/boards/krooz/imu_krooz.h

@@ -0,0 +1,142 @@
+/*
+ * Copyright (C) 2013 Sergey Krukowski <softsr@yahoo.de>
+ *
+ * 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/krooz/imu_krooz.h
+ *
+ * Driver for the IMU on the KroozSD board.
+ *
+ * Invensense MPU-6050
+ */
+
+#ifndef IMU_KROOZ_H
+#define IMU_KROOZ_H
+
+#include "std.h"
+#include "generated/airframe.h"
+#include "subsystems/imu.h"
+
+#include "peripherals/mpu60x0_i2c.h"
+#include "peripherals/hmc58xx.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
+#if !defined IMU_MAG_X_SIGN & !defined IMU_MAG_Y_SIGN & !defined IMU_MAG_Z_SIGN
+#define IMU_MAG_X_SIGN    1
+#define IMU_MAG_Y_SIGN    1
+#define IMU_MAG_Z_SIGN    1
+#endif
+
+/** default gyro sensitivy and neutral from the datasheet
+ * MPU with 250 deg/s has 131.072 LSB/(deg/s)
+ * sens = 1/131.072 * pi/180 * 2^INT32_RATE_FRAC
+ * sens = 1/131.072 * pi/180 * 4096 = 0.5454
+ I*/
+#if !defined IMU_GYRO_P_SENS & !defined IMU_GYRO_Q_SENS & !defined IMU_GYRO_R_SENS
+// FIXME
+#define IMU_GYRO_P_SENS 0.5454
+#define IMU_GYRO_P_SENS_NUM 2727
+#define IMU_GYRO_P_SENS_DEN 5000
+#define IMU_GYRO_Q_SENS 0.5454
+#define IMU_GYRO_Q_SENS_NUM 2727
+#define IMU_GYRO_Q_SENS_DEN 5000
+#define IMU_GYRO_R_SENS 0.5454
+#define IMU_GYRO_R_SENS_NUM 2727
+#define IMU_GYRO_R_SENS_DEN 5000
+#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 2g has 16384 LSB/g
+ * sens = 9.81 [m/s^2] / 16384 [LSB/g] * 2^INT32_ACCEL_FRAC = 0.6131
+ */
+#if !defined IMU_ACCEL_X_SENS & !defined IMU_ACCEL_Y_SENS & !defined IMU_ACCEL_Z_SENS
+// FIXME
+#define IMU_ACCEL_X_SENS 0.6131
+#define IMU_ACCEL_X_SENS_NUM 6131
+#define IMU_ACCEL_X_SENS_DEN 10000
+#define IMU_ACCEL_Y_SENS 0.6131
+#define IMU_ACCEL_Y_SENS_NUM 6131
+#define IMU_ACCEL_Y_SENS_DEN 10000
+#define IMU_ACCEL_Z_SENS 0.6131
+#define IMU_ACCEL_Z_SENS_NUM 6131
+#define IMU_ACCEL_Z_SENS_DEN 10000
+#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 ImuKrooz {
+  volatile bool_t gyr_valid;
+  volatile bool_t acc_valid;
+  volatile bool_t mag_valid;
+  struct Mpu60x0_I2c mpu;
+  struct Hmc58xx hmc;
+  struct Int32Rates rates_sum;
+  struct Int32Vect3 accel_sum;
+  volatile uint8_t  meas_nb;
+};
+
+extern struct ImuKrooz imu_krooz;
+
+
+/* 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_krooz_event( void );
+extern void imu_krooz_downlink_raw( void );
+
+static inline void ImuEvent(void (* _gyro_handler)(void), void (* _accel_handler)(void), void (* _mag_handler)(void) __attribute__((unused))) {
+  imu_krooz_event();
+  if (imu_krooz.gyr_valid) {
+    imu_krooz.gyr_valid = FALSE;
+    _gyro_handler();
+  }
+  if (imu_krooz.acc_valid) {
+    imu_krooz.acc_valid = FALSE;
+    _accel_handler();
+  }
+  if (imu_krooz.mag_valid) {
+    imu_krooz.mag_valid = FALSE;
+    _mag_handler();
+  }
+}
+
+#endif // IMU_KROOZ_H

sw/airborne/boards/krooz_sd.h

@@ -210,7 +210,6 @@
 #define BOARD_HAS_BARO 1
 
 /* PWM */
-//#define PWM_USE_TIM2 1
 #define PWM_USE_TIM3 1
 #define PWM_USE_TIM4 1
 #define PWM_USE_TIM5 1
@@ -227,7 +226,12 @@
 #define USE_PWM9 1
 //#define USE_PWM10 1
 
+#if USE_PWM10
+#define ACTUATORS_PWM_NB 11
+#define PWM_USE_TIM2 1
+#else
 #define ACTUATORS_PWM_NB 10
+#endif
 
 // PWM_SERVO_x is the index of the servo in the actuators_pwm_values array
 #if USE_PWM0
@@ -397,7 +401,7 @@
  * Spektrum
  */
 /* The line that is pulled low at power up to initiate the bind process */
-#define SPEKTRUM_BIND_PIN GPIO8
+#define SPEKTRUM_BIND_PIN GPIO9
 #define SPEKTRUM_BIND_PIN_PORT GPIOA
 
 #endif /* CONFIG_KROOZ_1_0_H */

sw/airborne/modules/sensors/baro_ms5611_i2c.c

@@ -55,6 +55,7 @@ uint8_t ms5611_status;
 uint16_t ms5611_c[PROM_NB];
 uint32_t ms5611_d1, ms5611_d2;
 int32_t prom_cnt;
+int64_t baroms;
 float fbaroms, ftempms;
 float baro_ms5611_alt;
 bool_t baro_ms5611_enabled;
@@ -193,7 +194,7 @@ void baro_ms5611_event( void ) {
       break;
 
     case  MS5611_ADC_D2: {
-      int64_t dt, baroms, tempms, off, sens, t2, off2, sens2;
+      int64_t dt, tempms, off, sens, t2, off2, sens2;
       /* read D2 (temperature) */
       ms5611_d2 = (ms5611_trans.buf[0] << 16) |
                   (ms5611_trans.buf[1] << 8) |

sw/airborne/modules/sensors/baro_ms5611_i2c.h

@@ -20,6 +20,7 @@ extern bool_t baro_ms5611_valid;
 extern bool_t baro_ms5611_enabled;
 extern float baro_ms5611_r;
 extern float baro_ms5611_sigma2;
+extern int64_t baroms;
 
 enum ms5611_stat{
   MS5611_UNINIT,
@@ -35,11 +36,11 @@ enum ms5611_stat{
   MS5611_ADC_D2
 };
 
-void baro_ms5611_init(void);
+extern void baro_ms5611_init(void);
-void baro_ms5611_periodic(void);
+extern void baro_ms5611_periodic(void);
-void baro_ms5611_d1(void);
+extern void baro_ms5611_d1(void);
-void baro_ms5611_d2(void);
+extern void baro_ms5611_d2(void);
-void baro_ms5611_event(void);
+extern void baro_ms5611_event(void);
 
 #define BaroMs5611Update(_b) { if (baro_ms5611_valid) { _b = baro_ms5611_alt; baro_ms5611_valid = FALSE; } }