[stab att] convert stab attitude quat to new state interface

paparazzi · Jul 24, 2012 · b2b006e · b2b006e
1 parent c633d34
commit b2b006e
Show file tree

Hide file tree

Showing 3 changed files with 10 additions and 9 deletions.
diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/quat_setpoint_int.c b/sw/airborne/firmwares/rotorcraft/stabilization/quat_setpoint_int.c
@@ -9,7 +9,7 @@
  *
  */
 
-#include "subsystems/ahrs.h"
+#include "state.h"
 
 #include "stabilization/stabilization_attitude_ref_quat_int.h"
 #include "stabilization/quat_setpoint_int.h"
@@ -94,7 +94,7 @@ void stabilization_attitude_read_rc_absolute(bool_t in_flight) {
     // update setpoint by rotating by incremental yaw command
     INT32_QUAT_COMP_NORM_SHORTEST(stab_att_sp_quat, prev_sp_quat, sticks_quat);
   } else { /* if not flying, use current body position + pitch/yaw from sticks to compose setpoint */
-    reset_sp_quat(RATE_BFP_OF_REAL(yaw * YAW_COEF), RATE_BFP_OF_REAL(pitch * PITCH_COEF), &ahrs.ltp_to_body_quat);
+    reset_sp_quat(RATE_BFP_OF_REAL(yaw * YAW_COEF), RATE_BFP_OF_REAL(pitch * PITCH_COEF), stateGetNedToBodyQuat_i());
   }
 
   // update euler setpoints for telemetry
@@ -104,5 +104,5 @@ void stabilization_attitude_read_rc_absolute(bool_t in_flight) {
 void stabilization_attitude_sp_enter(void)
 {
   // reset setpoint to "hover"
-  reset_sp_quat(0., 0., &ahrs.ltp_to_body_quat);
+  reset_sp_quat(0., 0., stateGetNedToBodyQuat_i());
 }
diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_int.c b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_int.c
@@ -33,7 +33,7 @@
 #include <stdio.h>
 #include "math/pprz_algebra_float.h"
 #include "math/pprz_algebra_int.h"
-#include "subsystems/ahrs.h"
+#include "state.h"
 #include "generated/airframe.h"
 
 struct Int32AttitudeGains stabilization_gains = {
@@ -80,7 +80,7 @@ void stabilization_attitude_enter(void) {
 
 #if !USE_SETPOINTS_WITH_TRANSITIONS
   /* reset psi setpoint to current psi angle */
-  stab_att_sp_euler.psi = ahrs.ltp_to_body_euler.psi;
+  stab_att_sp_euler.psi = stateGetNedToBodyEulers_i()->psi;
 #endif
 
   stabilization_attitude_ref_enter();
@@ -135,14 +135,16 @@ void stabilization_attitude_run(bool_t enable_integrator) {
 
   /* attitude error                          */
   struct Int32Quat att_err;
-  INT32_QUAT_INV_COMP(att_err, ahrs.ltp_to_body_quat, stab_att_ref_quat);
+  struct Int32Quat* att_quat = stateGetNedToBodyQuat_i();
+  INT32_QUAT_INV_COMP(att_err, *att_quat, stab_att_ref_quat);
   /* wrap it in the shortest direction       */
   INT32_QUAT_WRAP_SHORTEST(att_err);
   INT32_QUAT_NORMALIZE(att_err);
 
   /*  rate error                */
   struct Int32Rates rate_err;
-  RATES_DIFF(rate_err, stab_att_ref_rate, ahrs.body_rate);
+  struct Int32Rates* body_rate = stateGetBodyRates_i();
+  RATES_DIFF(rate_err, stab_att_ref_rate, *body_rate);
 
   /* integrated error */
   if (enable_integrator) {
@@ -194,7 +196,7 @@ void stabilization_attitude_read_rc(bool_t in_flight) {
   /* get current heading */
   const struct FloatVect3 zaxis = {0., 0., 1.};
   struct FloatQuat q_yaw;
-  FLOAT_QUAT_OF_AXIS_ANGLE(q_yaw, zaxis, ANGLE_FLOAT_OF_BFP(ahrs.ltp_to_body_euler.psi));
+  FLOAT_QUAT_OF_AXIS_ANGLE(q_yaw, zaxis, ANGLE_FLOAT_OF_BFP(stateGetNedToBodyEulers_i()->psi));
 
   /* apply roll and pitch commands with respect to current heading */
   struct FloatQuat q_sp;

diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_quat_int.c b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_quat_int.c
@@ -26,7 +26,6 @@
 
 #include "generated/airframe.h"
 #include "firmwares/rotorcraft/stabilization.h"
-#include "subsystems/ahrs.h"
 
 #include "stabilization_attitude_ref_int.h"