[rotorcraft] settings for ahrs_int_cmpl_quat (gravity_heuristic), sen…

…d gyro bias, update fraser to use gravity heuristic
paparazzi · Jan 24, 2013 · e84e488 · e84e488
1 parent 27dfbc0
commit e84e488
Show file tree

Hide file tree

Showing 5 changed files with 28 additions and 3 deletions.
diff --git a/conf/airframes/fraser_lisa_m_rotorcraft.xml b/conf/airframes/fraser_lisa_m_rotorcraft.xml
@@ -23,7 +23,9 @@
     <subsystem name="imu"           type="aspirin_v1.5"/>
     <subsystem name="gps"           type="ublox"/>
     <subsystem name="stabilization" type="int_quat"/>
-    <subsystem name="ahrs"          type="int_cmpl_quat"/>
+    <subsystem name="ahrs"          type="int_cmpl_quat">
+      <define name="AHRS_GRAVITY_UPDATE_NORM_HEURISTIC" value="TRUE"/>
+    </subsystem>
     <subsystem name="ins"/>
   </firmware>
 

diff --git a/conf/firmwares/subsystems/rotorcraft/ahrs_int_cmpl_quat.makefile b/conf/firmwares/subsystems/rotorcraft/ahrs_int_cmpl_quat.makefile
@@ -10,7 +10,7 @@ ifndef USE_MAGNETOMETER
 USE_MAGNETOMETER = 1
 endif
 
-AHRS_CFLAGS  = -DUSE_AHRS
+AHRS_CFLAGS  = -DUSE_AHRS -DUSE_AHRS_CMPL_QUAT
 AHRS_CFLAGS += -DUSE_AHRS_ALIGNER
 
 ifneq ($(USE_MAGNETOMETER),0)

diff --git a/conf/settings/estimation/ahrs_int_cmpl_quat.xml b/conf/settings/estimation/ahrs_int_cmpl_quat.xml
@@ -0,0 +1,11 @@
+<!DOCTYPE settings SYSTEM "../settings.dtd">
+
+<settings>
+  <dl_settings>
+
+    <dl_settings NAME="AHRS">
+       <dl_setting var="ahrs_impl.use_gravity_heuristic" min="0" step="1" max="1" module="subsystems/ahrs/ahrs_int_cmpl_quat" shortname="gravity_heuristic" values="OFF|ON"/>
+    </dl_settings>
+
+  </dl_settings>
+</settings>
diff --git a/conf/telemetry/default_rotorcraft.xml b/conf/telemetry/default_rotorcraft.xml
@@ -50,8 +50,9 @@
       <message name="ROTORCRAFT_STATUS"  period="1.2"/>
       <message name="DL_VALUE"           period="0.5"/>
       <message name="ALIVE"              period="2.1"/>
-<!--      <message name="FILTER_ALIGNER"     period=".1"/> -->
+      <message name="FILTER_ALIGNER"     period="2.2"/>
       <message name="FILTER"             period=".5"/>
+      <message name="AHRS_GYRO_BIAS_INT" period="0.08"/>
 <!--      <message name="AHRS_QUAT_INT"   period=".25"/> -->
       <message name="AHRS_EULER_INT"     period=".1"/>
 <!--      <message name="AHRS_RMAT_INT"   period=".5"/> -->

diff --git a/sw/airborne/firmwares/rotorcraft/telemetry.h b/sw/airborne/firmwares/rotorcraft/telemetry.h
@@ -358,6 +358,17 @@
 #define PERIODIC_SEND_FILTER(_trans, _dev) {}
 #endif
 
+#if USE_AHRS_CMPL_EULER || USE_AHRS_CMPL_QUAT
+#define PERIODIC_SEND_AHRS_GYRO_BIAS_INT(_trans, _dev) {    \
+  DOWNLINK_SEND_AHRS_GYRO_BIAS_INT(_trans, _dev,            \
+                                   &ahrs_impl.gyro_bias.p,  \
+                                   &ahrs_impl.gyro_bias.q,  \
+                                   &ahrs_impl.gyro_bias.r); \
+  }
+#else
+#define PERIODIC_SEND_AHRS_GYRO_BIAS_INT(_trans, _dev) {}
+#endif
+
 #if USE_AHRS_LKF
 #include "subsystems/ahrs.h"
 #include "ahrs/ahrs_float_lkf.h"