diff --git a/conf/firmwares/subsystems/lisa_passthrough/imu_b2_v1.2.makefile b/conf/firmwares/subsystems/lisa_passthrough/imu_b2_v1.2.makefile index 9640d6d933..5ee9b72864 100644 --- a/conf/firmwares/subsystems/lisa_passthrough/imu_b2_v1.2.makefile +++ b/conf/firmwares/subsystems/lisa_passthrough/imu_b2_v1.2.makefile @@ -51,8 +51,6 @@ imu_srcs += $(SRC_ARCH)/peripherals/hmc5843_arch.c ifeq ($(ARCH), lpc21) imu_CFLAGS += -DSSP_VIC_SLOT=9 imu_CFLAGS += -DMAX1168_EOC_VIC_SLOT=8 -#FIXME ms2100 not used on this imu -imu_CFLAGS += -DMS2100_DRDY_VIC_SLOT=11 else ifeq ($(ARCH), stm32) imu_CFLAGS += -DUSE_SPI2 imu_CFLAGS += -DUSE_I2C2 diff --git a/conf/firmwares/subsystems/shared/imu_b2_v1.1.makefile b/conf/firmwares/subsystems/shared/imu_b2_v1.1.makefile index 1fbf19a782..c8ad0c8768 100644 --- a/conf/firmwares/subsystems/shared/imu_b2_v1.1.makefile +++ b/conf/firmwares/subsystems/shared/imu_b2_v1.1.makefile @@ -48,6 +48,8 @@ imu_srcs += $(SRC_ARCH)/peripherals/ms2100_arch.c ifeq ($(ARCH), lpc21) imu_CFLAGS += -DUSE_SPI_SLAVE1 +imu_CFLAGS += -DMS2100_SLAVE_IDX=1 +imu_CFLAGS += -DMS2100_SPI_DEV=spi1 imu_CFLAGS += -DMS2100_DRDY_VIC_SLOT=12 else ifeq ($(ARCH), stm32) imu_CFLAGS += -DUSE_SPI_SLAVE4 diff --git a/sw/airborne/arch/lpc21/peripherals/ms2100_arch.c b/sw/airborne/arch/lpc21/peripherals/ms2100_arch.c index 4cab313b18..bac8617ee0 100644 --- a/sw/airborne/arch/lpc21/peripherals/ms2100_arch.c +++ b/sw/airborne/arch/lpc21/peripherals/ms2100_arch.c @@ -20,7 +20,10 @@ * Boston, MA 02111-1307, USA. */ -/** PNI ms2100. +/** + * @file arch/lpc21/peripherals/ms2100_arch.c + * + * LPC21xx specific functions for the ms2100 magnetic sensor from PNI. */ #include "peripherals/ms2100.h" @@ -56,7 +59,7 @@ void ms2100_arch_init( void ) { void EXTINT_ISR(void) { ISR_ENTRY(); /* no, we won't do anything asynchronously, so just notify */ - ms2100_status = MS2100_GOT_EOC; + ms2100.status = MS2100_GOT_EOC; /* clear EINT */ EXTINT = (1< diff --git a/sw/airborne/arch/stm32/peripherals/ms2100_arch.h b/sw/airborne/arch/stm32/peripherals/ms2100_arch.h index c404354ce2..b393cd0c43 100644 --- a/sw/airborne/arch/stm32/peripherals/ms2100_arch.h +++ b/sw/airborne/arch/stm32/peripherals/ms2100_arch.h @@ -19,6 +19,12 @@ * Boston, MA 02111-1307, USA. */ +/** + * @file arch/stm32/peripherals/ms2100_arch.h + * + * STM32 specific functions for the ms2100 magnetic sensor from PNI. + */ + #ifndef MS2100_ARCH_H #define MS2100_ARCH_H diff --git a/sw/airborne/peripherals/ms2100.c b/sw/airborne/peripherals/ms2100.c index 7928c2ae6b..699c0330cb 100644 --- a/sw/airborne/peripherals/ms2100.c +++ b/sw/airborne/peripherals/ms2100.c @@ -30,89 +30,115 @@ #include // for abs -volatile uint8_t ms2100_status; -volatile int16_t ms2100_values[MS2100_NB_AXIS]; -volatile uint8_t ms2100_cur_axe; -struct spi_transaction ms2100_trans; +#define MS2100_DIVISOR_128 2 +#define MS2100_DIVISOR_256 3 +#define MS2100_DIVISOR_512 4 +#define MS2100_DIVISOR_1024 5 -uint8_t ms2100_control_byte; -uint8_t ms2100_val[2]; +#ifndef MS2100_DIVISOR +#define MS2100_DIVISOR MS2100_DIVISOR_1024 +#endif + +// keep stupid global variable for now... +struct Ms2100 ms2100; -void ms2100_init( void ) { +void ms2100_init(struct Ms2100 *ms, struct spi_periph *spi_p, uint8_t slave_idx) { + + /* set spi_peripheral */ + ms->spi_p = spi_p; + + /* configure spi transaction for the request */ + ms->req_trans.slave_idx = slave_idx; + ms->req_trans.select = SPISelectUnselect; + ms->req_trans.cpol = SPICpolIdleLow; + ms->req_trans.cpha = SPICphaEdge1; + ms->req_trans.dss = SPIDss8bit; + ms->req_trans.output_buf = ms->req_buf; + ms->req_trans.output_length = 1; + ms->req_trans.input_buf = NULL; + ms->req_trans.input_length = 0; + // ms2100 has to be reset before each measurement: implemented in ms2100_arch.c + ms->req_trans.before_cb = ms2100_reset_cb; + ms->req_trans.status = SPITransDone; + + /* configure spi transaction to read the result */ + ms->read_trans.slave_idx = slave_idx; + ms->read_trans.select = SPISelectUnselect; + ms->read_trans.cpol = SPICpolIdleLow; + ms->read_trans.cpha = SPICphaEdge1; + ms->read_trans.dss = SPIDss8bit; + ms->read_trans.output_buf = NULL; + ms->read_trans.output_length = 0; + ms->read_trans.input_buf = ms->read_buf; + ms->read_trans.input_length = 2; + ms->read_trans.before_cb = NULL; + ms->read_trans.after_cb = NULL; + ms->read_trans.status = SPITransDone; ms2100_arch_init(); - uint8_t i; - for (i=0; idata.vect); + ms->cur_axe = 0; - // init spi transaction parameters - ms2100_trans.slave_idx = MS2100_SLAVE_IDX; - ms2100_trans.select = SPISelectUnselect; - ms2100_trans.cpol = SPICpolIdleLow; - ms2100_trans.cpha = SPICphaEdge1; - ms2100_trans.dss = SPIDss8bit; - ms2100_trans.status = SPITransDone; - - ms2100_status = MS2100_IDLE; + ms->status = MS2100_IDLE; } -void ms2100_read( void ) { - - /* set SPI transaction */ - ms2100_control_byte = (ms2100_cur_axe+1) << 0 | MS2100_DIVISOR << 4; - ms2100_trans.output_buf = &ms2100_control_byte; - ms2100_trans.output_length = 1; - ms2100_trans.input_buf = 0; - ms2100_trans.input_length = 0; - ms2100_trans.before_cb = ms2100_reset_cb; // implemented in ms2100_arch.c - - spi_submit(&(MS2100_SPI_DEV),&ms2100_trans); - - ms2100_status = MS2100_SENDING_REQ; +/// send request to read next axis +void ms2100_read(struct Ms2100 *ms) { + ms->req_buf[0] = (ms->cur_axe+1) << 0 | MS2100_DIVISOR << 4; + spi_submit(ms->spi_p, &(ms->req_trans)); + ms->status = MS2100_SENDING_REQ; } -void ms2100_event( void ) { - if (ms2100_trans.status == SPITransSuccess) { - if (ms2100_status == MS2100_GOT_EOC) { +#define Int16FromBuf(_buf,_idx) ((int16_t)((_buf[_idx+1]<<8) | _buf[_idx])) + +void ms2100_event(struct Ms2100 *ms) { + // handle request transaction + if (ms->req_trans.status == SPITransDone) { +#ifdef Ms2100HasEOC // stupid hack for now: poll pin to check for EOC with stm32 + if (Ms2100HasEOC()) ms->status = MS2100_GOT_EOC; +#endif + if (ms->status == MS2100_GOT_EOC) { // eoc occurs, submit reading req - // read 2 bytes - ms2100_trans.output_buf = 0; - ms2100_trans.output_length = 0; - ms2100_trans.input_buf = ms2100_val; - ms2100_trans.input_length = 2; - ms2100_trans.before_cb = 0; // no reset when reading values - spi_submit(&(MS2100_SPI_DEV),&ms2100_trans); - - ms2100_status = MS2100_READING_RES; - ms2100_trans.status = SPITransDone; + spi_submit(ms->spi_p, &(ms->read_trans)); + ms->status = MS2100_READING_RES; } - else if (ms2100_status == MS2100_READING_RES) { + } + else if (ms->req_trans.status == SPITransSuccess) { + ms->req_trans.status = SPITransDone; + } + else if (ms->req_trans.status == SPITransFailed) { + ms->status = MS2100_IDLE; + ms->cur_axe = 0; + ms->req_trans.status = SPITransDone; + } + + // handle reading transaction + if (ms->read_trans.status == SPITransSuccess) { + if (ms->status == MS2100_READING_RES) { // store value - int16_t new_val; - new_val = ms2100_trans.input_buf[0] << 8; - new_val += ms2100_trans.input_buf[1]; - if (abs(new_val) < 2000) - ms2100_values[ms2100_cur_axe] = new_val; - ms2100_cur_axe++; - if (ms2100_cur_axe > 2) { - ms2100_cur_axe = 0; - ms2100_status = MS2100_DATA_AVAILABLE; + int16_t new_val = new_val = Int16FromBuf(ms->read_buf,0); + // what is this check about? + if (abs(new_val) < 2000) { + ms->data.value[ms->cur_axe] = new_val; + } + ms->cur_axe++; + if (ms->cur_axe > 2) { + ms->cur_axe = 0; + ms->status = MS2100_DATA_AVAILABLE; } else { - ms2100_status = MS2100_IDLE; + ms->status = MS2100_IDLE; } - ms2100_trans.status = SPITransDone; + ms->read_trans.status = SPITransDone; } - else { /* TODO ? */ } } - else if (ms2100_trans.status == SPITransFailed) { - // TODO is it enough ? - ms2100_status = MS2100_IDLE; - ms2100_trans.status = SPITransDone; + else if (ms->read_trans.status == SPITransFailed) { + ms->status = MS2100_IDLE; + ms->cur_axe = 0; + ms->read_trans.status = SPITransDone; } } diff --git a/sw/airborne/peripherals/ms2100.h b/sw/airborne/peripherals/ms2100.h index fc941f3274..40ffb73d9a 100644 --- a/sw/airborne/peripherals/ms2100.h +++ b/sw/airborne/peripherals/ms2100.h @@ -20,44 +20,52 @@ * Boston, MA 02111-1307, USA. */ +/** + * @file peripherals/ms2100.h + * Driver for the ms2100 magnetic sensor from PNI. + */ + #ifndef MS2100_H #define MS2100_H -/** - * @file peripherals/ms2100.h - * Driver for the ms2100 magnetic sensor from PNI - */ - #include "std.h" +#include "mcu_periph/spi.h" +#include "math/pprz_algebra_int.h" -#define MS2100_NB_AXIS 3 -#ifndef MS2100_SLAVE_IDX -#define MS2100_SLAVE_IDX SPI_SLAVE1 -#endif +enum Ms2100Status { + MS2100_IDLE, + MS2100_SENDING_REQ, + MS2100_GOT_EOC, + MS2100_READING_RES, + MS2100_DATA_AVAILABLE +}; -#ifndef MS2100_SPI_DEV -#define MS2100_SPI_DEV spi1 -#endif +struct Ms2100 { + struct spi_periph *spi_p; + struct spi_transaction req_trans; + struct spi_transaction read_trans; + volatile uint8_t req_buf[1]; ///< SPI buffer for the command byte + volatile uint8_t read_buf[2]; ///< SPI buffer for reading a single axis + volatile enum Ms2100Status status; + volatile uint8_t cur_axe; + union { + struct Int16Vect3 vect; ///< data vector in mag coordinate system + int16_t value[3]; ///< data values accessible by channel index + } data; +}; -extern void ms2100_init( void ); -extern void ms2100_read( void ); -extern void ms2100_event( void ); +// keep gobal var for now... +extern struct Ms2100 ms2100; -#define MS2100_IDLE 0 -#define MS2100_SENDING_REQ 1 -#define MS2100_GOT_EOC 2 -#define MS2100_READING_RES 3 -#define MS2100_DATA_AVAILABLE 4 +extern void ms2100_init(struct Ms2100 *ms, struct spi_periph *spi_p, uint8_t slave_idx); +extern void ms2100_read(struct Ms2100 *ms); +extern void ms2100_event(struct Ms2100 *ms); -extern volatile uint8_t ms2100_status; -extern volatile int16_t ms2100_values[MS2100_NB_AXIS]; -extern volatile uint8_t ms2100_cur_axe; - -#define Ms2100Periodic() { \ - if (ms2100_status == MS2100_IDLE) { \ - ms2100_read(); \ - } \ +static inline void ms2100_periodic(struct Ms2100 *ms) { + if (ms->status == MS2100_IDLE) { + ms2100_read(ms); + } } /* underlying architecture */ @@ -65,14 +73,4 @@ extern volatile uint8_t ms2100_cur_axe; /* must be implemented by underlying architecture */ extern void ms2100_arch_init( void ); -#define MS2100_DIVISOR_128 2 -#define MS2100_DIVISOR_256 3 -#define MS2100_DIVISOR_512 4 -#define MS2100_DIVISOR_1024 5 - -#ifndef MS2100_DIVISOR -#define MS2100_DIVISOR MS2100_DIVISOR_1024 -#endif - - #endif /* MS2100_H */ diff --git a/sw/airborne/subsystems/imu/imu_b2.c b/sw/airborne/subsystems/imu/imu_b2.c index e1392fcdd0..2c573f97d8 100644 --- a/sw/airborne/subsystems/imu/imu_b2.c +++ b/sw/airborne/subsystems/imu/imu_b2.c @@ -34,7 +34,7 @@ void imu_impl_init(void) { max1168_init(); #if defined IMU_B2_MAG_TYPE && IMU_B2_MAG_TYPE == IMU_B2_MAG_MS2100 - ms2100_init(); + ms2100_init(&ms2100, &(MS2100_SPI_DEV), MS2100_SLAVE_IDX); #elif defined IMU_B2_MAG_TYPE && IMU_B2_MAG_TYPE == IMU_B2_MAG_AMI601 ami601_init(); #elif defined IMU_B2_MAG_TYPE && IMU_B2_MAG_TYPE == IMU_B2_MAG_HMC5843 @@ -52,7 +52,7 @@ void imu_periodic(void) { Max1168Periodic(); // read mag #if defined IMU_B2_MAG_TYPE && IMU_B2_MAG_TYPE == IMU_B2_MAG_MS2100 - Ms2100Periodic(); + ms2100_periodic(&ms2100); #endif #if defined IMU_B2_MAG_TYPE && IMU_B2_MAG_TYPE == IMU_B2_MAG_AMI601 RunOnceEvery(10, { ami601_read(); }); diff --git a/sw/airborne/subsystems/imu/imu_b2.h b/sw/airborne/subsystems/imu/imu_b2.h index cf03372205..1ed9a3cf2d 100644 --- a/sw/airborne/subsystems/imu/imu_b2.h +++ b/sw/airborne/subsystems/imu/imu_b2.h @@ -166,15 +166,15 @@ extern struct ImuBooz2 imu_b2; #if defined IMU_B2_MAG_TYPE && IMU_B2_MAG_TYPE == IMU_B2_MAG_MS2100 #include "peripherals/ms2100.h" -#define ImuMagEvent(_mag_handler) { \ - ms2100_event(); \ - if (ms2100_status == MS2100_DATA_AVAILABLE) { \ - imu.mag_unscaled.x = ms2100_values[IMU_MAG_X_CHAN]; \ - imu.mag_unscaled.y = ms2100_values[IMU_MAG_Y_CHAN]; \ - imu.mag_unscaled.z = ms2100_values[IMU_MAG_Z_CHAN]; \ - ms2100_status = MS2100_IDLE; \ - _mag_handler(); \ - } \ +static inline void ImuMagEvent(void (* _mag_handler)(void)) { + ms2100_event(&ms2100); + if (ms2100.status == MS2100_DATA_AVAILABLE) { + imu.mag_unscaled.x = ms2100.data.value[IMU_MAG_X_CHAN]; + imu.mag_unscaled.y = ms2100.data.value[IMU_MAG_Y_CHAN]; + imu.mag_unscaled.z = ms2100.data.value[IMU_MAG_Z_CHAN]; + ms2100.status = MS2100_IDLE; + _mag_handler(); + } } #elif defined IMU_B2_MAG_TYPE && IMU_B2_MAG_TYPE == IMU_B2_MAG_AMI601 #include "peripherals/ami601.h"