From 85fc5319c518e8f7c9328efcac4af348f5fe2bae Mon Sep 17 00:00:00 2001
From: Gautier Hattenberger <gautier.hattenberger@enac.fr>
Date: Tue, 21 Aug 2012 15:05:12 +0200
Subject: [PATCH] [ahrs] infrared ahrs not using old ahrs structure

---
 sw/airborne/subsystems/ahrs/ahrs_infrared.c | 63 +++++++--------------
 sw/airborne/subsystems/ahrs/ahrs_infrared.h |  3 -
 2 files changed, 20 insertions(+), 46 deletions(-)

diff --git a/sw/airborne/subsystems/ahrs/ahrs_infrared.c b/sw/airborne/subsystems/ahrs/ahrs_infrared.c
index c95c966a3ed..f5270a4390a 100644
--- a/sw/airborne/subsystems/ahrs/ahrs_infrared.c
+++ b/sw/airborne/subsystems/ahrs/ahrs_infrared.c
@@ -28,22 +28,12 @@
 #include "state.h"
 #include "estimator.h" // for wind FIXME use state interface
 
-
+float heading;
 
 void ahrs_init(void) {
   ahrs.status = AHRS_UNINIT;
 
-  /* set ltp_to_body to zero */
-  FLOAT_QUAT_ZERO(ahrs_float.ltp_to_body_quat);
-  FLOAT_EULERS_ZERO(ahrs_float.ltp_to_body_euler);
-  FLOAT_RMAT_ZERO(ahrs_float.ltp_to_body_rmat);
-  FLOAT_RATES_ZERO(ahrs_float.body_rate);
-
-  /* set ltp_to_body to zero */
-  FLOAT_QUAT_ZERO(ahrs_float.ltp_to_imu_quat);
-  FLOAT_EULERS_ZERO(ahrs_float.ltp_to_imu_euler);
-  FLOAT_RMAT_ZERO(ahrs_float.ltp_to_imu_rmat);
-  FLOAT_RATES_ZERO(ahrs_float.imu_rate);
+  heading = 0.;
 }
 
 void ahrs_align(void) {
@@ -54,15 +44,17 @@ void ahrs_align(void) {
 }
 
 void ahrs_propagate(void) {
+  struct FloatRates body_rate = { 0., 0., 0. };
 #ifdef ADC_CHANNEL_GYRO_P
-  ahrs_float.body_rate.p = RATE_FLOAT_OF_BFP(imu.gyro.p);
+  body_rate.p = RATE_FLOAT_OF_BFP(imu.gyro.p);
 #endif
 #ifdef ADC_CHANNEL_GYRO_Q
-  ahrs_float.body_rate.q = RATE_FLOAT_OF_BFP(imu.gyro.q);
+  body_rate.q = RATE_FLOAT_OF_BFP(imu.gyro.q);
 #endif
 #ifdef ADC_CHANNEL_GYRO_R
-  ahrs_float.body_rate.r = RATE_FLOAT_OF_BFP(imu.gyro.r);
+  body_rate.r = RATE_FLOAT_OF_BFP(imu.gyro.r);
 #endif
+  stateSetBodyRates_f(&body_rate);
 }
 
 void ahrs_update_accel(void) {
@@ -80,42 +72,27 @@ void ahrs_update_gps(void) {
   // wind_north and wind_east initialized to 0, so still correct if not updated
   float w_vn = cosf(course_f) * hspeed_mod_f - wind_north;
   float w_ve = sinf(course_f) * hspeed_mod_f - wind_east;
-  ahrs_float.ltp_to_body_euler.psi = atan2f(w_ve, w_vn);
-  if (ahrs_float.ltp_to_body_euler.psi < 0.)
-    ahrs_float.ltp_to_body_euler.psi += 2 * M_PI;
+  heading = atan2f(w_ve, w_vn);
+  if (heading < 0.)
+    heading += 2 * M_PI;
 
-  ahrs_update_fw_estimator();
 }
 
 void ahrs_update_infrared(void) {
-  ahrs_float.ltp_to_body_euler.phi  = atan2(infrared.roll, infrared.top) - infrared.roll_neutral;
+  float phi  = atan2(infrared.roll, infrared.top) - infrared.roll_neutral;
+  float theta  = atan2(infrared.pitch, infrared.top) - infrared.pitch_neutral;
 
-  ahrs_float.ltp_to_body_euler.theta  = atan2(infrared.pitch, infrared.top) - infrared.pitch_neutral;
+  if (theta < -M_PI_2) theta += M_PI;
+  else if (theta > M_PI_2) theta -= M_PI;
 
-  if (ahrs_float.ltp_to_body_euler.theta < -M_PI_2)
-    ahrs_float.ltp_to_body_euler.theta += M_PI;
-  else if (ahrs_float.ltp_to_body_euler.theta > M_PI_2)
-    ahrs_float.ltp_to_body_euler.theta -= M_PI;
+  if (phi >= 0) phi *= infrared.correction_right;
+  else phi *= infrared.correction_left;
 
-  if (ahrs_float.ltp_to_body_euler.phi >= 0)
-    ahrs_float.ltp_to_body_euler.phi *= infrared.correction_right;
-  else
-    ahrs_float.ltp_to_body_euler.phi *= infrared.correction_left;
+  if (theta >= 0) theta *= infrared.correction_up;
+  else theta *= infrared.correction_down;
 
-  if (ahrs_float.ltp_to_body_euler.theta >= 0)
-    ahrs_float.ltp_to_body_euler.theta *= infrared.correction_up;
-  else
-    ahrs_float.ltp_to_body_euler.theta *= infrared.correction_down;
+  struct FloatEulers att = { phi, theta, heading };
+  stateSetNedToBodyEulers_f(&att);
 
-  ahrs_update_fw_estimator();
 }
 
-
-// TODO use ahrs result directly
-void ahrs_update_fw_estimator(void)
-{
-  // export results to estimator
-  stateSetNedToBodyEulers_f(&ahrs_float.ltp_to_body_euler);
-  stateSetBodyRates_f(&ahrs_float.body_rate);
-
-}
diff --git a/sw/airborne/subsystems/ahrs/ahrs_infrared.h b/sw/airborne/subsystems/ahrs/ahrs_infrared.h
index a4d28707560..02c677260f8 100644
--- a/sw/airborne/subsystems/ahrs/ahrs_infrared.h
+++ b/sw/airborne/subsystems/ahrs/ahrs_infrared.h
@@ -33,7 +33,4 @@ extern float ins_pitch_neutral;
 
 extern void ahrs_update_infrared(void);
 
-// TODO copy ahrs to state instead of estimator
-extern void ahrs_update_fw_estimator(void);
-
 #endif /* AHRS_INFRARED_H */