diff --git a/conf/airframes/examples/krooz_sd/krooz_sd_bre_hexa_mkk.xml b/conf/airframes/examples/krooz_sd/krooz_sd_bre_hexa_mkk.xml new file mode 100644 index 0000000000..56f5c4ac28 --- /dev/null +++ b/conf/airframes/examples/krooz_sd/krooz_sd_bre_hexa_mkk.xml @@ -0,0 +1,241 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+ + +
+ + + + + + + + + + +
+ + + + + + + + + +
+ + + + + + + + + + + +
+ + + + + + + + + + + + + + + + + + + + + + + + + + +
+ +
+ + + + +
+ +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + +
+ + +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+ +
+ + + + + + + + + + + + + +
+ +
+ + + + + + + + + +
+ +
+ + + +
+ +
+ + + +
+ +
+ + + + + +
+ + diff --git a/conf/airframes/examples/krooz_sd/krooz_sd_hexa_mkk.xml b/conf/airframes/examples/krooz_sd/krooz_sd_hexa_mkk.xml index 6de9a1cd82..c847b2d53f 100644 --- a/conf/airframes/examples/krooz_sd/krooz_sd_hexa_mkk.xml +++ b/conf/airframes/examples/krooz_sd/krooz_sd_hexa_mkk.xml @@ -5,11 +5,6 @@ - - - - - @@ -24,15 +19,19 @@ - - + + + + + + + - + @@ -60,9 +59,6 @@ - - - @@ -111,7 +107,6 @@
- @@ -151,35 +146,35 @@ - + - + - - + + - + - - + + - + - - + + - - - + + + - - - + + + @@ -197,15 +192,14 @@ - - - - - - - - - + + + + + + + +
@@ -214,7 +208,6 @@ - @@ -237,6 +230,12 @@ + +
diff --git a/conf/airframes/examples/krooz_sd/krooz_sd_okto_mkk.xml b/conf/airframes/examples/krooz_sd/krooz_sd_okto_mkk.xml index a01191d274..2f438747d6 100644 --- a/conf/airframes/examples/krooz_sd/krooz_sd_okto_mkk.xml +++ b/conf/airframes/examples/krooz_sd/krooz_sd_okto_mkk.xml @@ -5,11 +5,6 @@ - - - - - @@ -24,16 +19,19 @@ - - - + + + + + + + - + @@ -114,7 +112,6 @@
- @@ -160,29 +157,29 @@ - + - - + + - + - - + + - + - - + + - - - + + + - - - + + + @@ -200,28 +197,26 @@ - - - - - - - - - + + + + + + + +
- + - - - - + + +
@@ -240,6 +235,12 @@ + +
diff --git a/conf/airframes/examples/krooz_sd/krooz_sd_quad_mkk.xml b/conf/airframes/examples/krooz_sd/krooz_sd_quad_mkk.xml index 1d854cd414..7bfc59b978 100644 --- a/conf/airframes/examples/krooz_sd/krooz_sd_quad_mkk.xml +++ b/conf/airframes/examples/krooz_sd/krooz_sd_quad_mkk.xml @@ -24,12 +24,18 @@ + + + + + + + - + + @@ -102,9 +108,7 @@
- - @@ -189,14 +193,14 @@ - - - - - + + + + + - - + +
@@ -206,7 +210,7 @@ - +
@@ -228,6 +232,12 @@ + + diff --git a/conf/firmwares/subsystems/shared/imu_krooz_sd.makefile b/conf/firmwares/subsystems/shared/imu_krooz_sd.makefile index baf9c6ae10..87e3aa8822 100644 --- a/conf/firmwares/subsystems/shared/imu_krooz_sd.makefile +++ b/conf/firmwares/subsystems/shared/imu_krooz_sd.makefile @@ -20,8 +20,10 @@ IMU_KROOZ_SRCS += peripherals/hmc58xx.c AHRS_PROPAGATE_FREQUENCY ?= 512 AHRS_CORRECT_FREQUENCY ?= 512 +AHRS_MAG_CORRECT_FREQUENCY ?= 75 ap.CFLAGS += -DAHRS_PROPAGATE_FREQUENCY=$(AHRS_PROPAGATE_FREQUENCY) ap.CFLAGS += -DAHRS_CORRECT_FREQUENCY=$(AHRS_CORRECT_FREQUENCY) +ap.CFLAGS += -DAHRS_MAG_CORRECT_FREQUENCY=$(AHRS_MAG_CORRECT_FREQUENCY) ap.CFLAGS += $(IMU_KROOZ_CFLAGS) ap.srcs += $(IMU_KROOZ_SRCS) diff --git a/conf/firmwares/subsystems/shared/imu_krooz_sd_memsic.makefile b/conf/firmwares/subsystems/shared/imu_krooz_sd_memsic.makefile new file mode 100644 index 0000000000..38a5fdf33f --- /dev/null +++ b/conf/firmwares/subsystems/shared/imu_krooz_sd_memsic.makefile @@ -0,0 +1,39 @@ +# Hey Emacs, this is a -*- makefile -*- +# +# KroozSD IMU +# + +include $(CFG_SHARED)/spi_master.makefile + +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_memsic.c \ + $(SRC_ARCH)/subsystems/imu/imu_krooz_sd_arch.c + +IMU_KROOZ_I2C_DEV=i2c2 +IMU_KROOZ_SPI_DEV=spi2 +IMU_KROOZ_CFLAGS += -DUSE_I2C -DUSE_I2C2 -DI2C2_CLOCK_SPEED=400000 +IMU_KROOZ_CFLAGS += -DUSE_SPI -DUSE_SPI2 -DUSE_SPI_SLAVE1 -DUSE_SPI_SLAVE2 -DIMU_KROOZ_SPI_SLAVE_IDX=1 + +IMU_KROOZ_CFLAGS += -DIMU_KROOZ_I2C_DEV=$(IMU_KROOZ_I2C_DEV) +IMU_KROOZ_CFLAGS += -DIMU_KROOZ_SPI_DEV=$(IMU_KROOZ_SPI_DEV) +IMU_KROOZ_SRCS += peripherals/mpu60x0.c +IMU_KROOZ_SRCS += peripherals/mpu60x0_i2c.c +IMU_KROOZ_SRCS += peripherals/hmc58xx.c + +AHRS_PROPAGATE_FREQUENCY ?= 512 +AHRS_CORRECT_FREQUENCY ?= 512 +AHRS_MAG_CORRECT_FREQUENCY ?= 75 +ap.CFLAGS += -DAHRS_PROPAGATE_FREQUENCY=$(AHRS_PROPAGATE_FREQUENCY) +ap.CFLAGS += -DAHRS_CORRECT_FREQUENCY=$(AHRS_CORRECT_FREQUENCY) +ap.CFLAGS += -DAHRS_MAG_CORRECT_FREQUENCY=$(AHRS_MAG_CORRECT_FREQUENCY) + +ap.CFLAGS += $(IMU_KROOZ_CFLAGS) +ap.srcs += $(IMU_KROOZ_SRCS) + +# +# NPS simulator +# +include $(CFG_SHARED)/imu_nps.makefile diff --git a/sw/airborne/boards/krooz/imu_krooz.c b/sw/airborne/boards/krooz/imu_krooz.c index 17c19e42f8..5a335a1eb8 100644 --- a/sw/airborne/boards/krooz/imu_krooz.c +++ b/sw/airborne/boards/krooz/imu_krooz.c @@ -59,10 +59,6 @@ PRINT_CONFIG_VAR(KROOZ_ACCEL_RANGE) struct ImuKrooz imu_krooz; - -#if IMU_KROOZ_USE_GYRO_MEDIAN_FILTER -struct MedianFilter3Int median_gyro; -#endif #if IMU_KROOZ_USE_ACCEL_MEDIAN_FILTER struct MedianFilter3Int median_accel; #endif @@ -83,9 +79,6 @@ void imu_impl_init( void ) hmc58xx_init(&imu_krooz.hmc, &(IMU_KROOZ_I2C_DEV), HMC58XX_ADDR); // Init median filters -#if IMU_KROOZ_USE_GYRO_MEDIAN_FILTER - InitMedianFilterRatesInt(median_gyro); -#endif #if IMU_KROOZ_USE_ACCEL_MEDIAN_FILTER InitMedianFilterVect3Int(median_accel); #endif @@ -116,19 +109,11 @@ void imu_periodic( void ) 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 IMU_KROOZ_USE_GYRO_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 IMU_KROOZ_USE_ACCEL_MEDIAN_FILTER UpdateMedianFilterVect3Int(median_accel, imu.accel_unscaled); #endif - - RATES_SMUL(imu_krooz.gyro_filtered, imu_krooz.gyro_filtered, IMU_KROOZ_GYRO_AVG_FILTER); - RATES_ADD(imu_krooz.gyro_filtered, imu.gyro_unscaled); - RATES_SDIV(imu_krooz.gyro_filtered, imu_krooz.gyro_filtered, (IMU_KROOZ_GYRO_AVG_FILTER + 1)); - RATES_COPY(imu.gyro_unscaled, imu_krooz.gyro_filtered); - VECT3_SMUL(imu_krooz.accel_filtered, imu_krooz.accel_filtered, IMU_KROOZ_ACCEL_AVG_FILTER); VECT3_ADD(imu_krooz.accel_filtered, imu.accel_unscaled); VECT3_SDIV(imu_krooz.accel_filtered, imu_krooz.accel_filtered, (IMU_KROOZ_ACCEL_AVG_FILTER + 1)); diff --git a/sw/airborne/boards/krooz/imu_krooz.h b/sw/airborne/boards/krooz/imu_krooz.h index 565e48a838..3584891fc1 100644 --- a/sw/airborne/boards/krooz/imu_krooz.h +++ b/sw/airborne/boards/krooz/imu_krooz.h @@ -100,11 +100,8 @@ #define IMU_ACCEL_Z_NEUTRAL 0 #endif -#ifndef IMU_KROOZ_GYRO_AVG_FILTER -#define IMU_KROOZ_GYRO_AVG_FILTER 5 -#endif #ifndef IMU_KROOZ_ACCEL_AVG_FILTER -#define IMU_KROOZ_ACCEL_AVG_FILTER 10 +#define IMU_KROOZ_ACCEL_AVG_FILTER 15 #endif struct ImuKrooz { @@ -119,7 +116,7 @@ struct ImuKrooz { struct Int32Vect3 accel_sum; volatile uint8_t meas_nb; struct Int32Vect3 accel_filtered; - struct Int32Rates gyro_filtered; + int32_t temperature; }; extern struct ImuKrooz imu_krooz; diff --git a/sw/airborne/boards/krooz/imu_krooz_memsic.c b/sw/airborne/boards/krooz/imu_krooz_memsic.c new file mode 100644 index 0000000000..0a3dfd2f39 --- /dev/null +++ b/sw/airborne/boards/krooz/imu_krooz_memsic.c @@ -0,0 +1,226 @@ +/* + * Copyright (C) 2013 Sergey Krukowski + * + * 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_memsic.c + * + * Driver for the IMU on the KroozSD Big Rotorcraft Edition board. + * + * Invensense MPU-6050 + * Memsic MXR9500 with AD7689 + * Honeywell HMC-5883 + */ + +#include +#include "boards/krooz/imu_krooz_memsic.h" +#include "subsystems/imu/imu_krooz_sd_arch.h" +#include "mcu_periph/i2c.h" +#include "led.h" +#include "filters/median_filter.h" +#include "mcu_periph/sys_time.h" + +#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 IMU_KROOZ_USE_ACCEL_MEDIAN_FILTER +struct MedianFilter3Int median_accel; +#endif +struct MedianFilter3Int median_mag; + +static uint32_t ad7689_event_timer; +static uint8_t axis_cnt; +static uint8_t axis_nb; + +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); + + // Init median filters +#if IMU_KROOZ_USE_ACCEL_MEDIAN_FILTER + InitMedianFilterVect3Int(median_accel); +#endif + InitMedianFilterVect3Int(median_mag); + + 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.hmc_eoc = FALSE; + imu_krooz.mpu_eoc = FALSE; + + imu_krooz.ad7689_trans.slave_idx = IMU_KROOZ_SPI_SLAVE_IDX; + imu_krooz.ad7689_trans.select = SPISelectUnselect; + imu_krooz.ad7689_trans.cpol = SPICpolIdleLow; + imu_krooz.ad7689_trans.cpha = SPICphaEdge1; + imu_krooz.ad7689_trans.dss = SPIDss8bit; + imu_krooz.ad7689_trans.bitorder = SPIMSBFirst; + imu_krooz.ad7689_trans.cdiv = SPIDiv16; + imu_krooz.ad7689_trans.output_length = sizeof(imu_krooz.ad7689_spi_tx_buffer); + imu_krooz.ad7689_trans.output_buf = (uint8_t*) imu_krooz.ad7689_spi_tx_buffer; + imu_krooz.ad7689_trans.input_length = sizeof(imu_krooz.ad7689_spi_rx_buffer); + imu_krooz.ad7689_trans.input_buf = (uint8_t*) imu_krooz.ad7689_spi_rx_buffer; + imu_krooz.ad7689_trans.before_cb = NULL; + imu_krooz.ad7689_trans.after_cb = NULL; + axis_cnt = 0; + axis_nb = 2; + + 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); + + RATES_ASSIGN(imu_krooz.rates_sum, 0, 0, 0); + imu_krooz.meas_nb = 0; + imu_krooz.gyr_valid = TRUE; + } + + if (imu_krooz.meas_nb_acc.x && imu_krooz.meas_nb_acc.y && imu_krooz.meas_nb_acc.z) { + imu.accel_unscaled.x = 65536 - imu_krooz.accel_sum.x / imu_krooz.meas_nb_acc.x; + imu.accel_unscaled.y = 65536 - imu_krooz.accel_sum.y / imu_krooz.meas_nb_acc.y; + imu.accel_unscaled.z = imu_krooz.accel_sum.z / imu_krooz.meas_nb_acc.z; + +#if IMU_KROOZ_USE_ACCEL_MEDIAN_FILTER + UpdateMedianFilterVect3Int(median_accel, imu.accel_unscaled); +#endif + VECT3_SMUL(imu_krooz.accel_filtered, imu_krooz.accel_filtered, IMU_KROOZ_ACCEL_AVG_FILTER); + VECT3_ADD(imu_krooz.accel_filtered, imu.accel_unscaled); + VECT3_SDIV(imu_krooz.accel_filtered, imu_krooz.accel_filtered, (IMU_KROOZ_ACCEL_AVG_FILTER + 1)); + VECT3_COPY(imu.accel_unscaled, imu_krooz.accel_filtered); + + INT_VECT3_ZERO(imu_krooz.accel_sum); + INT_VECT3_ZERO(imu_krooz.meas_nb_acc); + imu_krooz.acc_valid = TRUE; + } + + RunOnceEvery(128,{axis_nb = 5;}); +} + +void imu_krooz_event( void ) +{ + if (imu_krooz.mpu_eoc) { + mpu60x0_i2c_read(&imu_krooz.mpu); + imu_krooz.mpu_eoc = FALSE; + } + + // 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); + imu_krooz.meas_nb++; + imu_krooz.mpu.data_available = FALSE; + } + + if(SysTimeTimer(ad7689_event_timer) > 215) { + SysTimeTimerStart(ad7689_event_timer); + if(axis_cnt < axis_nb) + axis_cnt++; + else + axis_cnt = 0; + imu_krooz.ad7689_trans.output_buf[0] = + axis_cnt <= 2 ? 0xF0 | (axis_cnt << 1) : (axis_cnt >= 4 ? 0xF0 | ((axis_cnt - 3) << 1) : 0xB0); + imu_krooz.ad7689_trans.output_buf[1] = 0x44; + spi_submit(&(IMU_KROOZ_SPI_DEV), &imu_krooz.ad7689_trans); + } + if (imu_krooz.ad7689_trans.status == SPITransSuccess) { + imu_krooz.ad7689_trans.status = SPITransDone; + uint16_t buf = (imu_krooz.ad7689_trans.input_buf[0] << 8) | imu_krooz.ad7689_trans.input_buf[1]; + switch(axis_cnt) { + case 0: + case 3: + imu_krooz.accel_sum.x += (int32_t)buf; + imu_krooz.meas_nb_acc.x++; + break; + case 1: + case 4: + imu_krooz.accel_sum.y += (int32_t)buf; + imu_krooz.meas_nb_acc.y++; + break; + case 2: + imu_krooz.accel_sum.z += (int32_t)buf; + imu_krooz.meas_nb_acc.z++; + break; + case 5: + imu_krooz.temperature = (imu_krooz.temperature * 4 + (int32_t)buf) / 5; + //imu.temperature = 33000 * imu_krooz.temp / 65536 - 2400; + axis_nb = 2; + break; + default: + axis_cnt = 0; + break; + } + } + + if (imu_krooz.hmc_eoc) { + hmc58xx_read(&imu_krooz.hmc); + imu_krooz.hmc_eoc = FALSE; + } + + // If the HMC5883 I2C transaction has succeeded: convert the data + hmc58xx_event(&imu_krooz.hmc); + if (imu_krooz.hmc.data_available) { + VECT3_ASSIGN(imu.mag_unscaled, imu_krooz.hmc.data.vect.y, -imu_krooz.hmc.data.vect.x, imu_krooz.hmc.data.vect.z); + UpdateMedianFilterVect3Int(median_mag, imu.mag_unscaled); + imu_krooz.hmc.data_available = FALSE; + imu_krooz.mag_valid = TRUE; + } +} diff --git a/sw/airborne/boards/krooz/imu_krooz_memsic.h b/sw/airborne/boards/krooz/imu_krooz_memsic.h new file mode 100644 index 0000000000..9b7b6c7ddc --- /dev/null +++ b/sw/airborne/boards/krooz/imu_krooz_memsic.h @@ -0,0 +1,156 @@ +/* + * Copyright (C) 2013 Sergey Krukowski + * + * 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_memsic.h + * + * Driver for the IMU on the KroozSD Big Rotorcraft Edition board. + * + * Invensense MPU-6050 + * Memsic MXR9500 with AD7689 + * Honeywell HMC-5883 + */ + +#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" +#include "mcu_periph/spi.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 using 16 bit AD7689 adc + * MXR9500 with 1.5g has 21845 LSB/g + * sens = 9.81 [m/s^2] / 21845 [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.9197 +#define IMU_ACCEL_X_SENS_NUM 9197 +#define IMU_ACCEL_X_SENS_DEN 10000 +#define IMU_ACCEL_Y_SENS 0.9197 +#define IMU_ACCEL_Y_SENS_NUM 9197 +#define IMU_ACCEL_Y_SENS_DEN 10000 +#define IMU_ACCEL_Z_SENS 0.9197 +#define IMU_ACCEL_Z_SENS_NUM 9197 +#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 32768 +#define IMU_ACCEL_Y_NEUTRAL 32768 +#define IMU_ACCEL_Z_NEUTRAL 32768 +#endif + +#ifndef IMU_KROOZ_ACCEL_AVG_FILTER +#define IMU_KROOZ_ACCEL_AVG_FILTER 15 +#endif + +struct ImuKrooz { + volatile bool_t gyr_valid; + volatile bool_t acc_valid; + volatile bool_t mag_valid; + volatile bool_t mpu_eoc; + volatile bool_t hmc_eoc; + struct Mpu60x0_I2c mpu; + struct spi_transaction ad7689_trans; + volatile uint8_t ad7689_spi_tx_buffer[2]; + volatile uint8_t ad7689_spi_rx_buffer[2]; + struct Hmc58xx hmc; + struct Int32Rates rates_sum; + struct Int32Vect3 accel_sum; + volatile uint8_t meas_nb; + struct Uint8Vect3 meas_nb_acc; + struct Int32Vect3 accel_filtered; + int32_t temperature; +}; + +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