From e84e488bf9c94b7143fa02af643f6c327c4b060b Mon Sep 17 00:00:00 2001
From: Felix Ruess <felix.ruess@gmail.com>
Date: Thu, 24 Jan 2013 19:18:17 +0100
Subject: [PATCH] [rotorcraft] settings for ahrs_int_cmpl_quat
 (gravity_heuristic), send gyro bias, update fraser to use gravity heuristic

---
 conf/airframes/fraser_lisa_m_rotorcraft.xml           |  4 +++-
 .../subsystems/rotorcraft/ahrs_int_cmpl_quat.makefile |  2 +-
 conf/settings/estimation/ahrs_int_cmpl_quat.xml       | 11 +++++++++++
 conf/telemetry/default_rotorcraft.xml                 |  3 ++-
 sw/airborne/firmwares/rotorcraft/telemetry.h          | 11 +++++++++++
 5 files changed, 28 insertions(+), 3 deletions(-)
 create mode 100644 conf/settings/estimation/ahrs_int_cmpl_quat.xml

diff --git a/conf/airframes/fraser_lisa_m_rotorcraft.xml b/conf/airframes/fraser_lisa_m_rotorcraft.xml
index d88a3716a61..7dfd5936e54 100644
--- 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
index 1325776ead1..0e59aba5602 100644
--- 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
new file mode 100644
index 00000000000..920f3c0dc52
--- /dev/null
+++ 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
index 4320ce6d1dc..51282743a66 100644
--- 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
index a4e3f80a864..208aae300c5 100644
--- 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"