Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-06-06 07:53:43 +08:00
Code Issues Actions 26 Packages Projects Releases Wiki Activity

updated test_aspirin test program

Browse Source
This commit is contained in:
Antoine Drouin
2010-09-02 21:33:47 +00:00
parent 5146235a4b
commit c24ec22e1c
9 changed files with 195 additions and 69 deletions
Download Patch File Download Diff File Expand all files Collapse all files
conf/airframes/Poine/booz2_a7.xml
+1
View File
@@ -205,6 +205,7 @@
<target name="test_telemetry" board="lisa_l_1.0"/>
<target name="test_baro" board="lisa_l_1.0"/>
<target name="test_imu_b2" board="lisa_l_1.0"/>
<target name="test_imu_aspirin" board="lisa_l_1.0"/>
<target name="test_rc_spektrum" board="lisa_l_1.0"/>
<target name="test_rc_ppm" board="lisa_l_1.0"/>
<target name="test_adc" board="lisa_l_1.0"/>
conf/autopilot/lisa_l_test_progs.makefile
+52 -1
View File
@@ -277,6 +277,54 @@ test_imu_b2.srcs += $(SRC_BOOZ)/peripherals/booz_max1168.c $(SRC_BOOZ_ARCH)/peri
test_imu_b2.srcs += $(SRC_BOOZ)/peripherals/booz_ms2001.c $(SRC_BOOZ_ARCH)/peripherals/booz_ms2001_arch.c
#
# test IMU aspirin
#
test_imu_aspirin.ARCHDIR = $(ARCHI)
test_imu_aspirin.TARGET = test_imu_aspirin
test_imu_aspirin.TARGETDIR = test_imu_aspirin
test_imu_aspirin.CFLAGS = -I$(SRC_LISA) -I$(ARCHI) -I$(SRC_BOOZ) -I$(SRC_BOOZ_ARCH) -DPERIPHERALS_AUTO_INIT
test_imu_aspirin.CFLAGS += -DBOARD_CONFIG=$(BOARD_CFG)
test_imu_aspirin.srcs += $(SRC_BOOZ_TEST)/booz_test_imu.c \
$(SRC_ARCH)/stm32_exceptions.c \
$(SRC_ARCH)/stm32_vector_table.c
test_imu_aspirin.CFLAGS += -DUSE_LED
test_imu_aspirin.srcs += $(SRC_ARCH)/led_hw.c
test_imu_aspirin.CFLAGS += -DUSE_SYS_TIME -DSYS_TIME_LED=1
test_imu_aspirin.CFLAGS += -DPERIODIC_TASK_PERIOD='SYS_TICS_OF_SEC((1./512.))'
test_imu_aspirin.srcs += sys_time.c $(SRC_ARCH)/sys_time_hw.c
test_imu_aspirin.CFLAGS += -DUSE_UART2 -DUART2_BAUD=B57600
test_imu_aspirin.srcs += $(SRC_ARCH)/uart_hw.c
test_imu_aspirin.CFLAGS += -DDOWNLINK -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_DEVICE=Uart2
test_imu_aspirin.srcs += downlink.c pprz_transport.c
test_imu_aspirin.srcs += math/pprz_trig_int.c
test_imu_aspirin.CFLAGS += -DBOOZ_IMU_TYPE_H=\"imu/booz_imu_aspirin.h\" -DIMU_OVERRIDE_CHANNELS
test_imu_aspirin.srcs += $(SRC_BOOZ)/booz_imu.c \
$(SRC_BOOZ)/imu/booz_imu_aspirin.c \
$(SRC_BOOZ_ARCH)/imu/booz_imu_aspirin_arch.c
test_imu_aspirin.CFLAGS += -DUSE_I2C2
test_imu_aspirin.srcs += i2c.c $(SRC_ARCH)/i2c_hw.c
test_imu_aspirin.CFLAGS += -DUSE_EXTI15_10_IRQ # Gyro Int on PC14
test_imu_aspirin.CFLAGS += -DUSE_EXTI9_5_IRQ # Mag Int on PB5
test_imu_aspirin.CFLAGS += -DUSE_EXTI2_IRQ # Accel Int on PD2
test_imu_aspirin.CFLAGS += -DUSE_DMA1_C4_IRQ # SPI2 Rx DMA
#
# test hmc5843
#
@@ -362,4 +410,7 @@ test_adxl345.CFLAGS += -DDOWNLINK -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_
test_adxl345.srcs += downlink.c pprz_transport.c
test_adxl345.CFLAGS += -DUSE_EXTI2_IRQ # Accel Int on PD2
test_adxl345.CFLAGS += -DUSE_DMA1_C4_IRQ # SPI2 Rx DMA
test_adxl345.CFLAGS += -DUSE_DMA1_C4_IRQ # SPI2 Rx DMA
sw/airborne/booz/arch/stm32/imu/booz_imu_aspirin_arch.c
+26 -2
View File
@@ -223,18 +223,32 @@ void adxl345_start_reading_data(void) {
DMA_ITConfig(DMA1_Channel4, DMA_IT_TC, ENABLE);
}
/*
*
* Gyro data ready
*
*/
void exti15_10_irq_handler(void) {
/* clear EXTI */
if(EXTI_GetITStatus(EXTI_Line14) != RESET)
EXTI_ClearITPendingBit(EXTI_Line14);
i2c2.buf[0] = ITG3200_REG_GYRO_XOUT_H;
i2c2_transceive(ITG3200_ADDR,1, 6, &imu_aspirin.i2c_done);
imu_aspirin.i2c_trans_gyro.type = I2CTransTxRx;
imu_aspirin.i2c_trans_gyro.buf[0] = ITG3200_REG_GYRO_XOUT_H;
imu_aspirin.i2c_trans_gyro.slave_addr = ITG3200_ADDR;
imu_aspirin.i2c_trans_gyro.len_w = 1;
imu_aspirin.i2c_trans_gyro.len_r = 6;
i2c_submit(&i2c2,&imu_aspirin.i2c_trans_gyro);
imu_aspirin.status = AspirinStatusReadingGyro;
}
/*
*
* Mag data ready
*
*/
void exti9_5_irq_handler(void) {
/* clear EXTI */
@@ -245,6 +259,11 @@ void exti9_5_irq_handler(void) {
}
/*
*
* Accel data ready
*
*/
void exti2_irq_handler(void) {
/* clear EXTI */
@@ -255,6 +274,11 @@ void exti2_irq_handler(void) {
}
/*
*
* Accel end of DMA transfert
*
*/
void dma1_c4_irq_handler(void) {
Adxl345Unselect();
DMA_ITConfig(DMA1_Channel4, DMA_IT_TC, DISABLE);
sw/airborne/booz/arch/stm32/imu/booz_imu_aspirin_arch.h
-18
View File
@@ -10,23 +10,5 @@ extern void adxl345_write_to_reg(uint8_t addr, uint8_t val);
extern void adxl345_clear_rx_buf(void);
extern void adxl345_start_reading_data(void);
#if 0
#define OnI2CDone() { \
switch (imu_aspirin.status) { \
case AspirinStatusReadingGyro: \
{ \
\
int16_t gp = i2c2.buf[0]<<8 | i2c2.buf[1]; \
int16_t gq = i2c2.buf[2]<<8 | i2c2.buf[3]; \
int16_t gr = i2c2.buf[4]<<8 | i2c2.buf[5]; \
RATES_ASSIGN(booz_imu.gyro_unscaled, gp, gq, gr); \
/*if (abs(booz_imu.gyro_unscaled.p) > 32000 || abs(booz_imu.gyro_unscaled.q) > 32000) LED_ON(4);*/ \
} \
break; \
default: \
break; \
} \
}
#endif
#endif /* BOOZ_IMU_ASPIRIN_ARCH_H */
sw/airborne/booz/imu/booz_imu_aspirin.c
+37 -31
View File
@@ -13,7 +13,6 @@ static void configure_accel(void);
void booz_imu_impl_init(void) {
imu_aspirin.status = AspirinStatusUninit;
imu_aspirin.i2c_done = FALSE;
imu_aspirin.gyro_available = FALSE;
imu_aspirin.gyro_available_blaaa = FALSE;
imu_aspirin.mag_ready_for_read = FALSE;
@@ -28,7 +27,7 @@ void booz_imu_impl_init(void) {
void booz_imu_periodic(void) {
if (imu_aspirin.status == AspirinStatusUninit) {
configure_gyro();
// configure_mag();
configure_mag();
configure_accel();
imu_aspirin.status = AspirinStatusIdle;
}
@@ -40,47 +39,54 @@ void booz_imu_periodic(void) {
/* sends a serie of I2C commands to configure the ITG3200 gyro */
static void configure_gyro(void) {
struct i2c_transaction t;
t.type = I2CTransTx;
t.slave_addr = ITG3200_ADDR;
/* set gyro range to 2000deg/s and low pass at 256Hz */
i2c2.buf[0] = ITG3200_REG_DLPF_FS;
i2c2.buf[1] = (0x03<<3);
i2c2_transmit(ITG3200_ADDR, 2, &imu_aspirin.i2c_done);
while (!imu_aspirin.i2c_done);
t.buf[0] = ITG3200_REG_DLPF_FS;
t.buf[1] = (0x03<<3);
t.len_w = 2;
i2c_submit(&i2c2,&t);
while (t.status != I2CTransSuccess);
/* set sample rate to 533Hz */
i2c2.buf[0] = ITG3200_REG_SMPLRT_DIV;
i2c2.buf[1] = 0x0E;
i2c2_transmit(ITG3200_ADDR, 2, &imu_aspirin.i2c_done);
while (!imu_aspirin.i2c_done);
t.buf[0] = ITG3200_REG_SMPLRT_DIV;
t.buf[1] = 0x0E;
i2c_submit(&i2c2,&t);
while (t.status != I2CTransSuccess);
/* switch to gyroX clock */
i2c2.buf[0] = ITG3200_REG_PWR_MGM;
i2c2.buf[1] = 0x01;
i2c2_transmit(ITG3200_ADDR, 2, &imu_aspirin.i2c_done);
while (!imu_aspirin.i2c_done);
t.buf[0] = ITG3200_REG_PWR_MGM;
t.buf[1] = 0x01;
i2c_submit(&i2c2,&t);
while (t.status != I2CTransSuccess);
/* enable interrupt on data ready, idle hight */
i2c2.buf[0] = ITG3200_REG_INT_CFG;
i2c2.buf[1] = (0x01 | 0x01<<7);
i2c2_transmit(ITG3200_ADDR, 2, &imu_aspirin.i2c_done);
while (!imu_aspirin.i2c_done);
t.buf[0] = ITG3200_REG_INT_CFG;
t.buf[1] = (0x01 | 0x01<<7);
i2c_submit(&i2c2,&t);
while (t.status != I2CTransSuccess);
}
/* sends a serie of I2C commands to configure the ITG3200 gyro */
static void configure_mag(void) {
struct i2c_transaction t;
t.type = I2CTransTx;
t.slave_addr = HMC5843_ADDR;
/* set to rate to 50Hz */
i2c2.buf[0] = HMC5843_REG_CFGA;
i2c2.buf[1] = 0x00 | (0x06 << 2);
i2c2_transmit(HMC5843_ADDR, 2, &imu_aspirin.i2c_done);
while (!imu_aspirin.i2c_done);
t.buf[0] = HMC5843_REG_CFGA;
t.buf[1] = 0x00 | (0x06 << 2);
i2c_submit(&i2c2,&t);
while (t.status != I2CTransSuccess);
/* set to gain to 1 Gauss */
i2c2.buf[0] = HMC5843_REG_CFGB;
i2c2.buf[1] = 0x01<<5;
i2c2_transmit(HMC5843_ADDR, 2, &imu_aspirin.i2c_done);
while (!imu_aspirin.i2c_done);
t.buf[0] = HMC5843_REG_CFGB;
t.buf[1] = 0x01<<5;
i2c_submit(&i2c2,&t);
while (t.status != I2CTransSuccess);
/* set to continuous mode */
i2c2.buf[0] = HMC5843_REG_MODE;
i2c2.buf[1] = 0x00;
i2c2_transmit(HMC5843_ADDR, 2, &imu_aspirin.i2c_done);
while (!imu_aspirin.i2c_done);
t.buf[0] = HMC5843_REG_MODE;
t.buf[1] = 0x00;
i2c_submit(&i2c2,&t);
while (t.status != I2CTransSuccess);
}
sw/airborne/booz/imu/booz_imu_aspirin.h
+37 -13
View File
@@ -27,9 +27,27 @@
#include "airframe.h"
#include "booz_imu.h"
#include "peripherals/booz_itg3200.h"
#include "peripherals/booz_hmc5843.h"
#include "peripherals/booz_adxl345.h"
#include "i2c.h"
#include "booz/peripherals/booz_itg3200.h"
#include "booz/peripherals/booz_hmc5843.h"
#include "booz/peripherals/booz_adxl345.h"
#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
#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
enum AspirinStatus
{ AspirinStatusUninit,
@@ -40,7 +58,8 @@ enum AspirinStatus
struct BoozImuAspirin {
volatile enum AspirinStatus status;
volatile uint8_t i2c_done;
struct i2c_transaction i2c_trans_gyro;
struct i2c_transaction i2c_trans_mag;
uint8_t gyro_available;
uint8_t gyro_available_blaaa;
uint8_t mag_available;
@@ -56,14 +75,18 @@ extern struct BoozImuAspirin imu_aspirin;
#define BoozImuEvent(_gyro_accel_handler, _mag_handler) { \
if (imu_aspirin.status == AspirinStatusReadingGyro && imu_aspirin.i2c_done) { \
int16_t gp = i2c2.buf[0]<<8 | i2c2.buf[1]; \
int16_t gq = i2c2.buf[2]<<8 | i2c2.buf[3]; \
int16_t gr = i2c2.buf[4]<<8 | i2c2.buf[5]; \
if (imu_aspirin.status == AspirinStatusReadingGyro && \
imu_aspirin.i2c_trans_gyro.status == I2CTransSuccess) { \
int16_t gp = imu_aspirin.i2c_trans_gyro.buf[0]<<8 | imu_aspirin.i2c_trans_gyro.buf[1]; \
int16_t gq = imu_aspirin.i2c_trans_gyro.buf[2]<<8 | imu_aspirin.i2c_trans_gyro.buf[3]; \
int16_t gr = imu_aspirin.i2c_trans_gyro.buf[4]<<8 | imu_aspirin.i2c_trans_gyro.buf[5]; \
RATES_ASSIGN(booz_imu.gyro_unscaled, gp, gq, gr); \
if (imu_aspirin.mag_ready_for_read ) { \
/* read mag */ \
i2c2_receive(HMC5843_ADDR, 7, &imu_aspirin.i2c_done); \
imu_aspirin.i2c_trans_mag.type = I2CTransRx; \
imu_aspirin.i2c_trans_mag.slave_addr = HMC5843_ADDR; \
imu_aspirin.i2c_trans_mag.len_r = 7; \
i2c_submit(&i2c2,&imu_aspirin.i2c_trans_mag); \
imu_aspirin.mag_ready_for_read = FALSE; \
imu_aspirin.status = AspirinStatusReadingMag; \
} \
@@ -71,10 +94,11 @@ extern struct BoozImuAspirin imu_aspirin;
imu_aspirin.status = AspirinStatusIdle; \
} \
} \
if (imu_aspirin.status == AspirinStatusReadingMag && imu_aspirin.i2c_done) { \
int16_t mx = i2c2.buf[0]<<8 | i2c2.buf[1]; \
int16_t my = i2c2.buf[2]<<8 | i2c2.buf[3]; \
int16_t mz = i2c2.buf[4]<<8 | i2c2.buf[5]; \
if (imu_aspirin.status == AspirinStatusReadingMag && \
imu_aspirin.i2c_trans_mag.status == I2CTransSuccess) { \
int16_t mx = imu_aspirin.i2c_trans_mag.buf[0]<<8 | imu_aspirin.i2c_trans_mag.buf[1]; \
int16_t my = imu_aspirin.i2c_trans_mag.buf[2]<<8 | imu_aspirin.i2c_trans_mag.buf[3]; \
int16_t mz = imu_aspirin.i2c_trans_mag.buf[4]<<8 | imu_aspirin.i2c_trans_mag.buf[5]; \
VECT3_ASSIGN(booz_imu.mag_unscaled, mx, my, mz); \
imu_aspirin.mag_available = TRUE; \
imu_aspirin.status = AspirinStatusIdle; \
sw/airborne/lisa/test/lisa_test_aspirin.c
-1
View File
@@ -41,7 +41,6 @@ static inline void main_event_task( void );
static inline void main_init_hw(void);
static uint8_t i2c_done;
static volatile uint8_t gyro_ready_for_read;
extern void exti2_irq_handler(void);
sw/airborne/lisa/test/lisa_test_hmc5843.c
+41 -2
View File
@@ -42,8 +42,12 @@ static inline void main_periodic_task( void );
static inline void main_event_task( void );
static inline void main_init_hw(void);
static void send_config(void);
static struct i2c_transaction i2c_trans;
struct i2c_transaction t1;
struct i2c_transaction t2;
#define INITIALIZED 6
static uint8_t mag_state = 0;
static volatile uint8_t mag_ready_for_read = FALSE;
@@ -89,7 +93,9 @@ static inline void main_periodic_task( void ) {
&i2c2_errors.unexpected_event_cnt,
&i2c2_errors.last_unexpected_event);
});
if (mag_state == 2) send_config();
#if 0
switch (mag_state) {
case 2:
i2c_trans.type = I2CTransTx;
@@ -124,7 +130,7 @@ static inline void main_periodic_task( void ) {
default:
break;
}
#endif
// if (mag_state == 4) mag_state=1;
if (mag_state < INITIALIZED) mag_state++;
@@ -161,9 +167,42 @@ static inline void main_event_task( void ) {
reading_mag = FALSE;
}
}
static void send_config(void) {
t1.type = I2CTransTx;
t1.slave_addr = HMC5843_ADDR;
t1.buf[0] = HMC5843_REG_CFGA; // set to rate to 50Hz
t1.buf[1] = 0x00 | (0x06 << 2);
t1.len_w = 2;
i2c_submit(&i2c2,&t1);
t2.type = I2CTransTx;
t2.slave_addr = HMC5843_ADDR;
t2.buf[0] = HMC5843_REG_CFGB; // set to gain to 1 Gauss
t2.buf[1] = 0x01<<5;
t2.len_w = 2;
i2c_submit(&i2c2,&t2);
i2c_trans.type = I2CTransTx;
i2c_trans.slave_addr = HMC5843_ADDR;
i2c_trans.buf[0] = HMC5843_REG_MODE; // set to continuous mode
i2c_trans.buf[1] = 0x00;
i2c_trans.len_w = 2;
i2c_submit(&i2c2,&i2c_trans);
}
static inline void main_init_hw( void ) {
/* set mag ss as floating input (on PC12) = shorted to I2C2 sda ----------*/
/* set mag reset as floating input (on PC13) = shorted to I2C2 scl ----------*/
sw/airborne/stm32/i2c_hw.c
+1 -1
View File
@@ -702,7 +702,7 @@ bool_t i2c_submit(struct i2c_periph* p, struct i2c_transaction* t) {
/* if peripheral is idle, start the transaction */
if (p->status == I2CIdle)
start_transaction(p);
/* else it will be started by the interrupt handler when the previous transactions completes */
__enable_irq();
return TRUE;