diff --git a/conf/airframes/TUDELFT/tudelft_iris_indi.xml b/conf/airframes/TUDELFT/tudelft_iris_indi.xml index c12ad51d0a..1c5dcad224 100644 --- a/conf/airframes/TUDELFT/tudelft_iris_indi.xml +++ b/conf/airframes/TUDELFT/tudelft_iris_indi.xml @@ -3,8 +3,8 @@ @@ -21,14 +21,17 @@ - - - - - + - + + + + + + + + @@ -81,23 +84,38 @@ + + + + + + + + + + +
- - - - - - - - - + + + + + + + + + + + + +
diff --git a/conf/firmwares/subsystems/shared/baro_board.makefile b/conf/firmwares/subsystems/shared/baro_board.makefile index 096a079e4f..5c49b6bb8c 100644 --- a/conf/firmwares/subsystems/shared/baro_board.makefile +++ b/conf/firmwares/subsystems/shared/baro_board.makefile @@ -168,11 +168,14 @@ else ifeq ($(BOARD), krooz) # PX4FMU else ifeq ($(BOARD),$(filter $(BOARD),px4fmu)) - BARO_BOARD_CFLAGS += -DUSE_I2C2 - BARO_BOARD_CFLAGS += -DBB_MS5611_I2C_DEV=i2c2 + + BARO_BOARD_CFLAGS += -DUSE_SPI1 -DUSE_SPI_SLAVE3 + BARO_BOARD_CFLAGS += -DBB_MS5611_SPI_DEV=spi1 + BARO_BOARD_CFLAGS += -DBB_MS5611_SLAVE_IDX=SPI_SLAVE3 + BARO_BOARD_SRCS += peripherals/ms5611.c - BARO_BOARD_SRCS += peripherals/ms5611_i2c.c - BARO_BOARD_SRCS += boards/baro_board_ms5611_i2c.c + BARO_BOARD_SRCS += peripherals/ms5611_spi.c + BARO_BOARD_SRCS += boards/baro_board_ms5611_spi.c # apogee baro else ifeq ($(BOARD), apogee) diff --git a/conf/firmwares/subsystems/shared/imu_px4fmu_v2.4.makefile b/conf/firmwares/subsystems/shared/imu_px4fmu_v2.4.makefile new file mode 100644 index 0000000000..be3853280f --- /dev/null +++ b/conf/firmwares/subsystems/shared/imu_px4fmu_v2.4.makefile @@ -0,0 +1,51 @@ +# Hey Emacs, this is a -*- makefile -*- +# +# PX4 Pixhawk IMUconsists of two (internal) IMU's and one internal magneto. Also an optional external magneto +# +# L3GD20 + LSM303D + +include $(CFG_SHARED)/spi_master.makefile + +IMU_PX4FMU_CFLAGS += -DIMU_TYPE_H=\"imu/imu_px4fmu_v2.4.h\" +IMU_PX4FMU_SRCS = $(SRC_SUBSYSTEMS)/imu.c +IMU_PX4FMU_SRCS += $(SRC_SUBSYSTEMS)/imu/imu_px4fmu_v2.4.c + +#L3GD20 gyro +IMU_PX4FMU_SRCS += peripherals/l3gd20_spi.c + +#LSM303D accelero + magneto +IMU_PX4FMU_SRCS += peripherals/lsm303dlhc_spi.c + +# for fixedwing firmware and ap only +ifeq ($(TARGET), ap) + IMU_PX4FMU_CFLAGS += -DUSE_IMU +endif + +# set default SPI device +IMU_PX4FMU_SPI_DEV ?= spi1 +# convert spix to upper/lower case +IMU_PX4FMU_SPI_DEV_UPPER=$(shell echo $(IMU_PX4FMU_SPI_DEV) | tr a-z A-Z) +IMU_PX4FMU_SPI_DEV_LOWER=$(shell echo $(IMU_PX4FMU_SPI_DEV) | tr A-Z a-z) +IMU_PX4FMU_CFLAGS += -DIMU_PX4FMU_SPI_DEV=$(IMU_PX4FMU_SPI_DEV_LOWER) +IMU_PX4FMU_CFLAGS += -DUSE_$(IMU_PX4FMU_SPI_DEV_UPPER) + +#********** L3GD20 *********** +IMU_L3G_SPI_SLAVE_IDX ?= SPI_SLAVE0 +IMU_PX4FMU_CFLAGS += -DIMU_L3G_SPI_SLAVE_IDX=$(IMU_L3G_SPI_SLAVE_IDX) +IMU_PX4FMU_CFLAGS += -DUSE_$(IMU_L3G_SPI_SLAVE_IDX) + +#********** LSM303dlhc *********** +IMU_LSM_SPI_SLAVE_IDX ?= SPI_SLAVE1 +IMU_PX4FMU_CFLAGS += -DIMU_LSM_SPI_SLAVE_IDX=$(IMU_LSM_SPI_SLAVE_IDX) +IMU_PX4FMU_CFLAGS += -DUSE_$(IMU_LSM_SPI_SLAVE_IDX) + +# add it for all targets except sim, fbw and nps +ifeq (,$(findstring $(TARGET),sim fbw nps)) +$(TARGET).CFLAGS += $(IMU_PX4FMU_CFLAGS) +$(TARGET).srcs += $(IMU_PX4FMU_SRCS) +endif + +# +# NPS simulator +# +include $(CFG_SHARED)/imu_nps.makefile diff --git a/conf/modules/mag_hmc58xx.xml b/conf/modules/mag_hmc58xx.xml index 0205a3554d..c368aea194 100644 --- a/conf/modules/mag_hmc58xx.xml +++ b/conf/modules/mag_hmc58xx.xml @@ -9,6 +9,12 @@ + + + + + +
diff --git a/sw/airborne/boards/px4fmu/baro_board.h b/sw/airborne/boards/px4fmu/baro_board.h index 7cdd39b856..11d42e601a 100644 --- a/sw/airborne/boards/px4fmu/baro_board.h +++ b/sw/airborne/boards/px4fmu/baro_board.h @@ -8,7 +8,7 @@ #define BOARDS_PX4FMU_BARO_H // only for printing the baro type during compilation -#define BARO_BOARD BARO_BOARD_MS5611_I2C +#define BARO_BOARD BARO_BOARD_MS5611_SPI extern void baro_event(void); diff --git a/sw/airborne/modules/sensors/mag_hmc58xx.c b/sw/airborne/modules/sensors/mag_hmc58xx.c index f38e046fbb..d1b26aa25d 100644 --- a/sw/airborne/modules/sensors/mag_hmc58xx.c +++ b/sw/airborne/modules/sensors/mag_hmc58xx.c @@ -39,6 +39,15 @@ #ifndef HMC58XX_CHAN_Z #define HMC58XX_CHAN_Z 2 #endif +#ifndef HMC58XX_CHAN_X_SIGN +#define HMC58XX_CHAN_X_SIGN + +#endif +#ifndef HMC58XX_CHAN_Y_SIGN +#define HMC58XX_CHAN_Y_SIGN + +#endif +#ifndef HMC58XX_CHAN_Z_SIGN +#define HMC58XX_CHAN_Z_SIGN + +#endif #if MODULE_HMC58XX_UPDATE_AHRS #include "subsystems/imu.h" @@ -68,9 +77,9 @@ void mag_hmc58xx_module_event(void) // set channel order struct Int32Vect3 mag = { - (int32_t)(mag_hmc58xx.data.value[HMC58XX_CHAN_X]), - (int32_t)(mag_hmc58xx.data.value[HMC58XX_CHAN_Y]), - (int32_t)(mag_hmc58xx.data.value[HMC58XX_CHAN_Z]) + HMC58XX_CHAN_X_SIGN(int32_t)(mag_hmc58xx.data.value[HMC58XX_CHAN_X]), + HMC58XX_CHAN_Y_SIGN(int32_t)(mag_hmc58xx.data.value[HMC58XX_CHAN_Y]), + HMC58XX_CHAN_Z_SIGN(int32_t)(mag_hmc58xx.data.value[HMC58XX_CHAN_Z]) }; // unscaled vector VECT3_COPY(imu.mag_unscaled, mag); @@ -91,9 +100,9 @@ void mag_hmc58xx_module_event(void) void mag_hmc58xx_report(void) { struct Int32Vect3 mag = { - (int32_t)(mag_hmc58xx.data.value[HMC58XX_CHAN_X]), - (int32_t)(mag_hmc58xx.data.value[HMC58XX_CHAN_Y]), - (int32_t)(mag_hmc58xx.data.value[HMC58XX_CHAN_Z]) + HMC58XX_CHAN_X_SIGN(int32_t)(mag_hmc58xx.data.value[HMC58XX_CHAN_X]), + HMC58XX_CHAN_Y_SIGN(int32_t)(mag_hmc58xx.data.value[HMC58XX_CHAN_Y]), + HMC58XX_CHAN_Z_SIGN(int32_t)(mag_hmc58xx.data.value[HMC58XX_CHAN_Z]) }; DOWNLINK_SEND_IMU_MAG_RAW(DefaultChannel, DefaultDevice, &mag.x, &mag.y, &mag.z); } diff --git a/sw/airborne/peripherals/l3gd20_regs.h b/sw/airborne/peripherals/l3gd20_regs.h index 6dd9a570a5..8247c58de6 100644 --- a/sw/airborne/peripherals/l3gd20_regs.h +++ b/sw/airborne/peripherals/l3gd20_regs.h @@ -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 }; diff --git a/sw/airborne/peripherals/l3gd20_spi.h b/sw/airborne/peripherals/l3gd20_spi.h index 0bff591ee5..2b29f7ff66 100644 --- a/sw/airborne/peripherals/l3gd20_spi.h +++ b/sw/airborne/peripherals/l3gd20_spi.h @@ -51,7 +51,7 @@ struct L3gd20_Spi { }; // Functions -extern void l3gd20_spi_init(struct L3gd20_Spi *l3g, struct spi_periph *spi_p, uint8_t addr); +extern void l3gd20_spi_init(struct L3gd20_Spi *l3g, struct spi_periph *spi_p, uint8_t slave_idx); extern void l3gd20_spi_start_configure(struct L3gd20_Spi *l3g); extern void l3gd20_spi_read(struct L3gd20_Spi *l3g); extern void l3gd20_spi_event(struct L3gd20_Spi *l3g); diff --git a/sw/airborne/peripherals/lsm303dlhc.h b/sw/airborne/peripherals/lsm303dlhc.h index 2f4da82ddd..5122d133b8 100644 --- a/sw/airborne/peripherals/lsm303dlhc.h +++ b/sw/airborne/peripherals/lsm303dlhc.h @@ -25,85 +25,74 @@ * * Driver for ST LSM303DLHC 3D accelerometer and magnetometer. */ - -#ifndef LSM303DLHC_H -#define LSM303DLHC_H +#ifndef LSM303_H +#define LSM303_H #include "std.h" -#include "mcu_periph/i2c.h" -#include "math/pprz_algebra_int.h" - /* Address and register definitions */ #include "peripherals/lsm303dlhc_regs.h" +/* LSM303DLHC default conf */ +#ifndef LSM303DLHC_DEFAULT_AODR +#define LSM303DLHC_DEFAULT_AODR (0x01 << 4) //acc 3125 Hz +#endif + +#ifndef LSM303DLHC_DEFAULT_AFS +#define LSM303DLHC_DEFAULT_AFS (0x04 <<3) // acc +- 16G +#endif + +#ifndef LSM303DLHC_DEFAULT_MODR +#define LSM303DLHC_DEFAULT_MODR (0x5 << 2) // Magneto Data Output Rate (100Hz) +#endif + +#ifndef LSM303DLHC_DEFAULT_MFS +#define LSM303DLHC_DEFAULT_MFS (0x0 << 5) // Magneto gain configuration (+/- 2 Gauss) +#endif + +#ifndef LSM303DLHC_DEFAULT_MD +#define LSM303DLHC_DEFAULT_MD (0x00 << 0) // Magneto continious conversion mode +#endif + struct Lsm303dlhcAccConfig { - uint8_t rate; ///< Data Output Rate Bits(6 -> 50Hz with HMC5843, 75Hz with HMC5883) - uint8_t lp_mode; ///< Low power mode - uint8_t scale; ///< full scale selection - uint8_t hres; ///< high resolution output mode + uint8_t rate; ///< Data Output Rate (Hz) + uint8_t scale; ///< full scale selection (m/s²) }; struct Lsm303dlhcMagConfig { - uint8_t rate; ///< Data Output Rate Bits(6 -> 50Hz with HMC5843, 75Hz with HMC5883) - uint8_t gain; ///< Gain configuration (1 -> +- 1 Gauss) + uint8_t rate; ///< Data Output Rate Bits (Hz) + uint8_t scale; ///< Full scale gain configuration (Gauss) uint8_t mode; ///< Measurement mode }; /** config status states */ -enum Lsm303dlhcAccConfStatus { - LSM_CONF_ACC_UNINIT, - LSM_CONF_ACC_CTRL_REG4_A, - LSM_CONF_ACC_CTRL_REG1_A, - LSM_CONF_ACC_CTRL_REG3_A, - LSM_CONF_ACC_DONE +enum Lsm303dlhcConfStatus { + LSM_CONF_UNINIT, + LSM_CONF_WHO_AM_I, + LSM_CONF_CTRL_REG1, + LSM_CONF_CTRL_REG2, + LSM_CONF_CTRL_REG3, + LSM_CONF_CTRL_REG4, + LSM_CONF_CTRL_REG5, + LSM_CONF_CTRL_REG6, + LSM_CONF_CTRL_REG7, + LSM_CONF_DONE }; -/** config status states */ -enum Lsm303dlhcMagConfStatus { - LSM_CONF_MAG_UNINIT, - LSM_CONF_MAG_CRA_REG_M, - LSM_CONF_MAG_CRB_REG_M, - LSM_CONF_MAG_MR_REG_M, - LSM_CONF_MAG_DONE +enum Lsm303dlhcTarget { + LSM_TARGET_ACC, + LSM_TARGET_MAG }; -struct Lsm303dlhc { - struct i2c_periph *i2c_p; - struct i2c_transaction i2c_trans; - bool initialized; ///< config done flag - union { - enum Lsm303dlhcAccConfStatus acc; ///< init status - enum Lsm303dlhcMagConfStatus mag; ///< init status - } init_status; - volatile bool data_available; ///< data ready flag - union { - struct Int16Vect3 vect; ///< data vector in acc coordinate system - int16_t value[3]; ///< data values accessible by channel index - } data; - union { - struct Lsm303dlhcAccConfig acc; - struct Lsm303dlhcMagConfig mag; - } config; -}; - - - -// TODO IRQ handling - -// Functions -extern void lsm303dlhc_init(struct Lsm303dlhc *lsm, struct i2c_periph *i2c_p, uint8_t addr); -extern void lsm303dlhc_start_configure(struct Lsm303dlhc *lsm); -extern void lsm303dlhc_read(struct Lsm303dlhc *lsm); -extern void lsm303dlhc_event(struct Lsm303dlhc *lsm); - -/// convenience function: read or start configuration if not already initialized -static inline void lsm303dlhc_periodic(struct Lsm303dlhc *lsm) +static inline void lsm303dlhc_acc_set_default_config(struct Lsm303dlhcAccConfig *c) { - if (lsm->initialized) { - lsm303dlhc_read(lsm); - } else { - lsm303dlhc_start_configure(lsm); - } + c->rate = LSM303DLHC_DEFAULT_AODR; + c->scale = LSM303DLHC_DEFAULT_AFS; } -#endif /* LSM303DLHC_H */ +static inline void lsm303dlhc_mag_set_default_config(struct Lsm303dlhcMagConfig *c) +{ + c->rate = LSM303DLHC_DEFAULT_MODR; + c->scale = LSM303DLHC_DEFAULT_MFS; + c->mode = LSM303DLHC_DEFAULT_MD; +} +#endif // LSM303_H diff --git a/sw/airborne/peripherals/lsm303dlhc.c b/sw/airborne/peripherals/lsm303dlhc_i2c.c similarity index 79% rename from sw/airborne/peripherals/lsm303dlhc.c rename to sw/airborne/peripherals/lsm303dlhc_i2c.c index aa34d312c6..f8cb8a3443 100644 --- a/sw/airborne/peripherals/lsm303dlhc.c +++ b/sw/airborne/peripherals/lsm303dlhc_i2c.c @@ -21,65 +21,22 @@ */ /** - * @file peripherals/lsm303dlhc.c + * @file peripherals/lsm303dlhc_i2c.c * * Driver for ST LSM303DLHC 3D accelerometer and magnetometer. + * UNTESTED */ -#include "peripherals/lsm303dlhc.h" +#include "peripherals/lsm303dlhc_i2c.h" #include "std.h" -/* LSM303DLHC default conf */ -#ifndef LSM303DLHC_DEFAULT_ODR -#define LSM303DLHC_DEFAULT_ODR 0x90 //90 //normal 1.344khz, low power 5.376khz -#endif - -#ifndef LSM303DLHC_DEFAULT_LP -#define LSM303DLHC_DEFAULT_LP 0x00 //low power disabled -#endif - -#ifndef LSM303DLHC_DEFAULT_FS -#define LSM303DLHC_DEFAULT_FS 0x00 // +- 2G -#endif - -#ifndef LSM303DLHC_DEFAULT_HR -#define LSM303DLHC_DEFAULT_HR 0x04 // high res enabled -#endif - -#ifndef LSM303DLHC_DEFAULT_DO -#define LSM303DLHC_DEFAULT_DO (0x6 << 2) // Data Output Rate (75Hz) -#endif - -#ifndef LSM303DLHC_DEFAULT_GN -#define LSM303DLHC_DEFAULT_GN (0x1 << 5) // Gain configuration (1 -> +- 1.3 Gauss) -#endif - -#ifndef LSM303DLHC_DEFAULT_MD -#define LSM303DLHC_DEFAULT_MD 0x00 // Continious conversion mode -#endif - -static void lsm303dlhc_acc_set_default_config(struct Lsm303dlhcAccConfig *c) -{ - c->rate = LSM303DLHC_DEFAULT_ODR; - c->lp_mode = LSM303DLHC_DEFAULT_LP; - c->scale = LSM303DLHC_DEFAULT_FS; - c->hres = LSM303DLHC_DEFAULT_HR; -} - -static void lsm303dlhc_mag_set_default_config(struct Lsm303dlhcMagConfig *c) -{ - c->rate = (LSM303DLHC_DEFAULT_DO & LSM303DLHC_DO0_MASK); - c->gain = (LSM303DLHC_DEFAULT_GN & LSM303DLHC_GN_MASK); - c->mode = (LSM303DLHC_DEFAULT_MD & LSM303DLHC_MD_MASK); -} - /** * Initialize Lsm303dlhc struct and set default config options. * @param lsm Lsm303dlhc struct * @param i2c_p I2C peripheral to use * @param addr I2C address of Lsm303dlhc */ -void lsm303dlhc_init(struct Lsm303dlhc *lsm, struct i2c_periph *i2c_p, uint8_t addr) +void lsm303dlhc_i2c_init(struct Lsm303dlhc_i2c *lsm, struct i2c_periph *i2c_p, uint8_t addr) { /* set i2c_peripheral */ lsm->i2c_p = i2c_p; @@ -97,7 +54,7 @@ void lsm303dlhc_init(struct Lsm303dlhc *lsm, struct i2c_periph *i2c_p, uint8_t a lsm->initialized = false; } -static void lsm303dlhc_i2c_tx_reg(struct Lsm303dlhc *lsm, uint8_t reg, uint8_t val) +static void lsm303dlhc_i2c_tx_reg(struct Lsm303dlhc_I2c *lsm, uint8_t reg, uint8_t val) { lsm->i2c_trans.type = I2CTransTx; lsm->i2c_trans.buf[0] = reg; @@ -108,7 +65,7 @@ static void lsm303dlhc_i2c_tx_reg(struct Lsm303dlhc *lsm, uint8_t reg, uint8_t v } /// Configuration function called once before normal use -static void lsm303dlhc_send_config(struct Lsm303dlhc *lsm) +static void lsm303dlhc_i2c_send_config(struct Lsm303dlhc_I2c *lsm) { if (lsm->i2c_trans.slave_addr == LSM303DLHC_ACC_ADDR) { switch (lsm->init_status.acc) { @@ -132,7 +89,7 @@ static void lsm303dlhc_send_config(struct Lsm303dlhc *lsm) case LSM_CONF_ACC_DONE: lsm->initialized = true; lsm->i2c_trans.status = I2CTransDone; - lsm303dlhc_read(lsm); + lsm303dlhc_i2c_read(lsm); break; default: break; @@ -162,7 +119,7 @@ static void lsm303dlhc_send_config(struct Lsm303dlhc *lsm) } // Configure -void lsm303dlhc_start_configure(struct Lsm303dlhc *lsm) +void lsm303dlhc_i2c_start_configure(struct Lsm303dlhc_I2c *lsm) { if (lsm->i2c_trans.slave_addr == LSM303DLHC_ACC_ADDR) { if (lsm->init_status.acc == LSM_CONF_ACC_UNINIT) { @@ -175,14 +132,14 @@ void lsm303dlhc_start_configure(struct Lsm303dlhc *lsm) if (lsm->init_status.mag == LSM_CONF_MAG_UNINIT) { lsm->init_status.mag++; if (lsm->i2c_trans.status == I2CTransSuccess || lsm->i2c_trans.status == I2CTransDone) { - lsm303dlhc_send_config(lsm); + lsm303dlhc_i2c_send_config(lsm); } } } } // Normal reading -void lsm303dlhc_read(struct Lsm303dlhc *lsm) +void lsm303dlhc_i2c_read(struct Lsm303dlhc *lsm) { if (lsm->i2c_trans.slave_addr == LSM303DLHC_ACC_ADDR) { //if ((lsm->init_status.acc == LSM_CONF_ACC_CLR_INT_READ) && (lsm->i2c_trans.status == I2CTransDone)){ @@ -206,7 +163,7 @@ void lsm303dlhc_read(struct Lsm303dlhc *lsm) #define Int16FromBuf(_buf,_idx) ((int16_t)((_buf[_idx+1]<<8) | _buf[_idx])) -void lsm303dlhc_event(struct Lsm303dlhc *lsm) +void lsm303dlhc_i2c_event(struct Lsm303dlhc *lsm) { if (lsm->initialized) { if (lsm->i2c_trans.status == I2CTransFailed) { diff --git a/sw/airborne/peripherals/lsm303dlhc_i2c.h b/sw/airborne/peripherals/lsm303dlhc_i2c.h new file mode 100644 index 0000000000..08e4d4dae3 --- /dev/null +++ b/sw/airborne/peripherals/lsm303dlhc_i2c.h @@ -0,0 +1,78 @@ +/* + * Copyright (C) 2011 Gautier Hattenberger + * 2013 Felix Ruess + * + * 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 peripherals/lsm303dlhc.h + * + * Driver for ST LSM303DLHC 3D accelerometer and magnetometer. + * UNTESTED + */ + +#ifndef LSM303DLHC_H +#define LSM303DLHC_H + +#include "std.h" +#include "mcu_periph/i2c.h" +#include "math/pprz_algebra_int.h" + +#include "peripherals/lsm303dlhc.h" + +#warning "The Lsm303dlhc I2C driver has not been tested. Use with caution." + +struct Lsm303dlhc_i2c { + struct i2c_periph *i2c_p; + struct i2c_transaction i2c_trans; + bool initialized; ///< config done flag + union { + enum Lsm303dlhcAccConfStatus acc; ///< init status + enum Lsm303dlhcMagConfStatus mag; ///< init status + } init_status; + volatile bool data_available; ///< data ready flag + union { + struct Int16Vect3 vect; ///< data vector in acc coordinate system + int16_t value[3]; ///< data values accessible by channel index + } data_accel; + union { + struct Lsm303dlhcAccConfig acc; + struct Lsm303dlhcMagConfig mag; + } config; +}; + +// TODO IRQ handling + +// Functions +extern void lsm303dlhc_i2c_init(struct Lsm303dlhc_i2c *lsm, struct i2c_periph *i2c_p, uint8_t addr); +extern void lsm303dlhc_i2c_start_configure(struct Lsm303dlhc_i2c *lsm); +extern void lsm303dlhc_i2c_read(struct Lsm303dlhc_i2c *lsm); +extern void lsm303dlhc_i2c_event(struct Lsm303dlhc_i2c *lsm); + +/// convenience function: read or start configuration if not already initialized +static inline void lsm303dlhc_i2c_periodic(struct Lsm303dlhc_i2c *lsm) +{ + if (lsm->initialized) { + lsm303dlhc_i2c_read(lsm); + } else { + lsm303dlhc_i2c_start_configure(lsm); + } +} + +#endif /* LSM303DLHC_H */ diff --git a/sw/airborne/peripherals/lsm303dlhc_regs.h b/sw/airborne/peripherals/lsm303dlhc_regs.h index 1a325ee0ad..c93973666f 100644 --- a/sw/airborne/peripherals/lsm303dlhc_regs.h +++ b/sw/airborne/peripherals/lsm303dlhc_regs.h @@ -32,13 +32,43 @@ #define LSM303DLHC_MAG_ADDR 0x3C /* Registers */ -#define LSM303DLHC_REG_CTRL_REG1_A 0x20 -#define LSM303DLHC_REG_CTRL_REG2_A 0x21 -#define LSM303DLHC_REG_CTRL_REG3_A 0x22 -#define LSM303DLHC_REG_CTRL_REG4_A 0x23 -#define LSM303DLHC_REG_CTRL_REG5_A 0x24 -#define LSM303DLHC_REG_CTRL_REG6_A 0x25 -#define LSM303DLHC_REG_REF_DATA_CAP_A 0x26 + +//checked: +#define LSM303DLHC_REG_TEMP_OUT_L_M 0x05 +#define LSM303DLHC_REG_TEMP_OUT_H_M 0x06 + +#define LSM303DLHC_REG_STATUS_REG_M 0x07 +#define LSM303DLHC_REG_OUT_X_L_M 0x08 +#define LSM303DLHC_REG_OUT_X_H_M 0x09 +#define LSM303DLHC_REG_OUT_Y_L_M 0x0A +#define LSM303DLHC_REG_OUT_Y_H_M 0x0B +#define LSM303DLHC_REG_OUT_Z_L_M 0x0C +#define LSM303DLHC_REG_OUT_Z_H_M 0x0D + +#define LSM303DLHC_REG_WHO_AM_I 0x0F + +#define LSM303DLHC_REG_INT_CTRL_M 0x12 +#define LSM303DLHC_REG_INT_SRC_M 0x13 +#define LSM303DLHC_REG_INT_THS_L_M 0x14 +#define LSM303DLHC_REG_INT_THS_H_M 0x15 +#define LSM303DLHC_REG_OFFSET_X_L_M 0x16 +#define LSM303DLHC_REG_OFFSET_X_H_M 0x17 +#define LSM303DLHC_REG_OFFSET_Y_L_M 0x18 +#define LSM303DLHC_REG_OFFSET_Y_H_M 0x19 +#define LSM303DLHC_REG_OFFSET_Z_L_M 0x1A +#define LSM303DLHC_REG_OFFSET_Z_H_M 0x1B +#define LSM303DLHC_REG_REFERENCE_X 0x1C +#define LSM303DLHC_REG_REFERENCE_Y 0x1D +#define LSM303DLHC_REG_REFERENCE_Z 0x1E +#define LSM303DLHC_REG_CTRL0 0x1F +#define LSM303DLHC_REG_CTRL1 0x20 +#define LSM303DLHC_REG_CTRL2 0x21 +#define LSM303DLHC_REG_CTRL3 0x22 +#define LSM303DLHC_REG_CTRL4 0x23 +#define LSM303DLHC_REG_CTRL5 0x24 +#define LSM303DLHC_REG_CTRL6 0x25 +#define LSM303DLHC_REG_CTRL7 0x26 + #define LSM303DLHC_REG_STATUS_REG_A 0x27 #define LSM303DLHC_REG_OUT_X_L_A 0x28 #define LSM303DLHC_REG_OUT_X_H_A 0x29 @@ -46,55 +76,68 @@ #define LSM303DLHC_REG_OUT_Y_H_A 0x2B #define LSM303DLHC_REG_OUT_Z_L_A 0x2C #define LSM303DLHC_REG_OUT_Z_H_A 0x2D -#define LSM303DLHC_REG_FIFO_CTRL_REG_A 0x2E -#define LSM303DLHC_REG_FIFO_SRC_REG_A 0x2F -#define LSM303DLHC_REG_INT1_CFG_A 0x30 -#define LSM303DLHC_REG_INT1_SRC_A 0x31 -#define LSM303DLHC_REG_INT1_THS_A 0x32 -#define LSM303DLHC_REG_INT1_DURATION_A 0x33 -#define LSM303DLHC_REG_INT2_CFG_A 0x34 -#define LSM303DLHC_REG_INT2_SRC_A 0x35 -#define LSM303DLHC_REG_INT2_THS_A 0x36 -#define LSM303DLHC_REG_INT2_DURATION_A 0x37 -#define LSM303DLHC_REG_CLICK_CFG_A 0x38 -#define LSM303DLHC_REG_CLICK_SRC_A 0x39 -#define LSM303DLHC_REG_CLICK_THS_A 0x3A -#define LSM303DLHC_REG_TIME_LIMIT_A 0x3B -#define LSM303DLHC_REG_TIME_LATENCY_A 0x3C -#define LSM303DLHC_REG_TIME_WINDOW_A 0x3D -#define LSM303DLHC_REG_CRA_REG_M 0x00 -#define LSM303DLHC_REG_CRB_REG_M 0x01 -#define LSM303DLHC_REG_MR_REG_M 0x02 -#define LSM303DLHC_REG_OUT_X_H_M 0x03 -#define LSM303DLHC_REG_OUT_X_L_M 0x04 -#define LSM303DLHC_REG_OUT_Z_H_M 0x05 -#define LSM303DLHC_REG_OUT_Z_L_M 0x06 -#define LSM303DLHC_REG_OUT_Y_H_M 0x07 -#define LSM303DLHC_REG_OUT_Y_L_M 0x08 -#define LSM303DLHC_REG_SR_REG_M 0x09 -#define LSM303DLHC_REG_IRA_REG_M 0x0A -#define LSM303DLHC_REG_IRB_REG_M 0x0B -#define LSM303DLHC_REG_IRC_REG_M 0x0C -#define LSM303DLHC_REG_TEMP_OUT_H_M 0x31 -#define LSM303DLHC_REG_TEMP_OUT_L_M 0x32 + +#define LSM303DLHC_REG_FIFO_CTRL 0x2E +#define LSM303DLHC_REG_FIFO_SRC 0x2F +#define LSM303DLHC_REG_INT_CFG 0x30 +#define LSM303DLHC_REG_INT_SRC1 0x31 +#define LSM303DLHC_REG_INT_THS1 0x32 +#define LSM303DLHC_REG_INT_DUR1 0x33 +#define LSM303DLHC_REG_INT_CFG2 0x34 +#define LSM303DLHC_REG_INT_SRC2 0x35 +#define LSM303DLHC_REG_INT_THS2 0x36 +#define LSM303DLHC_REG_INT_DUR2 0x37 +#define LSM303DLHC_REG_CLICK_CFG 0x38 +#define LSM303DLHC_REG_CLICK_SRC 0x39 +#define LSM303DLHC_REG_CLICK_THS 0x3A +#define LSM303DLHC_REG_TIME_LIMIT 0x3B +#define LSM303DLHC_REG_TIME_LATENCY 0x3C +#define LSM303DLHC_REG_TIME_WINDOW 0x3D +#define LSM303DLHC_ACT_THS 0x3E +#define LSM303DLHC_ACT_DUR 0x3F + +#define LSM303DLHC_REG_STATUS_ZYXADA 0x08 +#define LSM303DLHC_REG_STATUS_ZYXMDA 0x08 + +#define LSM303DLHC_WHO_I_AM 0x49 /* Bit definitions */ -#define LSM303DLHC_ODR_MASK 0xF0 -#define LSM303DLHC_LPen (1 << 3) -#define LSM303DLHC_Xen (1 << 0) -#define LSM303DLHC_Yen (1 << 1) -#define LSM303DLHC_Zen (1 << 2) +//CTRL1 +#define LSM303DLHC_Xen (0x01 << 0) +#define LSM303DLHC_Yen (0x01 << 1) +#define LSM303DLHC_Zen (0x01 << 2) +#define LSM303DLHC_BDU (0x01 << 3) +#define LSM303DLHC_AODR_MASK (0x0F << 4) -#define LSM303DLHC_FS_MASK 0x30 -#define LSM303DLHC_HR (1 << 3) -#define LSM303DLHC_BDU (1 << 7) +//CTRL2 +#define LSM303DLHC_SIM (0x01 << 0) +#define LSM303DLHC_AST (0x01 << 1) +#define LSM303DLHC_FS_MASK (0x07 << 3) +#define LSM303DLHC_ABW_MASK (0x03 << 6) -#define LSM303DLHC_I1_DRDY1 (1 << 4) +//CTRL3 +#define LSM303DLHC_I1_DRDY_A (0x01 << 2) +//TODO: more CTRL3 regs -#define LSM303DLHC_DO0_MASK 0x1C -#define LSM303DLHC_GN_MASK 0xE0 -#define LSM303DLHC_MD_MASK 0x03 +//CTRL4 +#define LSM303DLHC_I2_DRDY_A (0x01 << 3) +#define LSM303DLHC_I2_DRDY_M (0x01 << 2) +//TODO: more CTRL4 regs +//CTRL5 +#define LSM303DLHC_TEMP_EN (0x01 << 7) +#define LSM303DLHC_M_RES (0x07 << 5) // only two modes, so no mask +#define LSM303DLHC_M_ODR_MASK (0x15 << 2) +#define LSM303DLHC_M_LIR_MASK (0x7 << 0) +//CTRL6 +#define LSM303DLHC_MFS_MASK (0x07 << 5) + +//CTRL7 +#define LSM303DLHC_AHPM_MASK (0x07 << 6) +#define LSM303DLHC_AFDS (0x01 << 5) +#define LSM303DLHC_T_ONLY (0x01 << 4) +#define LSM303DLHC_MLP (0x01 << 2) +#define LSM303DLHC_MD_MASK (0x07 << 0) #endif // LSM303DLHC_REGS_H diff --git a/sw/airborne/peripherals/lsm303dlhc_spi.c b/sw/airborne/peripherals/lsm303dlhc_spi.c new file mode 100644 index 0000000000..b321d2443f --- /dev/null +++ b/sw/airborne/peripherals/lsm303dlhc_spi.c @@ -0,0 +1,222 @@ +/* + * Copyright (C) 2011 Gautier Hattenberger + * 2013 Felix Ruess + * + * 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 peripherals/lsm303dlhc_spi.c + * + * Driver for ST LSM303DLHC 3D accelerometer and magnetometer. + */ + +#include "peripherals/lsm303dlhc_spi.h" +#include "std.h" + +void lsm303dlhc_spi_init(struct Lsm303dlhc_Spi *lsm, struct spi_periph *spi_p, uint8_t slave_idx, + enum Lsm303dlhcTarget target) +{ + /* set spi_peripheral */ + lsm->spi_p = spi_p; + + /* set internal target mag/acc*/ + lsm->target = target; + + /* configure spi transaction */ + lsm->spi_trans.cpol = SPICpolIdleHigh; + lsm->spi_trans.cpha = SPICphaEdge2; + lsm->spi_trans.dss = SPIDss8bit; + lsm->spi_trans.bitorder = SPIMSBFirst; + lsm->spi_trans.cdiv = SPIDiv64; + + lsm->spi_trans.select = SPISelectUnselect; + lsm->spi_trans.slave_idx = slave_idx; + lsm->spi_trans.output_length = 2; + lsm->spi_trans.input_length = 8; + // callback currently unused + lsm->spi_trans.before_cb = NULL; + lsm->spi_trans.after_cb = NULL; + lsm->spi_trans.input_buf = &(lsm->rx_buf[0]); + lsm->spi_trans.output_buf = &(lsm->tx_buf[0]); + + /* set inital status: Success or Done */ + lsm->spi_trans.status = SPITransDone; + + /* set default LSM303D config options */ + lsm303dlhc_acc_set_default_config(&(lsm->config.acc)); + lsm303dlhc_mag_set_default_config(&(lsm->config.mag)); + lsm->init_status = LSM_CONF_UNINIT; + + lsm->initialized = FALSE; + lsm->data_available_acc = FALSE; + lsm->data_available_mag = FALSE; +} + +static void lsm303dlhc_spi_tx_reg(struct Lsm303dlhc_Spi *lsm, uint8_t reg, uint8_t val) +{ + lsm->spi_trans.output_length = 2; + lsm->spi_trans.input_length = 0; + lsm->tx_buf[0] = reg; + lsm->tx_buf[1] = val; + spi_submit(lsm->spi_p, &(lsm->spi_trans)); +} + +/// Configuration function called once before normal use +static void lsm303dlhc_spi_send_config(struct Lsm303dlhc_Spi *lsm) +{ + if (lsm->target == + LSM_TARGET_ACC) { // the complete config done below currently is one shot for both acc and mag. So, only do it for one of the devices. + switch (lsm->init_status) { + case LSM_CONF_WHO_AM_I: + /* query device id */ + lsm->spi_trans.output_length = 1; + lsm->spi_trans.input_length = 2; + /* set read bit then reg address */ + lsm->tx_buf[0] = (1 << 7 | LSM303DLHC_REG_WHO_AM_I); + if (spi_submit(lsm->spi_p, &(lsm->spi_trans))) { + if (lsm->rx_buf[1] == LSM303DLHC_WHO_I_AM) { + lsm->init_status++; + } + } + break; + case LSM_CONF_CTRL_REG1: + lsm303dlhc_spi_tx_reg(lsm, LSM303DLHC_REG_CTRL1, + (lsm->config.acc.rate & LSM303DLHC_AODR_MASK) | + LSM303DLHC_Xen | LSM303DLHC_Yen | LSM303DLHC_Zen); + lsm->init_status++; + break; + case LSM_CONF_CTRL_REG2: + lsm303dlhc_spi_tx_reg(lsm, LSM303DLHC_REG_CTRL2, (lsm->config.acc.scale & LSM303DLHC_FS_MASK)); + lsm->init_status++; + break; + case LSM_CONF_CTRL_REG3: + lsm303dlhc_spi_tx_reg(lsm, LSM303DLHC_REG_CTRL3, LSM303DLHC_I1_DRDY_A); + lsm->init_status++; + break; + case LSM_CONF_CTRL_REG4: + lsm303dlhc_spi_tx_reg(lsm, LSM303DLHC_REG_CTRL4, LSM303DLHC_I2_DRDY_M); + lsm->init_status++; + return; + break; + case LSM_CONF_CTRL_REG5: + lsm303dlhc_spi_tx_reg(lsm, LSM303DLHC_REG_CTRL5, + (lsm->config.mag.rate & LSM303DLHC_M_ODR_MASK)); + lsm->init_status++; + return; + break; + case LSM_CONF_CTRL_REG6: + lsm303dlhc_spi_tx_reg(lsm, LSM303DLHC_REG_CTRL6, + (lsm->config.mag.scale & LSM303DLHC_MFS_MASK)); + lsm->init_status++; + break; + case LSM_CONF_CTRL_REG7: + lsm303dlhc_spi_tx_reg(lsm, LSM303DLHC_REG_CTRL7, (lsm->config.mag.mode & LSM303DLHC_AHPM_MASK)); + lsm->init_status++; + break; + case LSM_CONF_DONE: + lsm->initialized = TRUE; + lsm->spi_trans.status = SPITransDone; + return; + break; + default: + break; + } + } else { + lsm->initialized = TRUE; + lsm->spi_trans.status = SPITransDone; + } +} + +// Configure +void lsm303dlhc_spi_start_configure(struct Lsm303dlhc_Spi *lsm) +{ + if (lsm->init_status == LSM_CONF_UNINIT) { + lsm->init_status++; + if (lsm->spi_trans.status == SPITransSuccess || lsm->spi_trans.status == SPITransDone) { + lsm303dlhc_spi_send_config(lsm); + } + } +} + +// Normal reading +void lsm303dlhc_spi_read(struct Lsm303dlhc_Spi *lsm) +{ + if (lsm->target == LSM_TARGET_ACC) { + if (!(lsm->initialized) || (lsm->initialized && lsm->spi_trans.status == SPITransDone)) { + lsm->spi_trans.output_length = 1; + lsm->spi_trans.input_length = 8; + /* set read bit and multiple byte bit, then address */ + lsm->tx_buf[0] = LSM303DLHC_REG_STATUS_REG_A | 1 << 7 | 1 << 6; + spi_submit(lsm->spi_p, &(lsm->spi_trans)); + } + } else { + if (lsm->initialized && lsm->spi_trans.status == SPITransDone) { + lsm->spi_trans.output_length = 1; + lsm->spi_trans.input_length = 8; + /* set read bit and multiple byte bit, then address */ + lsm->tx_buf[0] = LSM303DLHC_REG_STATUS_REG_M | 1 << 7 | 1 << 6; + spi_submit(lsm->spi_p, &(lsm->spi_trans)); + } + } +} + +#define Int16FromBuf(_buf,_idx) ((int16_t)((_buf[_idx+1]<<8) | _buf[_idx])) + +void lsm303dlhc_spi_event(struct Lsm303dlhc_Spi *lsm) +{ + if (lsm->initialized) { + if (lsm->spi_trans.status == SPITransFailed) { + lsm->spi_trans.status = SPITransDone; + } else if (lsm->spi_trans.status == SPITransSuccess) { + if (lsm->target == LSM_TARGET_ACC) { + if (!(lsm->rx_buf[1] & LSM303DLHC_REG_STATUS_ZYXADA)) { + lsm->spi_trans.status = SPITransDone; + return; + } + lsm->data_accel.vect.x = Int16FromBuf(lsm->rx_buf, 2); + lsm->data_accel.vect.y = Int16FromBuf(lsm->rx_buf, 4); + lsm->data_accel.vect.z = Int16FromBuf(lsm->rx_buf, 6); + lsm->data_available_acc = TRUE; + lsm->spi_trans.status = SPITransDone; + } else { //magneto + if (!(lsm->rx_buf[1] & LSM303DLHC_REG_STATUS_ZYXMDA)) { + lsm->spi_trans.status = SPITransDone; + return; + } + lsm->data_mag.vect.x = Int16FromBuf(lsm->rx_buf, 2); + lsm->data_mag.vect.y = Int16FromBuf(lsm->rx_buf, 4); + lsm->data_mag.vect.z = Int16FromBuf(lsm->rx_buf, 6); + lsm->data_available_mag = TRUE; + lsm->spi_trans.status = SPITransDone; + } + } + } else { + if (lsm->init_status != LSM_CONF_UNINIT) { // Configuring but not yet initialized + if (lsm->spi_trans.status == SPITransSuccess || lsm->spi_trans.status == SPITransDone) { + lsm->spi_trans.status = SPITransDone; + lsm303dlhc_spi_send_config(lsm); + } + if (lsm->spi_trans.status == SPITransFailed) { + lsm->init_status--; + lsm->spi_trans.status = SPITransDone; + lsm303dlhc_spi_send_config(lsm); // Retry config (TODO max retry) + } + } + } +} diff --git a/sw/airborne/peripherals/lsm303dlhc_spi.h b/sw/airborne/peripherals/lsm303dlhc_spi.h new file mode 100644 index 0000000000..750d6996ad --- /dev/null +++ b/sw/airborne/peripherals/lsm303dlhc_spi.h @@ -0,0 +1,80 @@ +/* + * Copyright (C) 2011 Gautier Hattenberger + * 2013 Felix Ruess + * + * 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 peripherals/lsm303dlhc.h + * + * Driver for ST LSM303DLHC 3D accelerometer and magnetometer. + */ + +#ifndef LSM303DLHC_H +#define LSM303DLHC_H + +#include "std.h" +#include "mcu_periph/spi.h" +#include "math/pprz_algebra_int.h" +#include "peripherals/lsm303dlhc.h" + +struct Lsm303dlhc_Spi { + struct spi_periph *spi_p; + struct spi_transaction spi_trans; + bool initialized; ///< config done flag + enum Lsm303dlhcTarget target; + volatile uint8_t tx_buf[2]; + volatile uint8_t rx_buf[8]; + enum Lsm303dlhcConfStatus init_status; + volatile bool data_available_acc; ///< data ready flag accelero + volatile bool data_available_mag; ///< data ready flag magneto + union { + struct Int16Vect3 vect; ///< data vector in acc coordinate system + int16_t value[3]; ///< data values accessible by channel index + } data_accel; + union { + struct Int16Vect3 vect; ///< data vector in mag coordinate system + int16_t value[3]; ///< data values accessible by channel index + } data_mag; + union { + struct Lsm303dlhcAccConfig acc; + struct Lsm303dlhcMagConfig mag; + } config; +}; + +// TODO IRQ handling + +// Functions +extern void lsm303dlhc_spi_init(struct Lsm303dlhc_Spi *lsm, struct spi_periph *spi_p, uint8_t slave_idx, + enum Lsm303dlhcTarget target); +extern void lsm303dlhc_spi_start_configure(struct Lsm303dlhc_Spi *lsm); +extern void lsm303dlhc_spi_read(struct Lsm303dlhc_Spi *lsm); +extern void lsm303dlhc_spi_event(struct Lsm303dlhc_Spi *lsm); + +/// convenience function: read or start configuration if not already initialized +static inline void lsm303dlhc_spi_periodic(struct Lsm303dlhc_Spi *lsm) +{ + if (lsm->initialized) { + lsm303dlhc_spi_read(lsm); + } else { + lsm303dlhc_spi_start_configure(lsm); + } +} + +#endif /* LSM303DLHC_H */ diff --git a/sw/airborne/subsystems/abi_sender_ids.h b/sw/airborne/subsystems/abi_sender_ids.h index 17954941c5..b43edf1dfa 100644 --- a/sw/airborne/subsystems/abi_sender_ids.h +++ b/sw/airborne/subsystems/abi_sender_ids.h @@ -237,6 +237,10 @@ #define PX4FLOW_VELOCITY_ID 17 #endif +#ifndef IMU_PX4 +#define IMU_PX4_ID 18 +#endif + /* * IDs of RSSI measurements (message 13) */ diff --git a/sw/airborne/subsystems/imu/imu_px4_defaults.h b/sw/airborne/subsystems/imu/imu_px4_defaults.h new file mode 100644 index 0000000000..7750693a01 --- /dev/null +++ b/sw/airborne/subsystems/imu/imu_px4_defaults.h @@ -0,0 +1,64 @@ +/* + * Copyright (C) 2013 Felix Ruess + * + * 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_px4_defaults.h + * Default sensitivity definitions for the Pixhawk IMU using the l3d20 gyro and lsm303dlc acc. + */ + +#ifndef IMU_PX4_DEFAULTS_H +#define IMU_PX4_DEFAULTS_H + +#include "generated/airframe.h" + +/** default gyro sensitivy and neutral from the datasheet + * L3GD20 has 70e-3 LSB/(deg/s) at 2000deg/s range + * sens = 70e-3 * pi/180 * 2^INT32_RATE_FRAC + * sens = (70e-3 / 180.0f) * pi * 4096 + */ +#if !defined IMU_GYRO_P_SENS & !defined IMU_GYRO_Q_SENS & !defined IMU_GYRO_R_SENS +#define IMU_GYRO_P_SENS_NUM 5004 +#define IMU_GYRO_P_SENS_DEN 1000 +#define IMU_GYRO_Q_SENS_NUM 5004 +#define IMU_GYRO_Q_SENS_DEN 1000 +#define IMU_GYRO_R_SENS_NUM 5004 +#define IMU_GYRO_R_SENS_DEN 1000 +#endif + +/** default accel sensitivy from the datasheet + * LSM303DLHC has 732 LSB/g + * fixed point sens: 9.81 [m/s^2] / 732 [LSB/g] * 2^INT32_ACCEL_FRAC + * sens = 9.81 / 732 * 1024 = 13.72 + */ +#if !defined IMU_ACCEL_X_SENS & !defined IMU_ACCEL_Y_SENS & !defined IMU_ACCEL_Z_SENS + +#define IMU_ACCEL_X_SENS 13.723 +#define IMU_ACCEL_X_SENS_NUM 13723 +#define IMU_ACCEL_X_SENS_DEN 1000 +#define IMU_ACCEL_Y_SENS 13.723 +#define IMU_ACCEL_Y_SENS_NUM 13723 +#define IMU_ACCEL_Y_SENS_DEN 1000 +#define IMU_ACCEL_Z_SENS 13.723 +#define IMU_ACCEL_Z_SENS_NUM 13723 +#define IMU_ACCEL_Z_SENS_DEN 1000 +#endif + +#endif /* IMU_PX4_DEFAULTS_H */ diff --git a/sw/airborne/subsystems/imu/imu_px4fmu_v2.4.c b/sw/airborne/subsystems/imu/imu_px4fmu_v2.4.c new file mode 100644 index 0000000000..364821f42f --- /dev/null +++ b/sw/airborne/subsystems/imu/imu_px4fmu_v2.4.c @@ -0,0 +1,106 @@ +/* + * Copyright (C) 2013-2016 the paparazzi team + * + * 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_px4fmu_v2.4.h + * Driver for pixhawk IMU's. + * L3GD20H + LSM303D (both on spi) + */ + +#include "subsystems/imu.h" +#include "subsystems/abi.h" +#include "mcu_periph/spi.h" +#include "peripherals/l3gd20_regs.h" +#include "peripherals/lsm303dlhc_regs.h" +#include "peripherals/lsm303dlhc_spi.h" + +/* SPI defaults set in subsystem makefile, can be configured from airframe file */ +PRINT_CONFIG_VAR(IMU_LSM_SPI_SLAVE_IDX) +PRINT_CONFIG_VAR(IMU_L3G_SPI_SLAVE_IDX) +PRINT_CONFIG_VAR(IMU_PX4FMU_SPI_DEV) + +struct ImuPX4 imu_px4; + +void imu_impl_init(void) +{ + + /* L3GD20 gyro init */ + /* initialize gyro and set default options */ + l3gd20_spi_init(&imu_px4.l3g, &IMU_PX4FMU_SPI_DEV, IMU_L3G_SPI_SLAVE_IDX); + + /* LSM303dlhc acc + magneto init */ + lsm303dlhc_spi_init(&imu_px4.lsm_acc, &IMU_PX4FMU_SPI_DEV, IMU_LSM_SPI_SLAVE_IDX, LSM_TARGET_ACC); +#if !IMU_PX4_DISABLE_MAG + lsm303dlhc_spi_init(&imu_px4.lsm_mag, &IMU_PX4FMU_SPI_DEV, IMU_LSM_SPI_SLAVE_IDX, LSM_TARGET_MAG); +#endif + +} + +void imu_periodic(void) +{ + l3gd20_spi_periodic(&imu_px4.l3g); + lsm303dlhc_spi_periodic(&imu_px4.lsm_acc); + +#if !IMU_PX4_DISABLE_MAG + /* Read magneto's every 10 times of main freq + * at ~50Hz (main loop for rotorcraft: 512Hz) + */ + RunOnceEvery(10, lsm303dlhc_spi_periodic(&imu_px4.lsm_mag)); +#endif +} + +void imu_px4_event(void) +{ + + uint32_t now_ts = get_sys_time_usec(); + + /* L3GD20 event task */ + l3gd20_spi_event(&imu_px4.l3g); + if (imu_px4.l3g.data_available) { + //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); + } + + /* LSM303dlhc event task */ + lsm303dlhc_spi_event(&imu_px4.lsm_acc); + if (imu_px4.lsm_acc.data_available_acc) { + VECT3_COPY(imu.accel_unscaled, imu_px4.lsm_acc.data_accel.vect); + imu_px4.lsm_acc.data_available_acc = FALSE; + imu_scale_accel(&imu); + AbiSendMsgIMU_ACCEL_INT32(IMU_PX4_ID, now_ts, &imu.accel); + } +#if !IMU_PX4_DISABLE_MAG + 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_PX4_ID, now_ts, &imu.mag); + } +#endif + +} diff --git a/sw/airborne/subsystems/imu/imu_px4fmu_v2.4.h b/sw/airborne/subsystems/imu/imu_px4fmu_v2.4.h new file mode 100644 index 0000000000..325abecf45 --- /dev/null +++ b/sw/airborne/subsystems/imu/imu_px4fmu_v2.4.h @@ -0,0 +1,59 @@ +/* + * Copyright (C) 2013-2016 the paparazzi team + * + * 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_px4fmu_v2.4.h + * Driver for pixhawk IMU's. + * L3GD20H + LSM303D (both on spi) + */ + +#ifndef IMU_PX4FMUV24_H +#define IMU_PX4FMUV24_H + +#include "std.h" +#include "generated/airframe.h" +#include "subsystems/imu.h" + +#include "subsystems/imu/imu_px4_defaults.h" +#include "peripherals/l3gd20_spi.h" +#include "peripherals/lsm303dlhc_spi.h" + +#ifndef IMU_PX4_DISABLE_MAG +#if MODULE_HMC58XX_UPDATE_AHRS +#define IMU_PX4_DISABLE_MAG TRUE +#else +#define IMU_PX4_DISABLE_MAG FALSE +#endif +#endif + +struct ImuPX4 { + struct L3gd20_Spi l3g; + struct Lsm303dlhc_Spi lsm_acc; + struct Lsm303dlhc_Spi lsm_mag; +}; + +extern struct ImuPX4 imu_px4; + +extern void imu_px4_event(void); + +#define ImuEvent imu_px4_event + +#endif /* IMU_PX4FMUV24_H */ diff --git a/sw/airborne/subsystems/intermcu/intermcu_ap.c b/sw/airborne/subsystems/intermcu/intermcu_ap.c index 3ce6e576b8..f46eb7de0d 100644 --- a/sw/airborne/subsystems/intermcu/intermcu_ap.c +++ b/sw/airborne/subsystems/intermcu/intermcu_ap.c @@ -81,8 +81,9 @@ void disable_inter_comm(bool value) void intermcu_set_actuators(pprz_t *command_values, uint8_t ap_mode __attribute__((unused))) { if (!disable_comm) { + uint8_t autopilot_motors_on_tmp = autopilot_motors_on; pprz_msg_send_IMCU_COMMANDS(&(intermcu_transport.trans_tx), intermcu_device, - INTERMCU_AP, &autopilot_motors_on, COMMANDS_NB, command_values); //TODO: Append more status + INTERMCU_AP, &autopilot_motors_on_tmp, COMMANDS_NB, command_values); //TODO: Append more status } }