From 1987d26b88b1a737ae3c7e6ecde04809f9886218 Mon Sep 17 00:00:00 2001
From: Felix Ruess <felix.ruess@gmail.com>
Date: Tue, 23 Aug 2011 17:34:05 +0200
Subject: [PATCH] add possibilty to use ahrs_int_cmpl_euler for fixedwings

---
 .../subsystems/ahrs/ahrs_int_cmpl_euler.c     | 31 +++++++++++++++++++
 .../subsystems/ahrs/ahrs_int_cmpl_euler.h     |  8 +++++
 2 files changed, 39 insertions(+)

diff --git a/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_euler.c b/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_euler.c
index 12d6926b48..edc6893033 100644
--- a/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_euler.c
+++ b/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_euler.c
@@ -222,3 +222,34 @@ __attribute__ ((always_inline)) static inline void compute_body_orientation(void
   INT32_RMAT_TRANSP_RATEMULT(ahrs.body_rate, imu.body_to_imu_rmat, ahrs.imu_rate);
 
 }
+
+
+#ifdef AHRS_UPDATE_FW_ESTIMATOR
+// TODO use ahrs result directly
+#include "estimator.h"
+// remotely settable
+#ifndef INS_ROLL_NEUTRAL_DEFAULT
+#define INS_ROLL_NEUTRAL_DEFAULT 0
+#endif
+#ifndef INS_PITCH_NEUTRAL_DEFAULT
+#define INS_PITCH_NEUTRAL_DEFAULT 0
+#endif
+float ins_roll_neutral = INS_ROLL_NEUTRAL_DEFAULT;
+float ins_pitch_neutral = INS_PITCH_NEUTRAL_DEFAULT;
+void ahrs_update_fw_estimator(void)
+{
+  struct FloatEulers att;
+  // export results to estimator
+  EULERS_FLOAT_OF_BFP(att, ahrs.ltp_to_body_euler);
+
+  estimator_phi   = att.phi - ins_roll_neutral;
+  estimator_theta = att.theta - ins_pitch_neutral;
+  estimator_psi   = att.psi;
+
+  struct FloatRates rates;
+  RATES_FLOAT_OF_BFP(rates, ahrs.body_rate);
+  estimator_p = rates.p;
+  estimator_q = rates.q;
+
+}
+#endif //AHRS_UPDATE_FW_ESTIMATOR
diff --git a/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_euler.h b/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_euler.h
index d707cfe6c2..b6ad467aa4 100644
--- a/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_euler.h
+++ b/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_euler.h
@@ -37,4 +37,12 @@ struct AhrsIntCmplEuler {
 
 extern struct AhrsIntCmplEuler ahrs_impl;
 
+
+#ifdef AHRS_UPDATE_FW_ESTIMATOR
+// TODO copy ahrs to state instead of estimator
+void ahrs_update_fw_estimator(void);
+extern float ins_roll_neutral;
+extern float ins_pitch_neutral;
+#endif
+
 #endif /* AHRS_INT_CMPL_EULER_H */