mirror of
https://github.com/paparazzi/paparazzi.git
synced 2026-05-31 20:38:27 +08:00
[imu] initial commit of new aspirin2 driver using mpu60x0 peripheral
Mag slave still missing...
This commit is contained in:
Felix Ruess
@@ -32,7 +32,7 @@
|
||||
</subsystem>
|
||||
|
||||
<subsystem name="telemetry" type="transparent"/>
|
||||
<subsystem name="imu" type="aspirin_v2.1"/>
|
||||
<subsystem name="imu" type="aspirin_v2.1_new"/>
|
||||
<subsystem name="gps" type="ublox"/>
|
||||
<subsystem name="stabilization" type="int_quat"/>
|
||||
<subsystem name="ahrs" type="int_cmpl_quat">
|
||||
|
||||
@@ -0,0 +1,79 @@
|
||||
# Hey Emacs, this is a -*- makefile -*-
|
||||
#
|
||||
# Aspirin IMU v2.1
|
||||
#
|
||||
#
|
||||
# required xml:
|
||||
# <section name="IMU" prefix="IMU_">
|
||||
#
|
||||
# <!-- these gyro and accel calib values are the defaults for aspirin2.1 -->
|
||||
# <define name="GYRO_X_NEUTRAL" value="0"/>
|
||||
# <define name="GYRO_Y_NEUTRAL" value="0"/>
|
||||
# <define name="GYRO_Z_NEUTRAL" value="0"/>
|
||||
#
|
||||
# <define name="GYRO_X_SENS" value="4.359" integer="16"/>
|
||||
# <define name="GYRO_Y_SENS" value="4.359" integer="16"/>
|
||||
# <define name="GYRO_Z_SENS" value="4.359" integer="16"/>
|
||||
#
|
||||
# <define name="ACCEL_X_NEUTRAL" value="0"/>
|
||||
# <define name="ACCEL_Y_NEUTRAL" value="0"/>
|
||||
# <define name="ACCEL_Z_NEUTRAL" value="0"/>
|
||||
#
|
||||
# <define name="ACCEL_X_SENS" value="4.905" integer="16"/>
|
||||
# <define name="ACCEL_Y_SENS" value="4.905" integer="16"/>
|
||||
# <define name="ACCEL_Z_SENS" value="4.905" integer="16"/>
|
||||
#
|
||||
# <!-- replace the mag calibration with your own-->
|
||||
# <define name="MAG_X_NEUTRAL" value="-45"/>
|
||||
# <define name="MAG_Y_NEUTRAL" value="334"/>
|
||||
# <define name="MAG_Z_NEUTRAL" value="7"/>
|
||||
#
|
||||
# <define name="MAG_X_SENS" value="3.4936416" integer="16"/>
|
||||
# <define name="MAG_Y_SENS" value="3.607713" integer="16"/>
|
||||
# <define name="MAG_Z_SENS" value="4.90788848" integer="16"/>
|
||||
#
|
||||
# </section>
|
||||
#
|
||||
#
|
||||
|
||||
|
||||
# for fixedwing firmware and ap only
|
||||
ifeq ($(TARGET), ap)
|
||||
IMU_ASPIRIN_2_CFLAGS = -DUSE_IMU
|
||||
endif
|
||||
|
||||
IMU_ASPIRIN_2_CFLAGS += -DIMU_TYPE_H=\"imu/imu_aspirin_2.h\"
|
||||
IMU_ASPIRIN_2_SRCS = $(SRC_SUBSYSTEMS)/imu.c
|
||||
IMU_ASPIRIN_2_SRCS += $(SRC_SUBSYSTEMS)/imu/imu_aspirin_2.c
|
||||
IMU_ASPIRIN_2_SRCS += peripherals/mpu60x0_spi.c
|
||||
|
||||
# Magnetometer
|
||||
#IMU_ASPIRIN_2_SRCS += peripherals/hmc58xx.c
|
||||
|
||||
include $(CFG_SHARED)/spi_master.makefile
|
||||
|
||||
ifeq ($(ARCH), lpc21)
|
||||
IMU_ASPIRIN_2_CFLAGS += -DUSE_SPI_SLAVE0
|
||||
IMU_ASPIRIN_2_CFLAGS += -DASPIRIN_2_SPI_SLAVE_IDX=SPI_SLAVE0
|
||||
IMU_ASPIRIN_2_CFLAGS += -DASPIRIN_2_SPI_DEV=spi1
|
||||
IMU_ASPIRIN_2_CFLAGS += -DUSE_SPI1
|
||||
else ifeq ($(ARCH), stm32)
|
||||
IMU_ASPIRIN_2_CFLAGS += -DUSE_SPI2
|
||||
# Slave select configuration
|
||||
# SLAVE2 is on PB12 (NSS) (MPU600 CS)
|
||||
IMU_ASPIRIN_2_CFLAGS += -DUSE_SPI_SLAVE2
|
||||
endif
|
||||
|
||||
#IMU_ASPIRIN_2_CFLAGS += -DIMU_ASPIRIN_VERSION_2_1
|
||||
|
||||
# Keep CFLAGS/Srcs for imu in separate expression so we can assign it to other targets
|
||||
# see: conf/autopilot/subsystems/lisa_passthrough/imu_b2_v1.1.makefile for example
|
||||
|
||||
ap.CFLAGS += $(IMU_ASPIRIN_2_CFLAGS)
|
||||
ap.srcs += $(IMU_ASPIRIN_2_SRCS)
|
||||
|
||||
|
||||
#
|
||||
# NPS simulator
|
||||
#
|
||||
include $(CFG_SHARED)/imu_nps.makefile
|
||||
@@ -0,0 +1,121 @@
|
||||
/*
|
||||
* Copyright (C) 2013 Felix Ruess <felix.ruess@gmail.com>
|
||||
*
|
||||
* 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 subsystems/imu/imu_aspirin_2.c
|
||||
* Driver for the Aspirin v2.x IMU using SPI for the MPU6000.
|
||||
*/
|
||||
|
||||
#include "subsystems/imu.h"
|
||||
|
||||
#include "mcu_periph/i2c.h"
|
||||
#include "mcu_periph/spi.h"
|
||||
|
||||
|
||||
/* defaults suitable for Lisa */
|
||||
#ifndef ASPIRIN_2_SPI_SLAVE_IDX
|
||||
#define ASPIRIN_2_SPI_SLAVE_IDX SPI_SLAVE2
|
||||
#endif
|
||||
PRINT_CONFIG_VAR(ASPIRIN_2_SPI_SLAVE_IDX)
|
||||
|
||||
#ifndef ASPIRIN_2_SPI_DEV
|
||||
#define ASPIRIN_2_SPI_DEV spi2
|
||||
#endif
|
||||
PRINT_CONFIG_VAR(ASPIRIN_2_SPI_DEV)
|
||||
|
||||
#ifndef ASPIRIN_2_I2C_DEV
|
||||
#define ASPIRIN_2_I2C_DEV i2c2
|
||||
#endif
|
||||
PRINT_CONFIG_VAR(ASPIRIN_2_I2C_DEV)
|
||||
|
||||
|
||||
/* gyro internal lowpass frequency */
|
||||
#if !defined ASPIRIN_2_LOWPASS_FILTER && !defined ASPIRIN_2_SMPLRT_DIV
|
||||
#define ASPIRIN_2_LOWPASS_FILTER MPU60X0_DLPF_256HZ
|
||||
#define ASPIRIN_2_SMPLRT_DIV 1
|
||||
PRINT_CONFIG_MSG("Gyro/Accel output rate is 500Hz")
|
||||
#endif
|
||||
PRINT_CONFIG_VAR(ASPIRIN_2_LOWPASS_FILTER)
|
||||
PRINT_CONFIG_VAR(ASPIRIN_2_SMPLRT_DIV)
|
||||
|
||||
#ifndef ASPIRIN_2_GYRO_RANGE
|
||||
#define ASPIRIN_2_GYRO_RANGE MPU60X0_GYRO_RANGE_2000
|
||||
#endif
|
||||
PRINT_CONFIG_VAR(ASPIRIN_2_GYRO_RANGE)
|
||||
|
||||
#ifndef ASPIRIN_2_ACCEL_RANGE
|
||||
#define ASPIRIN_2_ACCEL_RANGE MPU60X0_ACCEL_RANGE_16G
|
||||
#endif
|
||||
PRINT_CONFIG_VAR(ASPIRIN_2_ACCEL_RANGE)
|
||||
|
||||
|
||||
struct ImuAspirin2 imu_aspirin2;
|
||||
|
||||
void imu_impl_init(void)
|
||||
{
|
||||
imu_aspirin2.accel_valid = FALSE;
|
||||
imu_aspirin2.gyro_valid = FALSE;
|
||||
imu_aspirin2.mag_valid = FALSE;
|
||||
|
||||
mpu60x0_spi_init(&imu_aspirin2.mpu, &(ASPIRIN_2_SPI_DEV), ASPIRIN_2_SPI_SLAVE_IDX);
|
||||
// change the default configuration
|
||||
imu_aspirin2.mpu.config.smplrt_div = ASPIRIN_2_SMPLRT_DIV;
|
||||
imu_aspirin2.mpu.config.dlpf_cfg = ASPIRIN_2_LOWPASS_FILTER;
|
||||
imu_aspirin2.mpu.config.gyro_range = ASPIRIN_2_GYRO_RANGE;
|
||||
imu_aspirin2.mpu.config.accel_range = ASPIRIN_2_ACCEL_RANGE;
|
||||
//imu_aspirin2.mpu.config.i2c_bypass = FALSE;
|
||||
//imu_aspirin2.mpu.config.drdy_int_enable = TRUE;
|
||||
|
||||
//hmc58xx_init(&imu_aspirin2.mag_hmc, &(ASPIRIN_2_I2C_DEV), HMC58XX_ADDR);
|
||||
}
|
||||
|
||||
|
||||
void imu_periodic(void)
|
||||
{
|
||||
mpu60x0_spi_periodic(&imu_aspirin2.mpu);
|
||||
|
||||
// Read HMC58XX at 50Hz (main loop for rotorcraft: 512Hz)
|
||||
//RunOnceEvery(10, hmc58xx_periodic(&imu_aspirin2.mag_hmc));
|
||||
}
|
||||
|
||||
void imu_aspirin2_event(void)
|
||||
{
|
||||
mpu60x0_spi_event(&imu_aspirin2.mpu);
|
||||
if (imu_aspirin2.mpu.data_available) {
|
||||
RATES_COPY(imu.gyro_unscaled, imu_aspirin2.mpu.data_rates.rates);
|
||||
VECT3_COPY(imu.accel_unscaled, imu_aspirin2.mpu.data_accel.vect);
|
||||
imu_aspirin2.mpu.data_available = FALSE;
|
||||
imu_aspirin2.gyro_valid = TRUE;
|
||||
imu_aspirin2.accel_valid = TRUE;
|
||||
}
|
||||
|
||||
#if 0
|
||||
/* HMC58XX event task */
|
||||
hmc58xx_event(&imu_aspirin2.mag_hmc);
|
||||
if (imu_aspirin2.mag_hmc.data_available) {
|
||||
imu.mag_unscaled.x = imu_aspirin2.mag_hmc.data.vect.y;
|
||||
imu.mag_unscaled.y = -imu_aspirin2.mag_hmc.data.vect.x;
|
||||
imu.mag_unscaled.z = imu_aspirin2.mag_hmc.data.vect.z;
|
||||
imu_aspirin2.mag_hmc.data_available = FALSE;
|
||||
imu_aspirin2.mag_valid = TRUE;
|
||||
}
|
||||
#endif
|
||||
}
|
||||
@@ -0,0 +1,126 @@
|
||||
/*
|
||||
* Copyright (C) 2013 Felix Ruess <felix.ruess@gmail.com>
|
||||
*
|
||||
* 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 subsystems/imu/imu_aspirin_2.h
|
||||
* Driver for the Aspirin v2.x IMU using SPI for the MPU6000.
|
||||
*/
|
||||
|
||||
#ifndef IMU_ASPIRIN_2_H
|
||||
#define IMU_ASPIRIN_2_H
|
||||
|
||||
#include "std.h"
|
||||
#include "generated/airframe.h"
|
||||
#include "subsystems/imu.h"
|
||||
|
||||
#include "peripherals/mpu60x0_spi.h"
|
||||
|
||||
#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
|
||||
|
||||
#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
|
||||
* MPU60X0 has 16.4 LSB/(deg/s) at 2000deg/s range
|
||||
* sens = 1/16.4 * pi/180 * 2^INT32_RATE_FRAC
|
||||
* sens = 1/16.4 * pi/180 * 4096 = 4.359066229
|
||||
*/
|
||||
#if !defined IMU_GYRO_P_SENS & !defined IMU_GYRO_Q_SENS & !defined IMU_GYRO_R_SENS
|
||||
#define IMU_GYRO_P_SENS 4.359
|
||||
#define IMU_GYRO_P_SENS_NUM 4359
|
||||
#define IMU_GYRO_P_SENS_DEN 1000
|
||||
#define IMU_GYRO_Q_SENS 4.359
|
||||
#define IMU_GYRO_Q_SENS_NUM 4359
|
||||
#define IMU_GYRO_Q_SENS_DEN 1000
|
||||
#define IMU_GYRO_R_SENS 4.359
|
||||
#define IMU_GYRO_R_SENS_NUM 4359
|
||||
#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
|
||||
* MPU60X0 has 2048 LSB/g
|
||||
* fixed point sens: 9.81 [m/s^2] / 2048 [LSB/g] * 2^INT32_ACCEL_FRAC
|
||||
* sens = 9.81 / 2048 * 1024 = 4.905
|
||||
*/
|
||||
#if !defined IMU_ACCEL_X_SENS & !defined IMU_ACCEL_Y_SENS & !defined IMU_ACCEL_Z_SENS
|
||||
#define IMU_ACCEL_X_SENS 4.905
|
||||
#define IMU_ACCEL_X_SENS_NUM 4905
|
||||
#define IMU_ACCEL_X_SENS_DEN 1000
|
||||
#define IMU_ACCEL_Y_SENS 4.905
|
||||
#define IMU_ACCEL_Y_SENS_NUM 4905
|
||||
#define IMU_ACCEL_Y_SENS_DEN 1000
|
||||
#define IMU_ACCEL_Z_SENS 4.905
|
||||
#define IMU_ACCEL_Z_SENS_NUM 4905
|
||||
#define IMU_ACCEL_Z_SENS_DEN 1000
|
||||
#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 ImuAspirin2 {
|
||||
volatile bool_t gyro_valid;
|
||||
volatile bool_t accel_valid;
|
||||
volatile bool_t mag_valid;
|
||||
struct Mpu60x0_Spi mpu;
|
||||
};
|
||||
|
||||
extern struct ImuAspirin2 imu_aspirin2;
|
||||
|
||||
extern void imu_aspirin2_event(void);
|
||||
|
||||
|
||||
static inline void ImuEvent(void (* _gyro_handler)(void), void (* _accel_handler)(void), void (* _mag_handler)(void)) {
|
||||
imu_aspirin2_event();
|
||||
if (imu_aspirin2.gyro_valid) {
|
||||
imu_aspirin2.gyro_valid = FALSE;
|
||||
_gyro_handler();
|
||||
}
|
||||
if (imu_aspirin2.accel_valid) {
|
||||
imu_aspirin2.accel_valid = FALSE;
|
||||
_accel_handler();
|
||||
}
|
||||
if (imu_aspirin2.mag_valid) {
|
||||
imu_aspirin2.mag_valid = FALSE;
|
||||
_mag_handler();
|
||||
}
|
||||
}
|
||||
|
||||
#endif /* IMU_ASPIRIN_2_H */
|
||||
Reference in New Issue
Block a user