diff --git a/conf/airframes/example_twog_analogimu.xml b/conf/airframes/example_twog_analogimu.xml
index 391bd21495..2705ce0ed4 100644
--- a/conf/airframes/example_twog_analogimu.xml
+++ b/conf/airframes/example_twog_analogimu.xml
@@ -29,7 +29,7 @@
-
+
@@ -45,22 +45,39 @@
-
-
-
+
+
+
+
+
+
+
+
-
-
-
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
-
-
+
+
-
+
-
-
+
+
-
-
+
+
ap.CFLAGS += -DANALOGIMU_ROTATED
ap.CFLAGS += -DANALOGIMU_ZERO_AVERAGE
-
- ap.CFLAGS += -DGPS_USE_LATLONG
-
- //ap.CFLAGS += -DWIND_INFO -DSTRONG_WIND
-
- //sim.CFLAGS += -DWIND_INFO -DSTRONG_WIND
-
-
-
-
diff --git a/conf/autopilot/subsystems/fixedwing/attitude_analogimu.makefile b/conf/autopilot/subsystems/fixedwing/attitude_analogimu.makefile
index 625f80613a..1503e3d091 100644
--- a/conf/autopilot/subsystems/fixedwing/attitude_analogimu.makefile
+++ b/conf/autopilot/subsystems/fixedwing/attitude_analogimu.makefile
@@ -7,7 +7,7 @@ ap.CFLAGS += -DUSE_ANALOG_IMU -DADC -DUSE_ADC_0 -DUSE_ADC_1 -DUSE_ADC_2 -DUSE_AD
ap.srcs += $(SRC_SUBSYSTEMS)/ahrs/dcm/dcm.c
ap.srcs += $(SRC_SUBSYSTEMS)/ahrs/dcm/analogimu.c
ap.srcs += $(SRC_SUBSYSTEMS)/imu/imu_analog.c
-ap.srcs += $(SRC_SUBSYSTEMS)/ahrs/dcm/analogimu_util.c
+ap.srcs += $(SRC_SUBSYSTEMS)/imu.c
endif
diff --git a/sw/airborne/firmwares/fixedwing/main_ap.c b/sw/airborne/firmwares/fixedwing/main_ap.c
index 90343fb2f9..84d3c31ab3 100644
--- a/sw/airborne/firmwares/fixedwing/main_ap.c
+++ b/sw/airborne/firmwares/fixedwing/main_ap.c
@@ -66,7 +66,10 @@
#ifdef USE_ANALOG_IMU
+#include "subsystems/imu/imu_analog.h"
#include "subsystems/ahrs/dcm/analogimu.h"
+static inline void on_gyro_accel_event( void );
+static inline void on_mag_event( void );
#endif
#if ! defined CATASTROPHIC_BAT_LEVEL && defined LOW_BATTERY
@@ -437,9 +440,9 @@ void periodic_task_ap( void ) {
#ifdef USE_ANALOG_IMU
if (!_20Hz) {
- estimator_update_state_analog_imu();
- analog_imu_downlink();
- }
+ imu_periodic();
+ //analog_imu_downlink();
+ }
#endif // USE_ANALOG_IMU
#if CONTROL_RATE == 20
@@ -495,7 +498,6 @@ void init_ap( void ) {
GpioInit();
#endif
-
#ifdef USE_ANALOG_IMU
analog_imu_init();
#endif
@@ -560,6 +562,10 @@ void init_ap( void ) {
/*********** EVENT ***********************************************************/
void event_task_ap( void ) {
+#ifdef USE_ANALOG_IMU
+ ImuEvent(on_gyro_accel_event, on_mag_event);
+#endif // USE_ANALOG_IMU
+
#ifdef USE_GPS
#if !(defined HITL) && !(defined UBX_EXTERNAL) /** else comes through the datalink */
if (GpsBuffer()) {
@@ -630,3 +636,15 @@ void event_task_ap( void ) {
modules_event_task();
} /* event_task_ap() */
+
+#ifdef USE_ANALOG_IMU
+static inline void on_gyro_accel_event( void ) {
+ ImuScaleGyro(imu);
+ ImuScaleAccel(imu);
+ estimator_update_state_analog_imu();
+}
+
+static inline void on_mag_event(void) {
+ //ImuScaleMag(imu);
+}
+#endif // USE_ANALOG_IMU
diff --git a/sw/airborne/subsystems/ahrs/dcm/analogimu.c b/sw/airborne/subsystems/ahrs/dcm/analogimu.c
index a8e7964cf7..ff02fed074 100644
--- a/sw/airborne/subsystems/ahrs/dcm/analogimu.c
+++ b/sw/airborne/subsystems/ahrs/dcm/analogimu.c
@@ -47,6 +47,7 @@
//#include "downlink.h"
//#include "ap_downlink.h"
#include "sys_time.h"
+#include "math/pprz_algebra_int.h"
#endif
@@ -61,62 +62,24 @@ float imu_pitch_neutral = RadOfDeg(IMU_PITCH_NEUTRAL_DEFAULT);
#if ! (defined SITL || defined HITL)
-// functions
-/**
- * accel2ms2():
- *
- * \return accel[ACC_X], accel[ACC_Y], accel[ACC_Z]
- */
-static void accel2ms2( void ) {
- accel[ACC_X] = (float)(adc_average[3]) * IMU_ACCEL_X_SENS;
- accel[ACC_Y] = (float)(adc_average[4]) * IMU_ACCEL_Y_SENS;
- accel[ACC_Z] = (float)(adc_average[5]) * IMU_ACCEL_Z_SENS;
-}
-/**
- * gyro2rads():
- *
- * \return gyro[G_ROLL], gyro[G_PITCH], gyro[G_YAW]
- */
-static void gyro2rads( void ) {
- /** 150 grad/sec 10Bit, 3,3Volt, 1rad = 2Pi/1024 => Pi/512 */
- gyro[G_ROLL] = (float)(adc_average[0]) * IMU_GYRO_P_SENS;
- gyro[G_PITCH] = (float)(adc_average[1]) * IMU_GYRO_Q_SENS;
- gyro[G_YAW] = (float)(adc_average[2]) * IMU_GYRO_R_SENS;
-}
-
void analog_imu_init( void ) {
imu_impl_init();
}
void analog_imu_offset_set( void ) {
- uint8_t i;
-
- // read IMU
+ // read IMU, really?
imu_periodic();
- for(i = 0; i < NB_ANALOG_IMU_ADC; i++) {
- analog_imu_offset[i] = analog_imu_values[i];
- }
+ imu.gyro_neutral.p = analog_imu_values[0];
+ imu.gyro_neutral.q = analog_imu_values[1];
+ imu.gyro_neutral.r = analog_imu_values[2];
+ imu.accel_neutral.x = analog_imu_values[3];
+ imu.accel_neutral.y = analog_imu_values[4];
+ imu.accel_neutral.z = analog_imu_values[5];
// Z channel should read
- analog_imu_offset[5] += (9.81f / IMU_ACCEL_Z_SENS);
-}
-/**
- * analog_imu_update():
- */
-
-void analog_imu_update( void ) {
- uint8_t i;
-
- // read IMU
- imu_periodic();
-
- for(i = 0; i < NB_ANALOG_IMU_ADC; i++) {
- adc_average[i] -= analog_imu_offset[i];
- }
-
- accel2ms2();
- gyro2rads();
+ // FIXME uugh...
+ //imu.accel_neutral.z += (9.81f / IMU_ACCEL_Z_SENS);
}
// functions
@@ -134,16 +97,25 @@ void analog_imu_downlink( void ) {
void estimator_update_state_analog_imu( void ) {
- analog_imu_update();
-
/* Offset is set dynamic on Ground*/
- Gyro_Vector[0]= -gyro_to_zero[G_ROLL] + gyro[G_ROLL];
+ /*Gyro_Vector[0]= -gyro_to_zero[G_ROLL] + gyro[G_ROLL];
Gyro_Vector[1]= -gyro_to_zero[G_PITCH] + gyro[G_PITCH];
- Gyro_Vector[2]= -gyro_to_zero[G_PITCH] + gyro[G_YAW];
+ Gyro_Vector[2]= -gyro_to_zero[G_PITCH] + gyro[G_YAW];*/
- Accel_Vector[0] = accel[ACC_X];
- Accel_Vector[1] = accel[ACC_Y];
- Accel_Vector[2] = accel[ACC_Z];
+ //FIXME run aligner to set gyro neutrals on ground
+
+ /* converts gyro to floating point */
+ struct FloatRates gyro_float;
+ RATES_FLOAT_OF_BFP(gyro_float, imu.gyro);
+ Gyro_Vector[0]= gyro_float.p;
+ Gyro_Vector[1]= gyro_float.q;
+ Gyro_Vector[2]= gyro_float.r;
+
+ struct FloatVect3 accel_float;
+ ACCELS_FLOAT_OF_BFP(accel_float, imu.accel);
+ Accel_Vector[0] = accel_float.x;
+ Accel_Vector[1] = accel_float.y;
+ Accel_Vector[2] = accel_float.z;
Matrix_update();