[stab att] converte stab attitude euler to new state interface

paparazzi · Jul 24, 2012 · c57e6c7 · c57e6c7
1 parent 5450ba7
commit c57e6c7
Show file tree

Hide file tree

Showing 3 changed files with 10 additions and 8 deletions.
diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_euler_int.c b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_euler_int.c
@@ -20,7 +20,7 @@
  */
 
 #include "firmwares/rotorcraft/stabilization.h"
-#include "subsystems/ahrs.h"
+#include "state.h"
 #include "subsystems/radio_control.h"
 
 #include "firmwares/rotorcraft/stabilization/stabilization_attitude_rc_setpoint.h"
@@ -87,7 +87,7 @@ void stabilization_attitude_read_rc(bool_t in_flight) {
 
 void stabilization_attitude_enter(void) {
 
-  stab_att_sp_euler.psi = ahrs.ltp_to_body_euler.psi;
+  stab_att_sp_euler.psi = stateGetNedToBodyEulers_i()->psi;
   reset_psi_ref_from_body();
   INT_EULERS_ZERO( stabilization_att_sum_err );
 
@@ -120,7 +120,8 @@ void stabilization_attitude_run(bool_t  in_flight) {
     OFFSET_AND_ROUND(stab_att_ref_euler.theta, (REF_ANGLE_FRAC - INT32_ANGLE_FRAC)),
     OFFSET_AND_ROUND(stab_att_ref_euler.psi,   (REF_ANGLE_FRAC - INT32_ANGLE_FRAC)) };
   struct Int32Eulers att_err;
-  EULERS_DIFF(att_err, att_ref_scaled, ahrs.ltp_to_body_euler);
+  struct Int32Eulers* ltp_to_body_euler = stateGetNedToBodyEulers_i();
+  EULERS_DIFF(att_err, att_ref_scaled, (*ltp_to_body_euler));
   INT32_ANGLE_NORMALIZE(att_err.psi);
 
   if (in_flight) {
@@ -138,7 +139,8 @@ void stabilization_attitude_run(bool_t  in_flight) {
     OFFSET_AND_ROUND(stab_att_ref_rate.q, (REF_RATE_FRAC - INT32_RATE_FRAC)),
     OFFSET_AND_ROUND(stab_att_ref_rate.r, (REF_RATE_FRAC - INT32_RATE_FRAC)) };
   struct Int32Rates rate_err;
-  RATES_DIFF(rate_err, rate_ref_scaled, ahrs.body_rate);
+  struct Int32Rates* body_rate = stateGetBodyRates_i();
+  RATES_DIFF(rate_err, rate_ref_scaled, (*body_rate));
 
   /* PID                  */
   stabilization_att_fb_cmd[COMMAND_ROLL] =

diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_rc_setpoint.h b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_rc_setpoint.h
@@ -32,7 +32,7 @@
 #include "math/pprz_algebra_float.h"
 
 #include "subsystems/radio_control.h"
-#include "subsystems/ahrs.h"
+#include "state.h"
 
 #ifdef STABILISATION_ATTITUDE_TYPE_INT
 #define SP_MAX_PHI     (int32_t)ANGLE_BFP_OF_REAL(STABILIZATION_ATTITUDE_SP_MAX_PHI)
@@ -80,7 +80,7 @@ static inline void stabilization_attitude_read_rc_setpoint_eulers(struct Int32Eu
     }
   }
   else { /* if not flying, use current yaw as setpoint */
-    sp->psi = ahrs.ltp_to_body_euler.psi;
+    sp->psi = stateGetNedToBodyEulers_i()->psi;
   }
 
 }

diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_int.h b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_int.h
@@ -29,7 +29,7 @@
 
 #include "math/pprz_algebra_int.h"
 
-#include "subsystems/ahrs.h"
+#include "state.h"
 
 extern struct Int32Eulers stab_att_sp_euler;  ///< with #INT32_ANGLE_FRAC
 extern struct Int32Quat   stab_att_sp_quat;   ///< with #INT32_QUAT_FRAC
@@ -58,7 +58,7 @@ extern struct Int32RefModel stab_att_ref_model;
 
 
 static inline void reset_psi_ref_from_body(void) {
-  stab_att_ref_euler.psi = ahrs.ltp_to_body_euler.psi << (REF_ANGLE_FRAC - INT32_ANGLE_FRAC);
+  stab_att_ref_euler.psi = stateGetNedToBodyEulers_i()->psi << (REF_ANGLE_FRAC - INT32_ANGLE_FRAC);
   stab_att_ref_rate.r = 0;
   stab_att_ref_accel.r = 0;
 }