Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-06-04 22:17:01 +08:00
Code Issues Actions 30 Packages Projects Releases Wiki Activity

Removed mpu6000 and hmc5833 from px4 imu

Browse Source
This commit is contained in:
kevindehecker
2016-03-23 14:29:39 +01:00
parent 6c9a2badd7
commit d8cb28390b
6 changed files with 48 additions and 124 deletions
Download Patch File Download Diff File Expand all files Collapse all files
conf/airframes/TUDELFT/tudelft_iris_indi.xml
+24 -10
View File
@@ -26,7 +26,11 @@
<subsystem name="stabilization" type="indi" />
<subsystem name="ahrs" type="int_cmpl_quat" >
<define name="AHRS_ICQ_IMU_ID" value="IMU_PX4_ID" /> <!-- Meaning the lsm303 and l3g -->
<define name="AHRS_ICQ_MAG_ID" value="IMU_MPU6000_HMC_ID" /> <!-- Meaning the external magnetometer -->
<define name="AHRS_ICQ_MAG_ID" value="MAG_HMC58XX_SENDER_ID" /> <!-- Meaning the external hmc-->
<configure name="USE_MAGNETOMETER" value="TRUE"/>
<define name="AHRS_USE_GPS_HEADING" value="FALSE"/>
</subsystem>
<subsystem name="ins" type="extended" />
<subsystem name="current_sensor">
@@ -80,23 +84,33 @@
<load name="air_data.xml" />
<load name="send_imu_mag_current.xml" />
<load name="gps_ubx_ucenter.xml" />
<load name="mag_hmc58xx.xml">
<configure name="MAG_HMC58XX_I2C_DEV" value="i2c1"/>
<define name="MODULE_HMC58XX_UPDATE_AHRS" value="TRUE"/>
</load>
<!-- <load name="spektrum_soft_bind.xml"/>-->
</modules>
<section name="MISC">
<define name="MilliAmpereOfAdc(adc)" value="((float)adc) * (3.3f / 4096.0f) * (90.0f / 5.0f)" />
<!-- 100Amp = 2Volt -> 2482,42 tick/100Amp"(0.0402832*adc)" -->
</section>
<section name="IMU" prefix="IMU_">
<!-- replace this with your own calibration -->
<define name="MAG_X_NEUTRAL" value="14" />
<define name="MAG_Y_NEUTRAL" value="116" />
<define name="MAG_Z_NEUTRAL" value="119" />
<define name="MAG_X_SENS" value="5.09245681612" integer="16" />
<define name="MAG_Y_SENS" value="5.29702744632" integer="16" />
<define name="MAG_Z_SENS" value="5.65287938992" integer="16" />
<define name="BODY_TO_IMU_PHI" value="180." unit="deg" />
<define name="BODY_TO_IMU_THETA" value="180." unit="deg" />
<define name="BODY_TO_IMU_PSI" value="90." unit="deg" />
<define name="ACCEL_X_NEUTRAL" value="274"/>
<define name="ACCEL_Y_NEUTRAL" value="-43"/>
<define name="ACCEL_Z_NEUTRAL" value="1112"/>
<define name="ACCEL_X_SENS" value="0.625515268183" integer="16"/>
<define name="ACCEL_Y_SENS" value="0.609477051286" integer="16"/>
<define name="ACCEL_Z_SENS" value="0.62150684522" integer="16"/>
<define name="MAG_X_NEUTRAL" value="-151"/>
<define name="MAG_Y_NEUTRAL" value="-288"/>
<define name="MAG_Z_NEUTRAL" value="-3949"/>
<define name="MAG_X_SENS" value="0.420984584445" integer="16"/>
<define name="MAG_Y_SENS" value="0.447747800597" integer="16"/>
<define name="MAG_Z_SENS" value="0.431245648647" integer="16"/>
</section>
<commands>
<axis name="PITCH" failsafe_value="0" />
conf/firmwares/subsystems/shared/imu_px4fmu_v2.4.makefile
+1 -22
View File
@@ -2,7 +2,7 @@
#
# PX4 Pixhawk IMUconsists of two (internal) IMU's and one internal magneto. Also an optional external magneto
#
#MPU6000 + L3GD20 + LSM303D + HMC5883
# L3GD20 + LSM303D
include $(CFG_SHARED)/spi_master.makefile
@@ -11,19 +11,12 @@ IMU_CFLAGS = -DIMU_TYPE_H=\"imu/imu_px4fmu_v2.4.h\"
IMU_SRCS = $(SRC_SUBSYSTEMS)/imu.c
IMU_SRCS += $(SRC_SUBSYSTEMS)/imu/imu_px4fmu_v2.4.c
# MPU
IMU_SRCS += peripherals/mpu60x0.c
IMU_SRCS += peripherals/mpu60x0_spi.c
#L3GD20 gyro
IMU_SRCS += peripherals/l3gd20_spi.c
#LSM303D accelero + magneto
IMU_SRCS += peripherals/lsm303dlhc_spi.c
# Magnetometer
IMU_SRCS += peripherals/hmc58xx.c
# for fixedwing firmware and ap only
ifeq ($(TARGET), ap)
IMU_CFLAGS += -DUSE_IMU
@@ -37,11 +30,6 @@ IMU_SPI_DEV_LOWER=$(shell echo $(IMU_SPI_DEV) | tr A-Z a-z)
IMU_CFLAGS += -DIMU_SPI_DEV=$(IMU_SPI_DEV_LOWER)
IMU_CFLAGS += -DUSE_$(IMU_SPI_DEV_UPPER)
#********** MPU6000 ***********
IMU_MPU_SPI_SLAVE_IDX ?= SPI_SLAVE2
IMU_CFLAGS += -DIMU_MPU_SPI_SLAVE_IDX=$(IMU_MPU_SPI_SLAVE_IDX)
IMU_CFLAGS += -DUSE_$(IMU_MPU_SPI_SLAVE_IDX)
#********** L3GD20 ***********
IMU_L3G_SPI_SLAVE_IDX ?= SPI_SLAVE0
IMU_CFLAGS += -DIMU_L3G_SPI_SLAVE_IDX=$(IMU_L3G_SPI_SLAVE_IDX)
@@ -52,15 +40,6 @@ IMU_LSM_SPI_SLAVE_IDX ?= SPI_SLAVE1
IMU_CFLAGS += -DIMU_LSM_SPI_SLAVE_IDX=$(IMU_LSM_SPI_SLAVE_IDX)
IMU_CFLAGS += -DUSE_$(IMU_LSM_SPI_SLAVE_IDX)
#********** HMC5883 ***********
IMU_HMC_I2C_DEV ?= i2c1
# convert i2cx to upper/lower case
IMU_HMC_I2C_DEV_UPPER=$(shell echo $(IMU_HMC_I2C_DEV) | tr a-z A-Z)
IMU_HMC_I2C_DEV_LOWER=$(shell echo $(IMU_HMC_I2C_DEV) | tr A-Z a-z)
IMU_CFLAGS += -DIMU_HMC_I2C_DEV=$(IMU_HMC_I2C_DEV_LOWER)
IMU_CFLAGS += -DUSE_$(IMU_HMC_I2C_DEV_UPPER)
# add it for all targets except sim, fbw and nps
ifeq (,$(findstring $(TARGET),sim fbw nps))
$(TARGET).CFLAGS += $(IMU_CFLAGS)
sw/airborne/peripherals/l3gd20_regs.h
+1 -1
View File
@@ -80,7 +80,7 @@ enum L3gd20FullScale {
L3GD20_FS_250dps = 0,
L3GD20_FS_500dps = 1,
L3GD20_FS_2000dps = 2,
L3GD20_FS_2000dps2 = 3,
L3GD20_FS_2000dps2 = 3, //yep, the same as L3GD20_FS_2000dps
};
sw/airborne/peripherals/lsm303dlhc_spi.c
+1 -1
View File
@@ -35,7 +35,7 @@
#endif
#ifndef LSM303DLHC_DEFAULT_AFS
#define LSM303DLHC_DEFAULT_AFS 0x00 // acc +- 2G
#define LSM303DLHC_DEFAULT_AFS (0x04 <<3) // acc +- 16G
#endif
#ifndef LSM303DLHC_DEFAULT_MODR
sw/airborne/subsystems/imu/imu_px4fmu_v2.4.c
+16 -80
View File
@@ -1,5 +1,5 @@
/*
* Copyright (C) 2013-2015 Felix Ruess <felix.ruess@gmail.com>
* Copyright (C) 2013-2016 the paparazzi team
*
* This file is part of paparazzi.
*
@@ -19,74 +19,29 @@
* Boston, MA 02111-1307, USA.
*/
/**
* @file subsystems/imu/imu_px4fmu_v2.4.h
* Driver for pixhawk IMU's.
* On with spi: L3GD20H + LSM303D and the MPU6000.
* On i2c: external HMC5883L (through 3dr gps).
* L3GD20H + LSM303D (both on spi)
*/
#include "subsystems/imu.h"
#include "subsystems/abi.h"
#include "mcu_periph/spi.h"
#include "peripherals/hmc58xx_regs.h"
#include "peripherals/l3gd20_regs.h"
#include "peripherals/lsm303dlhc_regs.h"
#include "peripherals/lsm303dlhc_spi.h"
/************MPU6000*****************/
/* SPI defaults set in subsystem makefile, can be configured from airframe file */
PRINT_CONFIG_VAR(IMU_MPU_SPI_SLAVE_IDX)
PRINT_CONFIG_VAR(IMU_LSM_SPI_SLAVE_IDX)
PRINT_CONFIG_VAR(IMU_L3G_SPI_SLAVE_IDX)
PRINT_CONFIG_VAR(IMU_SPI_DEV)
/* MPU60x0 gyro/accel internal lowpass frequency */
#if !defined IMU_MPU_LOWPASS_FILTER && !defined IMU_MPU_SMPLRT_DIV
#if (PERIODIC_FREQUENCY == 60) || (PERIODIC_FREQUENCY == 120)
/* Accelerometer: Bandwidth 44Hz, Delay 4.9ms
* Gyroscope: Bandwidth 42Hz, Delay 4.8ms sampling 1kHz
*/
#define IMU_MPU_LOWPASS_FILTER MPU60X0_DLPF_42HZ
#define IMU_MPU_SMPLRT_DIV 9
PRINT_CONFIG_MSG("Gyro/Accel output rate is 100Hz at 1kHz internal sampling")
#elif PERIODIC_FREQUENCY == 512
/* Accelerometer: Bandwidth 260Hz, Delay 0ms
* Gyroscope: Bandwidth 256Hz, Delay 0.98ms sampling 8kHz
*/
#define IMU_MPU_LOWPASS_FILTER MPU60X0_DLPF_256HZ
#define IMU_MPU_SMPLRT_DIV 3
PRINT_CONFIG_MSG("Gyro/Accel output rate is 2kHz at 8kHz internal sampling")
#else
#error Non-default PERIODIC_FREQUENCY: please define MPU_HMC_LOWPASS_FILTER and MPU_HMC_SMPLRT_DIV.
#endif
#endif
PRINT_CONFIG_VAR(IMU_MPU_LOWPASS_FILTER)
PRINT_CONFIG_VAR(IMU_MPU_SMPLRT_DIV)
#ifndef IMU_MPU_GYRO_RANGE
#define IMU_MPU_GYRO_RANGE MPU60X0_GYRO_RANGE_2000
#endif
PRINT_CONFIG_VAR(IMU_MPU_GYRO_RANGE)
#ifndef IMU_MPU_ACCEL_RANGE
#define IMU_MPU_ACCEL_RANGE MPU60X0_ACCEL_RANGE_16G
#endif
PRINT_CONFIG_VAR(IMU_MPU_ACCEL_RANGE)
/************HMC58XX*****************/
PRINT_CONFIG_VAR(IMU_HMC_I2C_DEV)
struct ImuPX4 imu_px4;
void imu_impl_init(void)
{
/* MPU6000 init */
mpu60x0_spi_init(&imu_px4.mpu, &IMU_SPI_DEV, IMU_MPU_SPI_SLAVE_IDX);
// change the default configuration
imu_px4.mpu.config.smplrt_div = IMU_MPU_SMPLRT_DIV;
imu_px4.mpu.config.dlpf_cfg = IMU_MPU_LOWPASS_FILTER;
imu_px4.mpu.config.gyro_range = IMU_MPU_GYRO_RANGE;
imu_px4.mpu.config.accel_range = IMU_MPU_ACCEL_RANGE;
/* L3GD20 gyro init */
/* initialize gyro and set default options */
@@ -94,25 +49,23 @@ void imu_impl_init(void)
/* LSM303dlhc acc + magneto init */
lsm303dlhc_spi_init(&imu_px4.lsm_acc, &IMU_SPI_DEV, IMU_LSM_SPI_SLAVE_IDX, LSM_TARGET_ACC);
#if MODULE_HMC58XX_UPDATE_AHRS
lsm303dlhc_spi_init(&imu_px4.lsm_mag, &IMU_SPI_DEV, IMU_LSM_SPI_SLAVE_IDX, LSM_TARGET_MAG);
/* HMC58XX magneto init */
/* initialize mag and set default options */
hmc58xx_init(&imu_px4.hmc, &IMU_HMC_I2C_DEV, HMC58XX_ADDR);
#endif
}
void imu_periodic(void)
{
mpu60x0_spi_periodic(&imu_px4.mpu);
l3gd20_spi_periodic(&imu_px4.l3g);
lsm303dlhc_spi_periodic(&imu_px4.lsm_acc);
#if MODULE_HMC58XX_UPDATE_AHRS
/* Read magneto's every 10 times of main freq
* at ~50Hz (main loop for rotorcraft: 512Hz)
*/
RunOnceEvery(10, hmc58xx_periodic(&imu_px4.hmc));
RunOnceEvery(10, lsm303dlhc_spi_periodic(&imu_px4.lsm_mag));
#endif
}
void imu_px4_event(void)
@@ -120,22 +73,13 @@ void imu_px4_event(void)
uint32_t now_ts = get_sys_time_usec();
/* MPU6000 event task */
mpu60x0_spi_event(&imu_px4.mpu);
if (imu_px4.mpu.data_available) {
RATES_COPY(imu.gyro_unscaled, imu_px4.mpu.data_rates.rates);
VECT3_COPY(imu.accel_unscaled, imu_px4.mpu.data_accel.vect);
imu_px4.mpu.data_available = FALSE;
imu_scale_gyro(&imu);
imu_scale_accel(&imu);
AbiSendMsgIMU_GYRO_INT32(IMU_MPU6000_HMC_ID, now_ts, &imu.gyro);
AbiSendMsgIMU_ACCEL_INT32(IMU_MPU6000_HMC_ID, now_ts, &imu.accel);
}
/* L3GD20 event task */
l3gd20_spi_event(&imu_px4.l3g);
if (imu_px4.l3g.data_available) {
RATES_COPY(imu.gyro_unscaled, imu_px4.l3g.data_rates.rates);
//the p and q seem to be swapped on the Pixhawk board compared to the acc
imu.gyro_unscaled.p = imu_px4.l3g.data_rates.rates.q;
imu.gyro_unscaled.q = -imu_px4.l3g.data_rates.rates.p;
imu.gyro_unscaled.r = imu_px4.l3g.data_rates.rates.r;
imu_px4.l3g.data_available = FALSE;
imu_scale_gyro(&imu);
AbiSendMsgIMU_GYRO_INT32(IMU_PX4_ID, now_ts, &imu.gyro);
@@ -149,22 +93,14 @@ void imu_px4_event(void)
imu_scale_accel(&imu);
AbiSendMsgIMU_ACCEL_INT32(IMU_PX4_ID, now_ts, &imu.accel);
}
#if MODULE_HMC58XX_UPDATE_AHRS
lsm303dlhc_spi_event(&imu_px4.lsm_mag);
if (imu_px4.lsm_mag.data_available_mag) {
VECT3_COPY(imu.mag_unscaled, imu_px4.lsm_mag.data_mag.vect);
imu_px4.lsm_mag.data_available_mag = FALSE;
imu_scale_mag(&imu);
AbiSendMsgIMU_MAG_INT32(IMU_MPU6000_HMC_ID, now_ts, &imu.mag);
}
/* HMC58XX event task */
hmc58xx_event(&imu_px4.hmc);
if (imu_px4.hmc.data_available) {
/* mag rotated by 90deg around z axis relative to MPU */
VECT3_COPY(imu.mag_unscaled, imu_px4.hmc.data.vect);
imu_px4.hmc.data_available = FALSE;
imu_scale_mag(&imu);
AbiSendMsgIMU_MAG_INT32(IMU_MPU6000_HMC_ID, now_ts, &imu.mag);
AbiSendMsgIMU_MAG_INT32(IMU_PX4_ID, now_ts, &imu.mag);
}
#endif
}
sw/airborne/subsystems/imu/imu_px4fmu_v2.4.h
+4 -9
View File
@@ -1,5 +1,5 @@
/*
* Copyright (C) 2013 Felix Ruess <felix.ruess@gmail.com>
* Copyright (C) 2013-2016 the paparazzi team
*
* This file is part of paparazzi.
*
@@ -20,10 +20,9 @@
*/
/**
* @file subsystems/imu/imu_mpu6000.h
* @file subsystems/imu/imu_px4fmu_v2.4.h
* Driver for pixhawk IMU's.
* On with spi: L3GD20H + LSM303D and the MPU6000.
* On i2c: external HMC5883L (through 3dr gps).
* L3GD20H + LSM303D (both on spi)
*/
#ifndef IMU_PX4FMUV24_H
@@ -33,16 +32,12 @@
#include "generated/airframe.h"
#include "subsystems/imu.h"
#include "subsystems/imu/imu_mpu60x0_defaults.h"
#include "peripherals/mpu60x0_spi.h"
#include "peripherals/hmc58xx.h"
#include "subsystems/imu/imu_px4_defaults.h"
#include "peripherals/l3gd20_spi.h"
#include "peripherals/lsm303dlhc_spi.h"
struct ImuPX4 {
struct Mpu60x0_Spi mpu;
struct Hmc58xx hmc;
struct L3gd20_Spi l3g;
struct Lsm303dlhc_Spi lsm_acc;
struct Lsm303dlhc_Spi lsm_mag;