[rotorcraft] use new state interface instead of ins in guidance

paparazzi · Jul 12, 2012 · f5054fc · f5054fc
1 parent 4668031
commit f5054fc
Show file tree

Hide file tree

Showing 2 changed files with 14 additions and 13 deletions.
diff --git a/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.c b/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.c
@@ -266,12 +266,12 @@ static inline void guidance_h_update_reference(bool_t use_ref) {
 static inline void guidance_h_traj_run(bool_t in_flight) {
 
   /* compute position error    */
-  VECT2_DIFF(guidance_h_pos_err, guidance_h_pos_ref, ins_ltp_pos);
+  VECT2_DIFF(guidance_h_pos_err, guidance_h_pos_ref, *stateGetPositionNed_i());
   /* saturate it               */
   VECT2_STRIM(guidance_h_pos_err, -MAX_POS_ERR, MAX_POS_ERR);
 
   /* compute speed error    */
-  VECT2_DIFF(guidance_h_speed_err, guidance_h_speed_ref, ins_ltp_speed);
+  VECT2_DIFF(guidance_h_speed_err, guidance_h_speed_ref, *stateGetSpeedNed_i());
   /* saturate it               */
   VECT2_STRIM(guidance_h_speed_err, -MAX_SPEED_ERR, MAX_SPEED_ERR);
 
@@ -326,7 +326,7 @@ static inline void guidance_h_traj_run(bool_t in_flight) {
 
 static inline void guidance_h_hover_enter(void) {
 
-  VECT2_COPY(guidance_h_pos_sp, ins_ltp_pos);
+  VECT2_COPY(guidance_h_pos_sp, *stateGetPositionNed_i());
 
   guidance_h_rc_sp.psi = ahrs.ltp_to_body_euler.psi;
   reset_psi_ref_from_body();
@@ -340,8 +340,8 @@ static inline void guidance_h_nav_enter(void) {
   INT32_VECT2_NED_OF_ENU(guidance_h_pos_sp, navigation_carrot);
   struct Int32Vect2 pos,speed,zero;
   INT_VECT2_ZERO(zero);
-  VECT2_COPY(pos, ins_ltp_pos);
-  VECT2_COPY(speed, ins_ltp_speed);
+  VECT2_COPY(pos, *stateGetPositionNed_i());
+  VECT2_COPY(speed, *stateGetSpeedNed_i());
   GuidanceHSetRef(pos, speed, zero);
 
   /* reset psi reference, set psi setpoint to current psi */

diff --git a/sw/airborne/firmwares/rotorcraft/guidance/guidance_v.c b/sw/airborne/firmwares/rotorcraft/guidance/guidance_v.c
@@ -35,7 +35,8 @@
 // #include "booz_fms.h" FIXME
 #include "firmwares/rotorcraft/navigation.h"
 
-#include "subsystems/ins.h"
+#include "state.h"
+
 #include "math/pprz_algebra_int.h"
 
 #include "generated/airframe.h"
@@ -133,17 +134,17 @@ void guidance_v_mode_changed(uint8_t new_mode) {
 
   switch (new_mode) {
   case GUIDANCE_V_MODE_HOVER:
-    guidance_v_z_sp = ins_ltp_pos.z; // set current altitude as setpoint
+    guidance_v_z_sp = stateGetPositionNed_i()->z; // set current altitude as setpoint
     guidance_v_z_sum_err = 0;
-    GuidanceVSetRef(ins_ltp_pos.z, 0, 0);
+    GuidanceVSetRef(stateGetPositionNed_i()->z, 0, 0);
     break;
 
   case GUIDANCE_V_MODE_RC_CLIMB:
   case GUIDANCE_V_MODE_CLIMB:
     guidance_v_zd_sp = 0;
   case GUIDANCE_V_MODE_NAV:
     guidance_v_z_sum_err = 0;
-    GuidanceVSetRef(ins_ltp_pos.z, ins_ltp_speed.z, 0);
+    GuidanceVSetRef(stateGetPositionNed_i()->z, stateGetSpeedNed_i()->z, 0);
     break;
 
   default:
@@ -167,13 +168,13 @@ void guidance_v_run(bool_t in_flight) {
   // FIXME... SATURATIONS NOT TAKEN INTO ACCOUNT
   // AKA SUPERVISION and co
   if (in_flight) {
-    gv_adapt_run(ins_ltp_accel.z, stabilization_cmd[COMMAND_THRUST], guidance_v_zd_ref);
+    gv_adapt_run(stateGetAccelNed_i()->z, stabilization_cmd[COMMAND_THRUST], guidance_v_zd_ref);
   }
 
   switch (guidance_v_mode) {
 
   case GUIDANCE_V_MODE_RC_DIRECT:
-    guidance_v_z_sp = ins_ltp_pos.z; // for display only
+    guidance_v_z_sp = stateGetPositionNed_i()->z; // for display only
     stabilization_cmd[COMMAND_THRUST] = guidance_v_rc_delta_t;
     break;
 
@@ -261,9 +262,9 @@ __attribute__ ((always_inline)) static inline void run_hover_loop(bool_t in_flig
   guidance_v_zd_ref = gv_zd_ref<<(INT32_SPEED_FRAC - GV_ZD_REF_FRAC);
   guidance_v_zdd_ref = gv_zdd_ref<<(INT32_ACCEL_FRAC - GV_ZDD_REF_FRAC);
   /* compute the error to our reference */
-  int32_t err_z  = guidance_v_z_ref - ins_ltp_pos.z;
+  int32_t err_z  = guidance_v_z_ref - stateGetPositionNed_i()->z;
   Bound(err_z, GUIDANCE_V_MIN_ERR_Z, GUIDANCE_V_MAX_ERR_Z);
-  int32_t err_zd = guidance_v_zd_ref - ins_ltp_speed.z;
+  int32_t err_zd = guidance_v_zd_ref - stateGetSpeedNed_i()->z;
   Bound(err_zd, GUIDANCE_V_MIN_ERR_ZD, GUIDANCE_V_MAX_ERR_ZD);
 
   if (in_flight) {