mirror of
https://github.com/paparazzi/paparazzi.git
synced 2026-06-04 13:55:40 +08:00
[boards] add driver for KroozSD BigRotorcraftEdition
- IMU with memsic MXR9500 accelerometer - set mag correct freq to 75Hz for krooz_sd_x closes #586
This commit is contained in:
softsr
Felix Ruess
@@ -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));
|
||||
|
||||
@@ -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;
|
||||
|
||||
@@ -0,0 +1,226 @@
|
||||
/*
|
||||
* 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_memsic.c
|
||||
*
|
||||
* Driver for the IMU on the KroozSD Big Rotorcraft Edition board.
|
||||
*
|
||||
* Invensense MPU-6050
|
||||
* Memsic MXR9500 with AD7689
|
||||
* Honeywell HMC-5883
|
||||
*/
|
||||
|
||||
#include <math.h>
|
||||
#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;
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,156 @@
|
||||
/*
|
||||
* 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_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
|
||||
Reference in New Issue
Block a user