From f4a736a545a41bf55cdb356a2ea279408a0bab12 Mon Sep 17 00:00:00 2001 From: Christophe De Wagter Date: Thu, 12 May 2011 14:13:47 +0200 Subject: [PATCH] Added support for PPZUAV-9DOF-IMU --- .../PPZUAV/fixed-wing/ppzimu_tiny.xml | 271 ++++++++++++++++++ conf/autopilot/fixedwing.makefile | 1 + .../fixedwing/attitude_dcm.makefile | 1 + .../fixedwing/imu_aspirin_i2c.makefile | 5 + .../subsystems/fixedwing/imu_ppzuav.makefile | 22 ++ conf/modules/ins_ppzuavimu.xml | 24 ++ conf/telemetry/default_fixedwing_imu.xml | 18 ++ sw/airborne/ap_downlink.h | 8 + sw/airborne/firmwares/fixedwing/main_ap.c | 22 +- sw/airborne/modules/ins/ins_ppzuavimu.c | 269 +++++++++++++++++ sw/airborne/modules/ins/ins_ppzuavimu.h | 57 ++++ sw/airborne/peripherals/adxl345.h | 2 + sw/airborne/peripherals/hmc5843.h | 4 +- sw/airborne/subsystems/ahrs/ahrs_float_dcm.c | 6 + sw/airborne/subsystems/ahrs/ahrs_float_dcm.h | 2 + .../subsystems/ahrs/ahrs_float_utils.h | 2 + sw/airborne/subsystems/ahrs/ahrs_int_utils.h | 4 +- .../ahrs/ahrs_magnetic_field_model.h | 10 + 18 files changed, 720 insertions(+), 8 deletions(-) create mode 100644 conf/airframes/PPZUAV/fixed-wing/ppzimu_tiny.xml create mode 100644 conf/autopilot/subsystems/fixedwing/imu_aspirin_i2c.makefile create mode 100644 conf/autopilot/subsystems/fixedwing/imu_ppzuav.makefile create mode 100644 conf/modules/ins_ppzuavimu.xml create mode 100644 sw/airborne/modules/ins/ins_ppzuavimu.c create mode 100644 sw/airborne/modules/ins/ins_ppzuavimu.h create mode 100644 sw/airborne/subsystems/ahrs/ahrs_magnetic_field_model.h diff --git a/conf/airframes/PPZUAV/fixed-wing/ppzimu_tiny.xml b/conf/airframes/PPZUAV/fixed-wing/ppzimu_tiny.xml new file mode 100644 index 00000000000..0fbca96a268 --- /dev/null +++ b/conf/airframes/PPZUAV/fixed-wing/ppzimu_tiny.xml @@ -0,0 +1,271 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+ + + +
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+ +
+ + +
+ +
+ + +
+ +
+ + + + + +
+ +
+ + + + + + + + + + + + +
+ +
+ + + + + + + + + + + + + + + + + + +
+ +
+ + + + + + + + + + + + + + + + + +
+ +
+ + + + + + + + +
+ +
+ + + + + + + +
+ +
+ + +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/conf/autopilot/fixedwing.makefile b/conf/autopilot/fixedwing.makefile index 139a7ca919b..3b0758e8bf7 100644 --- a/conf/autopilot/fixedwing.makefile +++ b/conf/autopilot/fixedwing.makefile @@ -15,6 +15,7 @@ SRC_FIXEDWING_TEST=$(SRC_FIXEDWING)/ SRC_FIRMWARE=firmwares/fixedwing SRC_SUBSYSTEMS=subsystems +SRC_MODULES=modules FIXEDWING_INC = -I$(SRC_FIRMWARE) -I$(SRC_FIXEDWING) diff --git a/conf/autopilot/subsystems/fixedwing/attitude_dcm.makefile b/conf/autopilot/subsystems/fixedwing/attitude_dcm.makefile index 91b1eb5ecd6..9aa26c0cdf3 100644 --- a/conf/autopilot/subsystems/fixedwing/attitude_dcm.makefile +++ b/conf/autopilot/subsystems/fixedwing/attitude_dcm.makefile @@ -10,6 +10,7 @@ ap.CFLAGS += -DUSE_AHRS ap.srcs += $(SRC_SUBSYSTEMS)/ahrs.c ap.srcs += $(SRC_SUBSYSTEMS)/ahrs/ahrs_aligner.c ap.srcs += $(SRC_SUBSYSTEMS)/ahrs/ahrs_float_dcm.c +ap.srcs += math/pprz_trig_int.c ifdef AHRS_ALIGNER_LED ap.CFLAGS += -DAHRS_ALIGNER_LED=$(AHRS_ALIGNER_LED) diff --git a/conf/autopilot/subsystems/fixedwing/imu_aspirin_i2c.makefile b/conf/autopilot/subsystems/fixedwing/imu_aspirin_i2c.makefile new file mode 100644 index 00000000000..87d72683c7e --- /dev/null +++ b/conf/autopilot/subsystems/fixedwing/imu_aspirin_i2c.makefile @@ -0,0 +1,5 @@ + + +include $(CFG_FIXEDWING)/imu_ppzuav.makefile + +ap.CFLAGS += -DASPIRIN_IMU diff --git a/conf/autopilot/subsystems/fixedwing/imu_ppzuav.makefile b/conf/autopilot/subsystems/fixedwing/imu_ppzuav.makefile new file mode 100644 index 00000000000..8f224a91668 --- /dev/null +++ b/conf/autopilot/subsystems/fixedwing/imu_ppzuav.makefile @@ -0,0 +1,22 @@ + +IMU_PPZUAVIMU_CFLAGS = -DUSE_IMU +IMU_PPZUAVIMU_CFLAGS += -DIMU_TYPE_H=\"modules/ins/ins_ppzuavimu.h\" + +IMU_PPZUAVIMU_SRCS = $(SRC_SUBSYSTEMS)/imu.c \ + $(SRC_MODULES)/ins/ins_ppzuavimu.c + + +IMU_PPZUAVIMU_CFLAGS += -DUSE_I2C +ifdef STM32 + IMU_PPZUAVIMU_CFLAGS += -DUSE_I2C2 + IMU_PPZUAVIMU_CFLAGS += -DPPZUAVIMU_I2C_DEVICE=i2c2 +else + IMU_PPZUAVIMU_CFLAGS += -DUSE_I2C0 + IMU_PPZUAVIMU_CFLAGS += -DPPZUAVIMU_I2C_DEVICE=i2c0 +endif + +ap.CFLAGS += $(IMU_PPZUAVIMU_CFLAGS) +ap.srcs += $(IMU_PPZUAVIMU_SRCS) + +ap.CFLAGS += -DAHRS_MAG_UPDATE_YAW_ONLY + diff --git a/conf/modules/ins_ppzuavimu.xml b/conf/modules/ins_ppzuavimu.xml new file mode 100644 index 00000000000..a2544c204df --- /dev/null +++ b/conf/modules/ins_ppzuavimu.xml @@ -0,0 +1,24 @@ + + + + +
+ +
+ + + + + + + + + + + + + + + + + diff --git a/conf/telemetry/default_fixedwing_imu.xml b/conf/telemetry/default_fixedwing_imu.xml index c106da4e29d..5435f101128 100644 --- a/conf/telemetry/default_fixedwing_imu.xml +++ b/conf/telemetry/default_fixedwing_imu.xml @@ -28,6 +28,7 @@ + @@ -50,6 +51,23 @@ + + + + + + + + + + + + + + + + + diff --git a/sw/airborne/ap_downlink.h b/sw/airborne/ap_downlink.h index 041a6a0b667..8a0c7bc43f1 100644 --- a/sw/airborne/ap_downlink.h +++ b/sw/airborne/ap_downlink.h @@ -142,13 +142,21 @@ #include "subsystems/imu.h" #define PERIODIC_SEND_IMU_ACCEL_RAW(_chan) { DOWNLINK_SEND_IMU_ACCEL_RAW(_chan, &imu.accel_unscaled.x, &imu.accel_unscaled.y, &imu.accel_unscaled.z)} #define PERIODIC_SEND_IMU_GYRO_RAW(_chan) { DOWNLINK_SEND_IMU_GYRO_RAW(_chan, &imu.gyro_unscaled.p, &imu.gyro_unscaled.q, &imu.gyro_unscaled.r)} +#define PERIODIC_SEND_IMU_MAG_RAW(_chan) { DOWNLINK_SEND_IMU_MAG_RAW(_chan, &imu.mag_unscaled.x, &imu.mag_unscaled.y, &imu.mag_unscaled.z)} #define PERIODIC_SEND_IMU_ACCEL(_chan) { struct FloatVect3 accel_float; ACCELS_FLOAT_OF_BFP(accel_float, imu.accel); DOWNLINK_SEND_IMU_ACCEL(_chan, &accel_float.x, &accel_float.y, &accel_float.z)} #define PERIODIC_SEND_IMU_GYRO(_chan) { struct FloatRates gyro_float; RATES_FLOAT_OF_BFP(gyro_float, imu.gyro); DOWNLINK_SEND_IMU_GYRO(_chan, &gyro_float.p, &gyro_float.q, &gyro_float.r)} +# ifdef USE_MAGNETOMETER +# define PERIODIC_SEND_IMU_MAG(_chan) { struct FloatVect3 mag_float; MAGS_FLOAT_OF_BFP(mag_float, imu.mag); DOWNLINK_SEND_IMU_MAG(_chan, &mag_float.x, &mag_float.y, &mag_float.z)} +# else +# define PERIODIC_SEND_IMU_MAG_RAW(_chan) {} +# endif #else #define PERIODIC_SEND_IMU_ACCEL_RAW(_chan) {} #define PERIODIC_SEND_IMU_GYRO_RAW(_chan) {} +#define PERIODIC_SEND_IMU_MAG_RAW(_chan) {} #define PERIODIC_SEND_IMU_ACCEL(_chan) {} #define PERIODIC_SEND_IMU_GYRO(_chan) {} +#define PERIODIC_SEND_IMU_MAG(_chan) {} #endif #ifdef IMU_ANALOG diff --git a/sw/airborne/firmwares/fixedwing/main_ap.c b/sw/airborne/firmwares/fixedwing/main_ap.c index bc5d0a6c01e..9e79daacad7 100644 --- a/sw/airborne/firmwares/fixedwing/main_ap.c +++ b/sw/airborne/firmwares/fixedwing/main_ap.c @@ -74,7 +74,7 @@ #ifdef USE_AHRS #include "subsystems/ahrs.h" #include "subsystems/ahrs/ahrs_aligner.h" -#include "subsystems/ahrs/ahrs_float_dcm.h" +#include AHRS_TYPE_H static inline void on_gyro_accel_event( void ); static inline void on_accel_event( void ); static inline void on_mag_event( void ); @@ -402,6 +402,12 @@ static inline void attitude_loop( void ) { } +#ifdef USE_IMU +#ifdef AHRS_TRIGGERED_ATTITUDE_LOOP +volatile uint8_t new_ins_attitude = 0; +#endif +#endif + void periodic_task_ap( void ) { static uint8_t _60Hz = 0; @@ -731,15 +737,21 @@ static inline void on_gyro_accel_event( void ) { LED_OFF(AHRS_CPU_LED); #endif +#ifdef AHRS_TRIGGERED_ATTITUDE_LOOP + new_ins_attitude = 1; +#endif + } -static inline void on_mag_event(void) { - /* +static inline void on_mag_event(void) +{ +#ifdef IMU_MAG_X_SIGN ImuScaleMag(imu); if (ahrs.status == AHRS_RUNNING) { ahrs_update_mag(); - ahrs_update_fw_estimator(); +// ahrs_update_fw_estimator(); } - */ +#endif } #endif // USE_AHRS + diff --git a/sw/airborne/modules/ins/ins_ppzuavimu.c b/sw/airborne/modules/ins/ins_ppzuavimu.c new file mode 100644 index 00000000000..c0921076161 --- /dev/null +++ b/sw/airborne/modules/ins/ins_ppzuavimu.c @@ -0,0 +1,269 @@ +/* + * 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. + * + */ + +#include +#include "ins_ppzuavimu.h" +#include "mcu_periph/i2c.h" +#include "led.h" + +// Downlink +#include "mcu_periph/uart.h" +#include "messages.h" +#include "downlink.h" + +#ifndef DOWNLINK_DEVICE +#define DOWNLINK_DEVICE DOWNLINK_AP_DEVICE +#endif + + +// Peripherials +#define HMC5843_NO_IRQ +#include "../../peripherals/itg3200.h" +#include "../../peripherals/adxl345.h" +#include "../../peripherals/hmc5843.h" + +// Results +volatile bool_t mag_valid; +volatile bool_t gyr_valid; +volatile bool_t acc_valid; + +// Communication +struct i2c_transaction ppzuavimu_hmc5843; +struct i2c_transaction ppzuavimu_itg3200; +struct i2c_transaction ppzuavimu_adxl345; + +// Standalone option: run module only +#ifndef IMU_TYPE_H +struct Imu imu; +#endif + +#ifndef PERIODIC_FREQUENCY +#define PERIODIC_FREQUENCY 60 +#endif + +void imu_impl_init(void) +{ + ///////////////////////////////////////////////////////////////////// + // ITG3200 + ppzuavimu_itg3200.type = I2CTransTx; + ppzuavimu_itg3200.slave_addr = ITG3200_ADDR; + ppzuavimu_itg3200.buf[0] = ITG3200_REG_DLPF_FS; +#if PERIODIC_FREQUENCY == 60 + /* set gyro range to 2000deg/s and low pass at 20Hz (< 60Hz/2) internal sampling at 1kHz */ + ppzuavimu_itg3200.buf[1] = (0x03<<3) | (0x04<<0); +# warning ITG3200 read at 50Hz +#else +# if PERIODIC_FREQUENCY == 120 +# warning ITG3200 read at 100Hz + /* set gyro range to 2000deg/s and low pass at 42Hz (< 120Hz/2) internal sampling at 1kHz */ + ppzuavimu_itg3200.buf[1] = (0x03<<3) | (0x03<<0); +# else +# error PERIODIC_FREQUENCY should be either 60Hz or 120Hz. Otherwise manually fix the sensor rates +# endif +#endif + ppzuavimu_itg3200.len_w = 2; + i2c_submit(&PPZUAVIMU_I2C_DEVICE,&ppzuavimu_itg3200); + while(ppzuavimu_itg3200.status == I2CTransPending); + + /* set sample rate to 66Hz: so at 60Hz there is always a new sample ready and you loose little */ + ppzuavimu_itg3200.buf[0] = ITG3200_REG_SMPLRT_DIV; +#if PERIODIC_FREQUENCY == 60 + ppzuavimu_itg3200.buf[1] = 19; // 50Hz +#else + ppzuavimu_itg3200.buf[1] = 9; // 100Hz +#endif + i2c_submit(&PPZUAVIMU_I2C_DEVICE,&ppzuavimu_itg3200); + while(ppzuavimu_itg3200.status == I2CTransPending); + + /* switch to gyroX clock */ + ppzuavimu_itg3200.buf[0] = ITG3200_REG_PWR_MGM; + ppzuavimu_itg3200.buf[1] = 0x01; + i2c_submit(&PPZUAVIMU_I2C_DEVICE,&ppzuavimu_itg3200); + while(ppzuavimu_itg3200.status == I2CTransPending); + + /* no interrupts for now, but set data ready interrupt to enable reading status bits */ + ppzuavimu_itg3200.buf[0] = ITG3200_REG_INT_CFG; + ppzuavimu_itg3200.buf[1] = 0x01; + i2c_submit(&PPZUAVIMU_I2C_DEVICE,&ppzuavimu_itg3200); + while(ppzuavimu_itg3200.status == I2CTransPending); + + ///////////////////////////////////////////////////////////////////// + // ADXL345 + + /* set data rate to 100Hz */ + ppzuavimu_adxl345.slave_addr = ADXL345_ADDR; + ppzuavimu_adxl345.type = I2CTransTx; + ppzuavimu_adxl345.buf[0] = ADXL345_REG_BW_RATE; +#if PERIODIC_FREQUENCY == 60 + ppzuavimu_adxl345.buf[1] = 0x09; // normal power and 50Hz sampling, 50Hz BW +#else + ppzuavimu_adxl345.buf[1] = 0x0a; // normal power and 100Hz sampling, 50Hz BW +#endif + ppzuavimu_adxl345.len_w = 2; + i2c_submit(&PPZUAVIMU_I2C_DEVICE,&ppzuavimu_adxl345); + while(ppzuavimu_adxl345.status == I2CTransPending); + + /* switch to measurement mode */ + ppzuavimu_adxl345.type = I2CTransTx; + ppzuavimu_adxl345.buf[0] = ADXL345_REG_POWER_CTL; + ppzuavimu_adxl345.buf[1] = 1<<3; + ppzuavimu_adxl345.len_w = 2; + i2c_submit(&PPZUAVIMU_I2C_DEVICE,&ppzuavimu_adxl345); + while(ppzuavimu_adxl345.status == I2CTransPending); + + /* Set range to 16g but keeping full resolution of 3.9 mV/g */ + ppzuavimu_adxl345.type = I2CTransTx; + ppzuavimu_adxl345.buf[0] = ADXL345_REG_DATA_FORMAT; + ppzuavimu_adxl345.buf[1] = 1<<3 | 0<<2 | 0x03; // bit 3 is full resolution bit, bit 2 is left justify bit 0,1 are range: 00=2g 01=4g 10=8g 11=16g + ppzuavimu_adxl345.len_w = 2; + i2c_submit(&PPZUAVIMU_I2C_DEVICE,&ppzuavimu_adxl345); + while(ppzuavimu_adxl345.status == I2CTransPending); + + ///////////////////////////////////////////////////////////////////// + // HMC5843 + ppzuavimu_hmc5843.slave_addr = HMC5843_ADDR; + ppzuavimu_hmc5843.type = I2CTransTx; + ppzuavimu_hmc5843.buf[0] = HMC5843_REG_CFGA; // set to rate to max speed: 50Hz no bias + ppzuavimu_hmc5843.buf[1] = 0x00 | (0x06 << 2); + ppzuavimu_hmc5843.len_w = 2; + i2c_submit(&PPZUAVIMU_I2C_DEVICE,&ppzuavimu_hmc5843); + while(ppzuavimu_hmc5843.status == I2CTransPending); + + ppzuavimu_hmc5843.type = I2CTransTx; + ppzuavimu_hmc5843.buf[0] = HMC5843_REG_CFGB; // set to gain to 1 Gauss + ppzuavimu_hmc5843.buf[1] = 0x01<<5; + ppzuavimu_hmc5843.len_w = 2; + i2c_submit(&PPZUAVIMU_I2C_DEVICE,&ppzuavimu_hmc5843); + while(ppzuavimu_hmc5843.status == I2CTransPending); + + ppzuavimu_hmc5843.type = I2CTransTx; + ppzuavimu_hmc5843.buf[0] = HMC5843_REG_MODE; // set to continuous mode + ppzuavimu_hmc5843.buf[1] = 0x00; + ppzuavimu_hmc5843.len_w = 2; + i2c_submit(&PPZUAVIMU_I2C_DEVICE,&ppzuavimu_hmc5843); + while(ppzuavimu_hmc5843.status == I2CTransPending); + +} + +void imu_periodic( void ) +{ + // Start reading the latest gyroscope data + ppzuavimu_itg3200.type = I2CTransTxRx; + ppzuavimu_itg3200.len_r = 9; + ppzuavimu_itg3200.len_w = 1; + ppzuavimu_itg3200.buf[0] = ITG3200_REG_INT_STATUS; + i2c_submit(&PPZUAVIMU_I2C_DEVICE, &ppzuavimu_itg3200); + + // Start reading the latest accelerometer data + ppzuavimu_adxl345.type = I2CTransTxRx; + ppzuavimu_adxl345.len_r = 6; + ppzuavimu_adxl345.len_w = 1; + ppzuavimu_adxl345.buf[0] = ADXL345_REG_DATA_X0; + i2c_submit(&PPZUAVIMU_I2C_DEVICE, &ppzuavimu_adxl345); + + // Start reading the latest magnetometer data +#if PERIODIC_FREQUENCY > 60 + RunOnceEvery(2,{ +#endif + ppzuavimu_hmc5843.type = I2CTransTxRx; + ppzuavimu_hmc5843.len_r = 6; + ppzuavimu_hmc5843.len_w = 1; + ppzuavimu_hmc5843.buf[0] = HMC5843_REG_DATXM; + i2c_submit(&PPZUAVIMU_I2C_DEVICE, &ppzuavimu_hmc5843); +#if PERIODIC_FREQUENCY > 60 + }); +#endif + //RunOnceEvery(10,ppzuavimu_module_downlink_raw()); +} + +void ppzuavimu_module_downlink_raw( void ) +{ + DOWNLINK_SEND_IMU_GYRO_RAW(DefaultChannel,&imu.gyro_unscaled.p,&imu.gyro_unscaled.q,&imu.gyro_unscaled.r); + DOWNLINK_SEND_IMU_ACCEL_RAW(DefaultChannel,&imu.accel_unscaled.x,&imu.accel_unscaled.y,&imu.accel_unscaled.z); + DOWNLINK_SEND_IMU_MAG_RAW(DefaultChannel,&imu.mag_unscaled.x,&imu.mag_unscaled.y,&imu.mag_unscaled.z); +} + +void ppzuavimu_module_event( void ) +{ + int32_t x, y, z; + + // If the itg3200 I2C transaction has succeeded: convert the data + if (ppzuavimu_itg3200.status == I2CTransSuccess) + { +#define ITG_STA_DAT_OFFSET 3 + x = (int16_t) ((ppzuavimu_itg3200.buf[0+ITG_STA_DAT_OFFSET] << 8) | ppzuavimu_itg3200.buf[1+ITG_STA_DAT_OFFSET]); + y = (int16_t) ((ppzuavimu_itg3200.buf[2+ITG_STA_DAT_OFFSET] << 8) | ppzuavimu_itg3200.buf[3+ITG_STA_DAT_OFFSET]); + z = (int16_t) ((ppzuavimu_itg3200.buf[4+ITG_STA_DAT_OFFSET] << 8) | ppzuavimu_itg3200.buf[5+ITG_STA_DAT_OFFSET]); + + // Is this is new data + if (ppzuavimu_itg3200.buf[0] & 0x01) + { + //LED_ON(3); + gyr_valid = TRUE; + //LED_OFF(3); + } + else + { + } + + // Signs depend on the way sensors are soldered on the board: so they are hardcoded +#ifdef ASPIRIN_IMU + RATES_ASSIGN(imu.gyro_unscaled, x, -y, -z); +#else // PPZIMU + RATES_ASSIGN(imu.gyro_unscaled, -x, y, -z); +#endif + + ppzuavimu_itg3200.status = I2CTransDone; // remove the I2CTransSuccess status, otherwise data ready will be triggered again without new data + } + + // If the adxl345 I2C transaction has succeeded: convert the data + if (ppzuavimu_adxl345.status == I2CTransSuccess) + { + x = (int16_t) ((ppzuavimu_adxl345.buf[1] << 8) | ppzuavimu_adxl345.buf[0]); + y = (int16_t) ((ppzuavimu_adxl345.buf[3] << 8) | ppzuavimu_adxl345.buf[2]); + z = (int16_t) ((ppzuavimu_adxl345.buf[5] << 8) | ppzuavimu_adxl345.buf[4]); + +#ifdef ASPIRIN_IMU + VECT3_ASSIGN(imu.accel_unscaled, x, -y, -z); +#else // PPZIMU + VECT3_ASSIGN(imu.accel_unscaled, -x, y, -z); +#endif + + acc_valid = TRUE; + ppzuavimu_adxl345.status = I2CTransDone; + } + + // If the hmc5843 I2C transaction has succeeded: convert the data + if (ppzuavimu_hmc5843.status == I2CTransSuccess) + { + x = (int16_t) ((ppzuavimu_hmc5843.buf[0] << 8) | ppzuavimu_hmc5843.buf[1]); + y = (int16_t) ((ppzuavimu_hmc5843.buf[2] << 8) | ppzuavimu_hmc5843.buf[3]); + z = (int16_t) ((ppzuavimu_hmc5843.buf[4] << 8) | ppzuavimu_hmc5843.buf[5]); + +#ifdef ASPIRIN_IMU + VECT3_ASSIGN(imu.mag_unscaled, x, -y, -z); +#else // PPZIMU + VECT3_ASSIGN(imu.mag_unscaled, -y, -x, -z); +#endif + + mag_valid = TRUE; + ppzuavimu_hmc5843.status = I2CTransDone; + } +} diff --git a/sw/airborne/modules/ins/ins_ppzuavimu.h b/sw/airborne/modules/ins/ins_ppzuavimu.h new file mode 100644 index 00000000000..8ffe325d299 --- /dev/null +++ b/sw/airborne/modules/ins/ins_ppzuavimu.h @@ -0,0 +1,57 @@ +/* + * 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. + * + */ + +#ifndef PPZUAVIMU_H +#define PPZUAVIMU_H + +#include "std.h" +#include "subsystems/imu.h" + +extern volatile bool_t gyr_valid; +extern volatile bool_t acc_valid; +extern volatile bool_t mag_valid; + +/* must be defined in order to be IMU code: declared in imu.h +extern void imu_impl_init(void); +extern void imu_periodic(void); +*/ + +#define ImuEvent(_gyro_handler, _accel_handler, _mag_handler) { \ + ppzuavimu_module_event(); \ + if (gyr_valid) { \ + gyr_valid = FALSE; \ + _gyro_handler(); \ + } \ + if (acc_valid) { \ + acc_valid = FALSE; \ + _accel_handler(); \ + } \ + if (mag_valid) { \ + mag_valid = FALSE; \ + _mag_handler(); \ + } \ +} + +/* Own Extra Functions */ +extern void ppzuavimu_module_event( void ); +extern void ppzuavimu_module_downlink_raw( void ); + + +#endif // PPZUAVIMU_H diff --git a/sw/airborne/peripherals/adxl345.h b/sw/airborne/peripherals/adxl345.h index de6fa023051..5e0b5559eff 100644 --- a/sw/airborne/peripherals/adxl345.h +++ b/sw/airborne/peripherals/adxl345.h @@ -1,6 +1,8 @@ #ifndef ADXL345_H #define ADXL345_H +#define ADXL345_ADDR 0xA6 +#define ADXL345_ADDR_ALT 0x3A #define ADXL345_REG_BW_RATE 0x2C #define ADXL345_REG_POWER_CTL 0x2D diff --git a/sw/airborne/peripherals/hmc5843.h b/sw/airborne/peripherals/hmc5843.h index 16a3d37351c..2601dae09d3 100644 --- a/sw/airborne/peripherals/hmc5843.h +++ b/sw/airborne/peripherals/hmc5843.h @@ -27,7 +27,6 @@ #include "std.h" #include "mcu_periph/i2c.h" -#include "peripherals/hmc5843_arch.h" struct Hmc5843 { struct i2c_transaction i2c_trans; @@ -44,8 +43,11 @@ struct Hmc5843 { extern struct Hmc5843 hmc5843; +#ifndef HMC5843_NO_IRQ +#include "peripherals/hmc5843_arch.h" extern void hmc5843_arch_init( void ); extern void hmc5843_arch_reset( void ); +#endif extern void hmc5843_init(void); extern void hmc5843_periodic(void); diff --git a/sw/airborne/subsystems/ahrs/ahrs_float_dcm.c b/sw/airborne/subsystems/ahrs/ahrs_float_dcm.c index b7ebe07f6bb..4a3e9abce3f 100644 --- a/sw/airborne/subsystems/ahrs/ahrs_float_dcm.c +++ b/sw/airborne/subsystems/ahrs/ahrs_float_dcm.c @@ -213,6 +213,12 @@ void ahrs_update_accel(void) ACCELS_FLOAT_OF_BFP(accel_float, imu.accel); + // DCM filter uses g-force as positive + // accelerometer measures [0 0 -g] in a static case + accel_float.x = -accel_float.x; + accel_float.y = -accel_float.y; + accel_float.z = -accel_float.z; + #ifdef USE_GPS if (gps_mode==3) { //Remove centrifugal acceleration. accel_float.y += gps_speed_3d/100. * Omega[2]; // Centrifugal force on Acc_y = GPS_speed*GyroZ diff --git a/sw/airborne/subsystems/ahrs/ahrs_float_dcm.h b/sw/airborne/subsystems/ahrs/ahrs_float_dcm.h index ea54f0c1d45..f731cf5a71d 100644 --- a/sw/airborne/subsystems/ahrs/ahrs_float_dcm.h +++ b/sw/airborne/subsystems/ahrs/ahrs_float_dcm.h @@ -53,7 +53,9 @@ void ahrs_update_fw_estimator(void); #define GRAVITY 9.81 +#ifndef OUTPUTMODE #define OUTPUTMODE 1 +#endif // Mode 0 = DCM integration without Ki gyro bias // Mode 1 = DCM integration with Kp and Ki // Mode 2 = direct accelerometer -> euler diff --git a/sw/airborne/subsystems/ahrs/ahrs_float_utils.h b/sw/airborne/subsystems/ahrs/ahrs_float_utils.h index 03374c81498..69ac9c28c2a 100644 --- a/sw/airborne/subsystems/ahrs/ahrs_float_utils.h +++ b/sw/airborne/subsystems/ahrs/ahrs_float_utils.h @@ -1,6 +1,8 @@ #ifndef AHRS_FLOAT_UTILS_H #define AHRS_FLOAT_UTILS_H +#include "subsystems/ahrs/ahrs_magnetic_field_model.h" + static inline void ahrs_float_get_euler_from_accel_mag(struct FloatEulers* e, struct Int32Vect3* accel, struct Int32Vect3* mag) { /* get phi and theta from accelerometer */ struct FloatVect3 accelf; diff --git a/sw/airborne/subsystems/ahrs/ahrs_int_utils.h b/sw/airborne/subsystems/ahrs/ahrs_int_utils.h index 84918fcc620..73450f57916 100644 --- a/sw/airborne/subsystems/ahrs/ahrs_int_utils.h +++ b/sw/airborne/subsystems/ahrs/ahrs_int_utils.h @@ -4,7 +4,7 @@ //#include "../../test/pprz_algebra_print.h" #include "math/pprz_algebra_int.h" -#include "generated/airframe.h" +#include "subsystems/ahrs/ahrs_magnetic_field_model.h" static inline void ahrs_int_get_euler_from_accel_mag(struct Int32Eulers* e, struct Int32Vect3* accel, struct Int32Vect3* mag) { // DISPLAY_INT32_VECT3("# accel", (*accel)); @@ -17,7 +17,7 @@ static inline void ahrs_int_get_euler_from_accel_mag(struct Int32Eulers* e, stru int32_t cphi_ax = -INT_MULT_RSHIFT(cphi, accel->x, INT32_TRIG_FRAC); const float ftheta = atan2f(-cphi_ax, -accel->z); e->theta = ANGLE_BFP_OF_REAL(ftheta); - + int32_t sphi; PPRZ_ITRIG_SIN(sphi, e->phi); int32_t stheta; diff --git a/sw/airborne/subsystems/ahrs/ahrs_magnetic_field_model.h b/sw/airborne/subsystems/ahrs/ahrs_magnetic_field_model.h new file mode 100644 index 00000000000..b548ea3b048 --- /dev/null +++ b/sw/airborne/subsystems/ahrs/ahrs_magnetic_field_model.h @@ -0,0 +1,10 @@ +#include "generated/airframe.h" + +#ifndef AHRS_H_X +#ifdef AHRS_H_Y +#error Either define both AHRS_H_X and AHRS_H_Y or none, but not half +#endif +#define AHRS_H_X 1 +#define AHRS_H_Y 0 +#endif +