diff --git a/conf/airframes/PPZUAV/fixed-wing/ppzimu_tiny.xml b/conf/airframes/PPZUAV/fixed-wing/ppzimu_tiny.xml
index 8628daf58f..d60dfa5734 100644
--- a/conf/airframes/PPZUAV/fixed-wing/ppzimu_tiny.xml
+++ b/conf/airframes/PPZUAV/fixed-wing/ppzimu_tiny.xml
@@ -7,11 +7,11 @@
-
-
-
-
-
+
+
+
+
+
@@ -28,80 +28,30 @@
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
+
+
-
-
-
+
-
-
-
+
+
-
-
-
-
+
+
+
-
-
-
-
+
+
+
+
@@ -110,21 +60,21 @@
-
-
-
+
+
+
-
-
+
+
-
-
-
-
+
+
+
+
-
+
@@ -135,8 +85,6 @@
-
-
@@ -270,14 +218,12 @@
-
-
+
+
@@ -313,6 +259,8 @@
ap.srcs += $(SRC_SUBSYSTEMS)/imu.c
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
diff --git a/conf/modules/ins_ppzuavimu.xml b/conf/modules/ins_ppzuavimu.xml
index b6779b0f05..9537dbe34d 100644
--- a/conf/modules/ins_ppzuavimu.xml
+++ b/conf/modules/ins_ppzuavimu.xml
@@ -5,13 +5,16 @@
+
+
+
-
+
diff --git a/sw/airborne/firmwares/fixedwing/main_ap.c b/sw/airborne/firmwares/fixedwing/main_ap.c
index 0cd47e82c3..45fba32463 100644
--- a/sw/airborne/firmwares/fixedwing/main_ap.c
+++ b/sw/airborne/firmwares/fixedwing/main_ap.c
@@ -75,7 +75,7 @@
#include "subsystems/ahrs.h"
#include "subsystems/ahrs/ahrs_aligner.h"
#include "subsystems/ahrs/ahrs_float_dcm.h"
-static inline void on_gyro_accel_event( void );
+static inline void on_gyro_event( void );
static inline void on_accel_event( void );
static inline void on_mag_event( void );
#endif
@@ -398,7 +398,7 @@ static inline void attitude_loop( void ) {
v_ctl_throttle_slew();
ap_state->commands[COMMAND_THROTTLE] = v_ctl_throttle_slewed;
ap_state->commands[COMMAND_ROLL] = h_ctl_aileron_setpoint;
-
+
ap_state->commands[COMMAND_PITCH] = h_ctl_elevator_setpoint;
#if defined MCU_SPI_LINK
@@ -608,7 +608,7 @@ void event_task_ap( void ) {
#endif
#ifdef USE_AHRS
- ImuEvent(on_gyro_accel_event, on_accel_event, on_mag_event);
+ ImuEvent(on_gyro_event, on_accel_event, on_mag_event);
#endif // USE_AHRS
#ifdef USE_GPS
@@ -630,7 +630,7 @@ void event_task_ap( void ) {
}
modules_event_task();
-
+
#ifdef AHRS_TRIGGERED_ATTITUDE_LOOP
if (new_ins_attitude > 0)
{
@@ -639,7 +639,7 @@ void event_task_ap( void ) {
new_ins_attitude = 0;
}
#endif
-
+
} /* event_task_ap() */
@@ -656,7 +656,7 @@ static inline void on_gps_solution( void ) {
static inline void on_accel_event( void ) {
}
-static inline void on_gyro_accel_event( void ) {
+static inline void on_gyro_event( void ) {
#ifdef AHRS_CPU_LED
LED_ON(AHRS_CPU_LED);
diff --git a/sw/airborne/modules/ins/ins_ppzuavimu.c b/sw/airborne/modules/ins/ins_ppzuavimu.c
index 3da102b6be..73cf7ce5e7 100644
--- a/sw/airborne/modules/ins/ins_ppzuavimu.c
+++ b/sw/airborne/modules/ins/ins_ppzuavimu.c
@@ -106,10 +106,10 @@ void ppzuavimu_module_init( void )
i2c_submit(&PPZUAVIMU_I2C_DEVICE,&ppzuavimu_adxl345);
while(ppzuavimu_adxl345.status == I2CTransPending);
- /* Set range to 4g */
+ /* 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 | 0x01; // 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.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);
@@ -162,10 +162,13 @@ void ppzuavimu_module_periodic( void )
ppzuavimu_hmc5843.len_w = 1;
ppzuavimu_hmc5843.buf[0] = HMC5843_REG_DATXM;
i2c_submit(&PPZUAVIMU_I2C_DEVICE, &ppzuavimu_hmc5843);
+}
- RunOnceEvery(6,DOWNLINK_SEND_IMU_GYRO_RAW(DefaultChannel,&gyr_x,&gyr_y,&gyr_z));
- RunOnceEvery(6,DOWNLINK_SEND_IMU_ACCEL_RAW(DefaultChannel,&acc_x,&acc_y,&acc_z));
- RunOnceEvery(6,DOWNLINK_SEND_IMU_MAG_RAW(DefaultChannel,&mag_x,&mag_y,&mag_z));
+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);
}
void ppzuavimu_module_event( void )
@@ -176,7 +179,15 @@ void ppzuavimu_module_event( void )
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]);
+
+#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);
+#endif
+
gyr_valid = TRUE;
+ ppzuavimu_itg3200.status = I2CTransDone;
}
// If the adxl345 I2C transaction has succeeded: convert the data
@@ -185,7 +196,11 @@ void ppzuavimu_module_event( void )
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]);
+
+ VECT3_ASSIGN(imu.accel_unscaled, acc_x, acc_y, acc_z);
+
acc_valid = TRUE;
+ ppzuavimu_adxl345.status = I2CTransDone;
}
// If the hmc5843 I2C transaction has succeeded: convert the data
@@ -195,5 +210,6 @@ void ppzuavimu_module_event( void )
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]);
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
index 82d0bed4ca..1da5a745d7 100644
--- a/sw/airborne/modules/ins/ins_ppzuavimu.h
+++ b/sw/airborne/modules/ins/ins_ppzuavimu.h
@@ -24,10 +24,6 @@
#include "std.h"
#include "subsystems/imu.h"
-extern int32_t mag_x, mag_y, mag_z;
-extern int32_t gyr_x, gyr_y, gyr_z;
-extern int32_t acc_x, acc_y, acc_z;
-
extern volatile bool_t gyr_valid;
extern volatile bool_t acc_valid;
extern volatile bool_t mag_valid;
@@ -36,11 +32,9 @@ extern volatile bool_t mag_valid;
extern void ppzuavimu_module_init( void );
extern void ppzuavimu_module_periodic( void );
extern void ppzuavimu_module_event( void );
+extern void ppzuavimu_module_downlink_raw( void );
-extern volatile bool_t analog_imu_available;
-extern int imu_overrun;
-
#define ImuEvent(_gyro_handler, _accel_handler, _mag_handler) { \
if (gyr_valid) { \
gyr_valid = FALSE; \
@@ -54,8 +48,7 @@ extern int imu_overrun;
mag_valid = FALSE; \
_mag_handler(); \
} \
- }
-
+}
#endif // PPZUAVIMU_H
diff --git a/sw/airborne/subsystems/ahrs/ahrs_float_dcm.c b/sw/airborne/subsystems/ahrs/ahrs_float_dcm.c
index 04e30d69db..62a5e78f5b 100644
--- a/sw/airborne/subsystems/ahrs/ahrs_float_dcm.c
+++ b/sw/airborne/subsystems/ahrs/ahrs_float_dcm.c
@@ -113,7 +113,7 @@ void ahrs_update_fw_estimator( void )
estimator_psi = ahrs_float.ltp_to_imu_euler.psi;
estimator_p = Omega_Vector[0];
-
+/*
RunOnceEvery(6,DOWNLINK_SEND_RMAT_DEBUG(DefaultChannel,
&(DCM_Matrix[0][0]),
&(DCM_Matrix[0][1]),
@@ -128,7 +128,7 @@ void ahrs_update_fw_estimator( void )
&(DCM_Matrix[2][2])
));
-
+*/
}
diff --git a/sw/airborne/subsystems/ahrs/ahrs_float_dcm.h b/sw/airborne/subsystems/ahrs/ahrs_float_dcm.h
index ea54f0c1d4..f731cf5a71 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