Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-06-05 15:30:08 +08:00
Code Issues Actions 27 Packages Projects Releases Wiki Activity

Aspirin IMU now works with LPC2148 too using I2C only (intended for fixedwing)

Browse Source
This commit is contained in:
Christophe De Wagter
2011-05-06 10:36:57 +02:00
parent 737d8b1592
commit 162005d918
2 changed files with 42 additions and 31 deletions
Download Patch File Download Diff File Expand all files Collapse all files
conf/airframes/PPZUAV/fixed-wing/ppzimu_tiny.xml
+11 -8
View File
@@ -44,9 +44,9 @@
<section name="IMU" prefix="IMU_">
<!-- Calibration Neutral -->
<define name="GYRO_P_NEUTRAL" value="-7"/>
<define name="GYRO_Q_NEUTRAL" value="-77"/>
<define name="GYRO_R_NEUTRAL" value="-23"/>
<define name="GYRO_P_NEUTRAL" value="0"/>
<define name="GYRO_Q_NEUTRAL" value="0"/>
<define name="GYRO_R_NEUTRAL" value="0"/>
<!-- SENS = 14.375 LSB/(deg/sec) * 57.6 deg/rad = 828 LSB/rad/sec / 12bit FRAC: 4096 / 828 -->
<define name="GYRO_P_SENS" value="4.947" integer="16"/>
@@ -60,12 +60,12 @@
<define name="GYRO_R_P" value="0."/>
<define name="GYRO_R_Q" value="0."/>
<define name="GYRO_P_SIGN" value="-1"/>
<define name="GYRO_P_SIGN" value="1"/>
<define name="GYRO_Q_SIGN" value="1"/>
<define name="GYRO_R_SIGN" value="1"/>
<define name="ACCEL_X_NEUTRAL" value="12"/>
<define name="ACCEL_Y_NEUTRAL" value="2"/>
<define name="ACCEL_X_NEUTRAL" value="0"/>
<define name="ACCEL_Y_NEUTRAL" value="0"/>
<define name="ACCEL_Z_NEUTRAL" value="0"/>
<!-- SENS = 256 LSB/g @ 2.5V [X&Y: 265 LSB/g @ 3.3V] / 9.81 ms2/g = 26.095 LSB/ms2 / 10bit FRAC: 1024 / 26.095 for z and 1024 / 27.01 for X&Y -->
@@ -74,7 +74,7 @@
<define name="ACCEL_Z_SENS" value="39.24" integer="16"/>
<define name="ACCEL_X_SIGN" value="1"/>
<define name="ACCEL_Y_SIGN" value="-1"/>
<define name="ACCEL_Y_SIGN" value="1"/>
<define name="ACCEL_Z_SIGN" value="1"/>
<define name="MAG_X_NEUTRAL" value="0"/>
@@ -260,7 +260,10 @@
ap.CFLAGS += -DAHRS_H_X=0.51562740288882 -DAHRS_H_Y=-0.05707735220832 -DAHRS_H_Z=0.85490967783446
ap.CFLAGS += -DIMU_TYPE_H=\"modules/ins/ins_ppzuavimu.h\"
# -DOUTPUTMODE=2
# Test attitude using accelerometers only:
# ap.CFLAGS += -DOUTPUTMODE=2
# ap.CFLAGS += -DASPIRIN_IMU
</makefile>
</airframe>
sw/airborne/modules/ins/ins_ppzuavimu.c
+31 -23
View File
@@ -39,16 +39,10 @@
#include "../../peripherals/hmc5843.h"
// Results
int32_t mag_x, mag_y, mag_z;
volatile bool_t mag_valid;
int32_t gyr_x, gyr_y, gyr_z;
volatile bool_t gyr_valid;
int32_t acc_x, acc_y, acc_z;
volatile bool_t acc_valid;
// Communication
struct i2c_transaction ppzuavimu_hmc5843;
struct i2c_transaction ppzuavimu_itg3200;
@@ -166,38 +160,45 @@ void ppzuavimu_module_periodic( void )
void ppzuavimu_module_downlink_raw( void )
{
DOWNLINK_SEND_IMU_GYRO_RAW(DefaultChannel,&gyr_x,&gyr_y,&gyr_z);
DOWNLINK_SEND_IMU_ACCEL_RAW(DefaultChannel,&acc_x,&acc_y,&acc_z);
DOWNLINK_SEND_IMU_MAG_RAW(DefaultChannel,&mag_x,&mag_y,&mag_z);
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)
{
gyr_x = (int16_t) ((ppzuavimu_itg3200.buf[0] << 8) | ppzuavimu_itg3200.buf[1]);
gyr_y = (int16_t) ((ppzuavimu_itg3200.buf[2] << 8) | ppzuavimu_itg3200.buf[3]);
gyr_z = (int16_t) ((ppzuavimu_itg3200.buf[4] << 8) | ppzuavimu_itg3200.buf[5]);
x = (int16_t) ((ppzuavimu_itg3200.buf[0] << 8) | ppzuavimu_itg3200.buf[1]);
y = (int16_t) ((ppzuavimu_itg3200.buf[2] << 8) | ppzuavimu_itg3200.buf[3]);
z = (int16_t) ((ppzuavimu_itg3200.buf[4] << 8) | ppzuavimu_itg3200.buf[5]);
// Signs depend on the way sensors are soldered on the board: so they are hardcoded
#ifdef ASPIRIN_IMU
RATES_ASSIGN(imu.gyro_unscaled, gyr_x, gyr_y, gyr_z);
#else
RATES_ASSIGN(imu.gyro_unscaled, gyr_x, gyr_y, gyr_z);
RATES_ASSIGN(imu.gyro_unscaled, x, -y, -z);
#else // PPZIMU
RATES_ASSIGN(imu.gyro_unscaled, -x, y, -z);
#endif
gyr_valid = TRUE;
ppzuavimu_itg3200.status = I2CTransDone;
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)
{
acc_x = (int16_t) ((ppzuavimu_adxl345.buf[1] << 8) | ppzuavimu_adxl345.buf[0]);
acc_y = (int16_t) ((ppzuavimu_adxl345.buf[3] << 8) | ppzuavimu_adxl345.buf[2]);
acc_z = (int16_t) ((ppzuavimu_adxl345.buf[5] << 8) | ppzuavimu_adxl345.buf[4]);
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]);
VECT3_ASSIGN(imu.accel_unscaled, acc_x, acc_y, acc_z);
#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;
@@ -206,9 +207,16 @@ void ppzuavimu_module_event( void )
// If the hmc5843 I2C transaction has succeeded: convert the data
if (ppzuavimu_hmc5843.status == I2CTransSuccess)
{
mag_x = (int16_t) ((ppzuavimu_hmc5843.buf[0] << 8) | ppzuavimu_hmc5843.buf[1]);
mag_y = (int16_t) ((ppzuavimu_hmc5843.buf[2] << 8) | ppzuavimu_hmc5843.buf[3]);
mag_z = (int16_t) ((ppzuavimu_hmc5843.buf[4] << 8) | ppzuavimu_hmc5843.buf[5]);
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, x, y, z);
#endif
mag_valid = TRUE;
ppzuavimu_hmc5843.status = I2CTransDone;
}