Added defaults for px4 imu

conf/airframes/TUDELFT/tudelft_conf.xml

@@ -116,7 +116,7 @@
    radio="radios/FrSky3dr.xml"
    telemetry="telemetry/default_rotorcraft_slow.xml"
    flight_plan="flight_plans/TUDELFT/tudelft_delft_basic.xml"
-   settings="settings/rotorcraft_basic.xml settings/control/rotorcraft_guidance.xml [settings/control/stabilization_att_int_quat.xml] settings/control/stabilization_indi.xml"
+   settings="settings/rotorcraft_basic.xml settings/control/rotorcraft_guidance.xml [settings/control/stabilization_att_int_quat.xml] settings/control/stabilization_indi.xml settings/estimation/body_to_imu.xml"
    settings_modules="modules/geo_mag.xml modules/air_data.xml modules/gps_ubx_ucenter.xml"
    gui_color="#ffffcccaccca"
   />

conf/airframes/TUDELFT/tudelft_iris_indi.xml

@@ -97,20 +97,19 @@
   </section>
   <section name="IMU" prefix="IMU_">
     <!-- replace this with your own calibration -->
-    <define name="ACCEL_X_NEUTRAL" value="274"/>
+    <define name="ACCEL_X_NEUTRAL" value="248"/>
-    <define name="ACCEL_Y_NEUTRAL" value="-43"/>
+    <define name="ACCEL_Y_NEUTRAL" value="-21"/>
-    <define name="ACCEL_Z_NEUTRAL" value="1112"/>
+    <define name="ACCEL_Z_NEUTRAL" value="1123"/>
-    <define name="ACCEL_X_SENS" value="0.625515268183" integer="16"/>
+    <define name="ACCEL_X_SENS" value="0.62584356013" integer="16"/>
-    <define name="ACCEL_Y_SENS" value="0.609477051286" integer="16"/>
+    <define name="ACCEL_Y_SENS" value="0.609921478775" integer="16"/>
-    <define name="ACCEL_Z_SENS" value="0.62150684522" integer="16"/>
+    <define name="ACCEL_Z_SENS" value="0.619923942578" integer="16"/>
-
-    <define name="MAG_X_NEUTRAL" value="-151"/>
-    <define name="MAG_Y_NEUTRAL" value="-288"/>
-    <define name="MAG_Z_NEUTRAL" value="-3949"/>
-    <define name="MAG_X_SENS" value="0.420984584445" integer="16"/>
-    <define name="MAG_Y_SENS" value="0.447747800597" integer="16"/>
-    <define name="MAG_Z_SENS" value="0.431245648647" integer="16"/>
 
+    <define name="MAG_X_NEUTRAL" value="21"/>
+    <define name="MAG_Y_NEUTRAL" value="-93"/>
+    <define name="MAG_Z_NEUTRAL" value="-15"/>
+    <define name="MAG_X_SENS" value="5.1784060163" integer="16"/>
+    <define name="MAG_Y_SENS" value="5.55867516912" integer="16"/>
+    <define name="MAG_Z_SENS" value="5.06809390796" integer="16"/>
   </section>
   <commands>
     <axis name="PITCH" failsafe_value="0" />

sw/airborne/subsystems/imu/imu_px4_defaults.h

@@ -0,0 +1,64 @@
+/*
+ * Copyright (C) 2013 Felix Ruess <felix.ruess@gmail.com>
+ *
+ * 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.
+ */
+
+/**
+ * @file subsystems/imu/imu_px4_defaults.h
+ * Default sensitivity definitions for the Pixhawk IMU using the l3d20 gyro and lsm303dlc acc.
+ */
+
+#ifndef IMU_PX4_DEFAULTS_H
+#define IMU_PX4_DEFAULTS_H
+
+#include "generated/airframe.h"
+
+/** default gyro sensitivy and neutral from the datasheet
+ * L3GD20 has 70e-3 LSB/(deg/s) at 2000deg/s range
+ * sens = 70e-3 * pi/180 * 2^INT32_RATE_FRAC
+ * sens = (70e-3 / 180.0f) * pi * 4096
+  */
+#if !defined IMU_GYRO_P_SENS & !defined IMU_GYRO_Q_SENS & !defined IMU_GYRO_R_SENS
+#define IMU_GYRO_P_SENS_NUM 5004
+#define IMU_GYRO_P_SENS_DEN 1000
+#define IMU_GYRO_Q_SENS_NUM 5004
+#define IMU_GYRO_Q_SENS_DEN 1000
+#define IMU_GYRO_R_SENS_NUM 5004
+#define IMU_GYRO_R_SENS_DEN 1000
+#endif
+
+/** default accel sensitivy from the datasheet
+ * LSM303DLHC has 732 LSB/g
+ * fixed point sens: 9.81 [m/s^2] / 732 [LSB/g] * 2^INT32_ACCEL_FRAC
+ * sens = 9.81 / 732 * 1024 = 13.72
+ */
+#if !defined IMU_ACCEL_X_SENS & !defined IMU_ACCEL_Y_SENS & !defined IMU_ACCEL_Z_SENS
+
+#define IMU_ACCEL_X_SENS 13.723
+#define IMU_ACCEL_X_SENS_NUM 13723
+#define IMU_ACCEL_X_SENS_DEN 1000
+#define IMU_ACCEL_Y_SENS 13.723
+#define IMU_ACCEL_Y_SENS_NUM 13723
+#define IMU_ACCEL_Y_SENS_DEN 1000
+#define IMU_ACCEL_Z_SENS 13.723
+#define IMU_ACCEL_Z_SENS_NUM 13723
+#define IMU_ACCEL_Z_SENS_DEN 1000
+#endif
+
+#endif /* IMU_PX4_DEFAULTS_H */

sw/airborne/subsystems/imu/imu_px4fmu_v2.4.c

@@ -49,7 +49,7 @@ void imu_impl_init(void)
 
   /* LSM303dlhc acc + magneto init */
   lsm303dlhc_spi_init(&imu_px4.lsm_acc, &IMU_SPI_DEV, IMU_LSM_SPI_SLAVE_IDX, LSM_TARGET_ACC);
-#if MODULE_HMC58XX_UPDATE_AHRS
+#if !MODULE_HMC58XX_UPDATE_AHRS
   lsm303dlhc_spi_init(&imu_px4.lsm_mag, &IMU_SPI_DEV, IMU_LSM_SPI_SLAVE_IDX, LSM_TARGET_MAG);
 #endif
 
@@ -60,7 +60,7 @@ void imu_periodic(void)
   l3gd20_spi_periodic(&imu_px4.l3g);
   lsm303dlhc_spi_periodic(&imu_px4.lsm_acc);
 
-#if MODULE_HMC58XX_UPDATE_AHRS
+#if !MODULE_HMC58XX_UPDATE_AHRS
   /* Read magneto's every 10 times of main freq
    * at ~50Hz (main loop for rotorcraft: 512Hz)
    */
@@ -93,7 +93,7 @@ void imu_px4_event(void)
     imu_scale_accel(&imu);
     AbiSendMsgIMU_ACCEL_INT32(IMU_PX4_ID, now_ts, &imu.accel);
   }
-#if MODULE_HMC58XX_UPDATE_AHRS
+#if !MODULE_HMC58XX_UPDATE_AHRS
   lsm303dlhc_spi_event(&imu_px4.lsm_mag);
   if (imu_px4.lsm_mag.data_available_mag) {
     VECT3_COPY(imu.mag_unscaled, imu_px4.lsm_mag.data_mag.vect);