Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-06-06 16:58:48 +08:00
Code Issues Actions 25 Packages Projects Releases Wiki Activity

[peripherals] first shot at refactoring ms2100, not quite nice yet...

Browse Source
This commit is contained in:
Felix Ruess
2013-01-17 20:00:44 +01:00
parent 8685d221b7
commit d35ca39c9d
10 changed files with 161 additions and 116 deletions
Download Patch File Download Diff File Expand all files Collapse all files
conf/firmwares/subsystems/lisa_passthrough/imu_b2_v1.2.makefile
-2
View File
@@ -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
conf/firmwares/subsystems/shared/imu_b2_v1.1.makefile
+2
View File
@@ -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
sw/airborne/arch/lpc21/peripherals/ms2100_arch.c
+5 -2
View File
@@ -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<<MS2100_DRDY_EINT);
VICVectAddr = 0x00000000; /* clear this interrupt from the VIC */
sw/airborne/arch/lpc21/peripherals/ms2100_arch.h
+6
View File
@@ -20,6 +20,12 @@
* Boston, MA 02111-1307, USA.
*/
/**
* @file arch/lpc21/peripherals/ms2100_arch.h
*
* LPC21xx specific functions for the ms2100 magnetic sensor from PNI.
*/
#ifndef MS2100_ARCH_H
#define MS2100_ARCH_H
sw/airborne/arch/stm32/peripherals/ms2100_arch.c
+6
View File
@@ -19,6 +19,12 @@
* Boston, MA 02111-1307, USA.
*/
/**
* @file arch/stm32/peripherals/ms2100_arch.c
*
* STM32 specific functions for the ms2100 magnetic sensor from PNI.
*/
#include "peripherals/ms2100.h"
#include <libopencm3/stm32/f1/rcc.h>
sw/airborne/arch/stm32/peripherals/ms2100_arch.h
+6
View File
@@ -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
sw/airborne/peripherals/ms2100.c
+89 -63
View File
@@ -30,89 +30,115 @@
#include <stdlib.h> // 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; i<MS2100_NB_AXIS; i++)
ms2100_values[i] = 0;
ms2100_cur_axe = 0;
INT_VECT3_ZERO(ms->data.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;
}
}
sw/airborne/peripherals/ms2100.h
+36 -38
View File
@@ -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 */
sw/airborne/subsystems/imu/imu_b2.c
+2 -2
View File
@@ -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(); });
sw/airborne/subsystems/imu/imu_b2.h
+9 -9
View File
@@ -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"