From d054afb5577d07b788663ad8c15758abdae08b7d Mon Sep 17 00:00:00 2001
From: Gautier Hattenberger <gautier.hattenberger@enac.fr>
Date: Tue, 24 Jul 2012 15:18:33 +0200
Subject: [PATCH] [cam control] use state interface in rotorcraft cam control

---
 sw/airborne/modules/cam_control/booz_cam.c  |  5 ++---
 sw/airborne/modules/cam_control/cam_track.c | 17 +++++++++++------
 2 files changed, 13 insertions(+), 9 deletions(-)

diff --git a/sw/airborne/modules/cam_control/booz_cam.c b/sw/airborne/modules/cam_control/booz_cam.c
index 20594d2ea39..ddd5dd3451d 100644
--- a/sw/airborne/modules/cam_control/booz_cam.c
+++ b/sw/airborne/modules/cam_control/booz_cam.c
@@ -24,9 +24,8 @@
 
 #include "cam_control/booz_cam.h"
 #include "modules/core/booz_pwm_arch.h"
-#include "subsystems/ahrs.h"
+#include "state.h"
 #include "firmwares/rotorcraft/navigation.h"
-#include "subsystems/ins.h"
 #include "generated/flight_plan.h"
 #include "std.h"
 
@@ -103,7 +102,7 @@ void booz_cam_periodic(void) {
       booz_cam_tilt_pwm = BOOZ_CAM_TILT_NEUTRAL;
 #endif
 #ifdef BOOZ_CAM_USE_PAN
-      booz_cam_pan = ahrs.ltp_to_body_euler.psi;
+      booz_cam_pan = stateGetNedToBodyEulers_i()->psi;
 #endif
       break;
     case BOOZ_CAM_MODE_MANUAL:
diff --git a/sw/airborne/modules/cam_control/cam_track.c b/sw/airborne/modules/cam_control/cam_track.c
index 78167c838aa..055123659be 100644
--- a/sw/airborne/modules/cam_control/cam_track.c
+++ b/sw/airborne/modules/cam_control/cam_track.c
@@ -25,7 +25,7 @@
 #include "cam_track.h"
 
 #include "subsystems/ins.h"
-#include "subsystems/ahrs.h"
+#include "state.h"
 
 #if USE_HFF
 #include "subsystems/ins/hf_float.h"
@@ -72,7 +72,8 @@ void track_periodic_task(void) {
 
   cmd_msg[c++] = 'A';
   cmd_msg[c++] = ' ';
-  float phi = ANGLE_FLOAT_OF_BFP(ahrs.ltp_to_body_euler.phi);
+  struct FloatEulers* att = stateGetNedToBodyEulers_f();
+  float phi = att->phi;
   if (phi > 0) cmd_msg[c++] = ' ';
   else { cmd_msg[c++] = '-'; phi = -phi; }
   cmd_msg[c++] = '0' + ((unsigned int) phi % 10);
@@ -81,7 +82,7 @@ void track_periodic_task(void) {
   cmd_msg[c++] = '0' + ((unsigned int) (1000*phi) % 10);
   cmd_msg[c++] = '0' + ((unsigned int) (10000*phi) % 10);
   cmd_msg[c++] = ' ';
-  float theta = ANGLE_FLOAT_OF_BFP(ahrs.ltp_to_body_euler.theta);
+  float theta = att->theta;
   if (theta > 0) cmd_msg[c++] = ' ';
   else { cmd_msg[c++] = '-'; theta = -theta; }
   cmd_msg[c++] = '0' + ((unsigned int) theta % 10);
@@ -90,7 +91,7 @@ void track_periodic_task(void) {
   cmd_msg[c++] = '0' + ((unsigned int) (1000*theta) % 10);
   cmd_msg[c++] = '0' + ((unsigned int) (10000*theta) % 10);
   cmd_msg[c++] = ' ';
-  float psi = ANGLE_FLOAT_OF_BFP(ahrs.ltp_to_body_euler.psi);
+  float psi = att->psi;
   if (psi > 0) cmd_msg[c++] = ' ';
   else { cmd_msg[c++] = '-'; psi = -psi; }
   cmd_msg[c++] = '0' + ((unsigned int) psi % 10);
@@ -99,7 +100,7 @@ void track_periodic_task(void) {
   cmd_msg[c++] = '0' + ((unsigned int) (1000*psi) % 10);
   cmd_msg[c++] = '0' + ((unsigned int) (10000*psi) % 10);
   cmd_msg[c++] = ' ';
-  float alt = -POS_FLOAT_OF_BFP(ins_ltp_pos.z);
+  float alt = stateGetPositionEnu_f()->z;
   //alt = 0.40;
   if (alt > 0) cmd_msg[c++] = ' ';
   else { cmd_msg[c++] = '-'; alt = -alt; }
@@ -133,7 +134,7 @@ void track_event(void) {
     pos.y = -target_pos_ned.y;
     ins_realign_h(pos, zero);
   }
-  const stuct FlotVect2 measuremet_noise = { 10.0, 10.0);
+  const stuct FlotVect2 measuremet_noise = { 10.0, 10.0 };
   b2_hff_update_pos(-target_pos_ned, measurement_noise);
   ins_ltp_accel.x = ACCEL_BFP_OF_REAL(b2_hff_state.xdotdot);
   ins_ltp_accel.y = ACCEL_BFP_OF_REAL(b2_hff_state.ydotdot);
@@ -141,6 +142,8 @@ void track_event(void) {
   ins_ltp_speed.y = SPEED_BFP_OF_REAL(b2_hff_state.ydot);
   ins_ltp_pos.x   = POS_BFP_OF_REAL(b2_hff_state.x);
   ins_ltp_pos.y   = POS_BFP_OF_REAL(b2_hff_state.y);
+
+  INS_NED_TO_STATE();
 #else
   // store pos in ins
   ins_ltp_pos.x = -(POS_BFP_OF_REAL(target_pos_ned.x));
@@ -149,6 +152,8 @@ void track_event(void) {
   // TODO get delta T
   // store last pos
   VECT3_COPY(last_pos_ned, target_pos_ned);
+
+  stateSetPositionNed_i(&ins_ltp_pos);
 #endif
 
   b2_hff_lost_counter = 0;