From 08cdc771ebcaa605130e783239e33d52765bf3dc Mon Sep 17 00:00:00 2001
From: Gautier Hattenberger <gautier.hattenberger@enac.fr>
Date: Thu, 23 Aug 2012 11:29:50 +0200
Subject: [PATCH] [flight plan] update flight plans to use state interface

---
 conf/flight_plans/EMAV2008.xml                | 10 +++---
 conf/flight_plans/EMAV2009.xml                |  2 +-
 conf/flight_plans/IS.xml                      | 14 ++++----
 conf/flight_plans/basic.xml                   |  6 ++--
 conf/flight_plans/ccc_gl.xml                  | 18 +++++------
 conf/flight_plans/corsica.xml                 |  6 ++--
 conf/flight_plans/creidlitz.xml               |  6 ++--
 conf/flight_plans/form_follow.xml             |  4 +--
 conf/flight_plans/form_leader.xml             |  4 +--
 conf/flight_plans/fp_tp_auto.xml              | 14 ++++----
 conf/flight_plans/generic.xml                 |  6 ++--
 conf/flight_plans/grosslobke_demo.xml         |  8 ++---
 conf/flight_plans/grosslobke_kreise.xml       |  2 +-
 conf/flight_plans/hsif.xml                    |  6 ++--
 conf/flight_plans/huit.xml                    |  2 +-
 conf/flight_plans/ingolfsskali.xml            |  8 ++---
 conf/flight_plans/joystick.xml                |  6 ++--
 conf/flight_plans/kalscott.xml                | 32 +++++++++----------
 conf/flight_plans/kv_svalbard.xml             |  6 ++--
 conf/flight_plans/landing.xml                 |  4 +--
 conf/flight_plans/mav06.xml                   |  8 ++---
 conf/flight_plans/mav07.xml                   | 12 +++----
 conf/flight_plans/mav08.xml                   |  6 ++--
 conf/flight_plans/muret_for.xml               |  2 +-
 conf/flight_plans/nordlys.xml                 | 10 +++---
 conf/flight_plans/poles.xml                   |  6 ++--
 conf/flight_plans/sinsat.xml                  |  2 +-
 conf/flight_plans/slayer_training.xml         |  6 ++--
 conf/flight_plans/snav.xml                    |  6 ++--
 conf/flight_plans/tcas.xml                    |  6 ++--
 conf/flight_plans/versatile.xml               | 20 ++++++------
 conf/flight_plans/versatile_airspeed.xml      | 18 +++++------
 .../versatile_carto_fixe_muret.xml            |  6 ++--
 conf/flight_plans/xsens_cachejunction.xml     |  6 ++--
 sw/airborne/subsystems/nav.h                  |  5 +++
 35 files changed, 144 insertions(+), 139 deletions(-)

diff --git a/conf/flight_plans/EMAV2008.xml b/conf/flight_plans/EMAV2008.xml
index 4e109393328..560eabb11de 100644
--- a/conf/flight_plans/EMAV2008.xml
+++ b/conf/flight_plans/EMAV2008.xml
@@ -48,7 +48,7 @@
       <attitude roll="0" throttle="0" vmode="throttle"/>
     </block>
     <block name="Takeoff" strip_button="Takeoff (wp CLIMB)" strip_icon="takeoff.png">
-      <exception cond="estimator_z > ground_alt+40" deroute="Standby"/>
+      <exception cond="GetPosAlt() > ground_alt+40" deroute="Standby"/>
       <set value="0" var="kill_throttle"/>
       <go wp="CLIMB"/>
     </block>
@@ -84,14 +84,14 @@
     </block>
     <block name="land">
       <call fun="nav_compute_baseleg(WP_AF, WP_TD, WP_BASELEG, nav_radius)"/>
-      <circle radius="nav_radius" until="NavQdrCloseTo(DegOfRad(baseleg_out_qdr)-(nav_radius/fabs(nav_radius))*10) && 15 > fabs(estimator_z - WaypointAlt(WP_BASELEG))" wp="BASELEG"/>
+      <circle radius="nav_radius" until="NavQdrCloseTo(DegOfRad(baseleg_out_qdr)-(nav_radius/fabs(nav_radius))*10) && 15 > fabs(GetPosAlt() - WaypointAlt(WP_BASELEG))" wp="BASELEG"/>
     </block>
     <block name="final">
-<!--      <exception cond="ground_alt + 6 > estimator_z" deroute="flare"/> -->
+<!--      <exception cond="ground_alt + 6 > GetPosAlt()" deroute="flare"/> -->
       <go approaching_time="0" from="AF" hmode="route" wp="SF"/>
     </block>
     <block name="shortfinal">
-<!--      <exception cond="ground_alt + 6 > estimator_z" deroute="flare"/> -->
+<!--      <exception cond="ground_alt + 6 > GetPosAlt()" deroute="flare"/> -->
       <go approaching_time="0" from="SF" hmode="route" wp="TD"/>
     </block>
     <block name="flare">
@@ -114,7 +114,7 @@
     </block>
     <block name="Circle 1">
       <exception cond="datalink_time > 25" deroute="Standby"/>
-      <circle alt="WaypointAlt(WP_1)" radius="nav_radius" until="15 > fabs(estimator_z - WaypointAlt(WP_1))" wp="STDBY"/>
+      <circle alt="WaypointAlt(WP_1)" radius="nav_radius" until="15 > fabs(GetPosAlt() - WaypointAlt(WP_1))" wp="STDBY"/>
       <go alt="WaypointAlt(WP_1)" approaching_time="-15" from="STDBY" hmode="route" wp="1"/>
       <circle radius="nav_radius" until="NavCircleCount() > 0.7" wp="1"/>
       <go alt="WaypointAlt(WP_1)" approaching_time="-15" from="1" hmode="route" wp="STDBY"/>
diff --git a/conf/flight_plans/EMAV2009.xml b/conf/flight_plans/EMAV2009.xml
index fbb603488bc..00960a73aae 100644
--- a/conf/flight_plans/EMAV2009.xml
+++ b/conf/flight_plans/EMAV2009.xml
@@ -26,7 +26,7 @@
       <attitude roll="0" throttle="0" vmode="throttle"/>
     </block>
     <block name="Takeoff" strip_button="Takeoff (wp CLIMB)" strip_icon="takeoff.png">
-      <exception cond="estimator_z > ground_alt+25" deroute="Standby"/>
+      <exception cond="GetPosAlt() > ground_alt+25" deroute="Standby"/>
       <set value="0" var="kill_throttle"/>
       <set value="0" var="estimator_flight_time"/>
       <go from="HOME" pitch="15" throttle="1.0" vmode="throttle" wp="CLIMB"/>
diff --git a/conf/flight_plans/IS.xml b/conf/flight_plans/IS.xml
index dddd2b9cb5f..c04ad1b09f0 100644
--- a/conf/flight_plans/IS.xml
+++ b/conf/flight_plans/IS.xml
@@ -79,8 +79,8 @@
     </block>
 
     <block name="ChemotaxisHere">
-      <set var="waypoints[WP_C].x" value="estimator_x"/>
-      <set var="waypoints[WP_C].y" value="estimator_y"/>
+      <set var="waypoints[WP_C].x" value="GetPosX()"/>
+      <set var="waypoints[WP_C].y" value="GetPosY()"/>
       <deroute block="Chemotaxis"/>
     </block>
 
@@ -107,15 +107,15 @@
     </block>
 
     <block name="climb 75">
-      <circle radius="50+(estimator_z-ground_alt)/2" wp="1" throttle="0.75" pitch="15" vmode="throttle" until="10 > PowerVoltage()"/>
+      <circle radius="50+(GetPosAlt()-ground_alt)/2" wp="1" throttle="0.75" pitch="15" vmode="throttle" until="10 > PowerVoltage()"/>
     </block>
 
     <block name="climb 1">
-      <circle radius="50+(estimator_z-ground_alt)/2" wp="1" climb="1" pitch="5" vmode="climb" until="10 > PowerVoltage()"/>
+      <circle radius="50+(GetPosAlt()-ground_alt)/2" wp="1" climb="1" pitch="5" vmode="climb" until="10 > PowerVoltage()"/>
     </block>
 
     <block name="descent 0">
-      <circle radius="50+(estimator_z-ground_alt)/2" wp="1" throttle="0.0" pitch="-15" vmode="throttle" until="ground_alt+50 > estimator_z"/>
+      <circle radius="50+(GetPosAlt()-ground_alt)/2" wp="1" throttle="0.0" pitch="-15" vmode="throttle" until="ground_alt+50 > GetPosAlt()"/>
       <deroute block="wait"/>
     </block>
 
@@ -148,12 +148,12 @@
       <circle radius="nav_radius" until="NavCircleCount() > 0.5" wp="BASELEG"/>
       <set value="V_CTL_AUTO_THROTTLE_MIN_CRUISE_THROTTLE" var="v_ctl_auto_throttle_cruise_throttle"/>
       <circle radius="nav_radius"
-              until="NavQdrCloseTo(DegOfRad(baseleg_out_qdr)-(nav_radius/fabs(nav_radius))*10) && 10 > fabs(estimator_z - WaypointAlt(WP_BASELEG))"
+              until="NavQdrCloseTo(DegOfRad(baseleg_out_qdr)-(nav_radius/fabs(nav_radius))*10) && 10 > fabs(GetPosAlt() - WaypointAlt(WP_BASELEG))"
               wp="BASELEG"/>
     </block>
 
     <block name="final">
-      <exception cond="ground_alt + 10 > estimator_z" deroute="flare"/>
+      <exception cond="ground_alt + 10 > GetPosAlt()" deroute="flare"/>
       <go from="AF" hmode="route" vmode="glide" wp="TD"/>
     </block>
 
diff --git a/conf/flight_plans/basic.xml b/conf/flight_plans/basic.xml
index f1c0fb690e0..43850512472 100644
--- a/conf/flight_plans/basic.xml
+++ b/conf/flight_plans/basic.xml
@@ -36,7 +36,7 @@
       <attitude roll="0" throttle="0" vmode="throttle"/>
     </block>
     <block key="t" name="Takeoff" strip_button="Takeoff (wp CLIMB)" strip_icon="takeoff.png" group="home">
-      <exception cond="stateGetPositionEnu_f()->z > ground_alt+25" deroute="Standby"/>
+      <exception cond="GetPosAlt() > ground_alt+25" deroute="Standby"/>
       <set value="0" var="kill_throttle"/>
       <set value="0" var="estimator_flight_time"/>
       <go from="HOME" throttle="1.0" vmode="throttle" wp="CLIMB" pitch="15"/>
@@ -78,10 +78,10 @@
     <block name="land">
       <call fun="nav_compute_baseleg(WP_AF, WP_TD, WP__BASELEG, nav_radius)"/>
       <circle radius="nav_radius" until="NavCircleCount() > 0.5" wp="_BASELEG"/>
-      <circle radius="nav_radius" until="And(NavQdrCloseTo(DegOfRad(baseleg_out_qdr)-(nav_radius/fabs(nav_radius))*10), 10 > fabs(stateGetPositionEnu_f()->z - WaypointAlt(WP__BASELEG)))" wp="_BASELEG"/>
+      <circle radius="nav_radius" until="And(NavQdrCloseTo(DegOfRad(baseleg_out_qdr)-(nav_radius/fabs(nav_radius))*10), 10 > fabs(GetPosAlt() - WaypointAlt(WP__BASELEG)))" wp="_BASELEG"/>
     </block>
     <block name="final">
-      <exception cond="ground_alt + 10 > stateGetPositionEnu_f()->z" deroute="flare"/>
+      <exception cond="ground_alt + 10 > GetPosAlt()" deroute="flare"/>
       <go from="AF" hmode="route" vmode="glide" wp="TD"/>
     </block>
     <block name="flare">
diff --git a/conf/flight_plans/ccc_gl.xml b/conf/flight_plans/ccc_gl.xml
index d913ac6c92e..ea161cde0ae 100644
--- a/conf/flight_plans/ccc_gl.xml
+++ b/conf/flight_plans/ccc_gl.xml
@@ -45,7 +45,7 @@
       <attitude roll="0" throttle="0" vmode="throttle"/>
     </block>
     <block name="Takeoff" strip_icon="takeoff.png" strip_button="Takeoff (wp CLIMB)">
-      <exception cond="estimator_z > ground_alt+25" deroute="Standby"/>
+      <exception cond="GetPosAlt() > ground_alt+25" deroute="Standby"/>
       <set value="0" var="kill_throttle"/>
       <go wp="CLIMB"/>
     </block>
@@ -66,19 +66,19 @@
       <circle pitch="auto" radius="75" throttle="0.7" wp="1"/>
     </block>
     <block name="Climb 75% throttle">
-      <circle pitch="10" radius="50+(estimator_z-ground_alt)/2" throttle="0.75" until="(10 > PowerVoltage()) || (estimator_z > ground_alt+ 1350)" vmode="throttle" wp="1"/>
+      <circle pitch="10" radius="50+(GetPosAlt()-ground_alt)/2" throttle="0.75" until="(10 > PowerVoltage()) || (GetPosAlt() > ground_alt+ 1350)" vmode="throttle" wp="1"/>
     </block>
     <block name="Climb 0m/s">
       <circle climb="0" radius="nav_radius" until="10 > PowerVoltage()" vmode="climb" wp="1"/>
     </block>
     <block name="Climb 1m/s">
-      <circle climb="1" pitch="5" radius="50+(estimator_z-ground_alt)/2" until="10 > PowerVoltage()" vmode="climb" wp="1"/>
+      <circle climb="1" pitch="5" radius="50+(GetPosAlt()-ground_alt)/2" until="10 > PowerVoltage()" vmode="climb" wp="1"/>
     </block>
     <block name="Climb nav_climb m/s">
-      <circle climb="nav_climb" radius="nav_radius" until="(10 > PowerVoltage()) || (estimator_z > ground_alt+ 1350)" vmode="climb" wp="1"/>
+      <circle climb="nav_climb" radius="nav_radius" until="(10 > PowerVoltage()) || (GetPosAlt() > ground_alt+ 1350)" vmode="climb" wp="1"/>
     </block>
     <block name="Descent 0% throttle">
-      <circle pitch="-5" radius="50+(estimator_z-ground_alt)/2" throttle="0.0" until="ground_alt+50 > estimator_z" vmode="throttle" wp="1"/>
+      <circle pitch="-5" radius="50+(GetPosAlt()-ground_alt)/2" throttle="0.0" until="ground_alt+50 > GetPosAlt()" vmode="throttle" wp="1"/>
       <deroute block="Standby"/>
     </block>
     <block name="Route 1-2">
@@ -112,10 +112,10 @@
       <call fun="nav_compute_baseleg(WP_AF, WP_TD, WP_BASELEG, nav_radius)"/>
       <circle radius="nav_radius" until="NavCircleCount() > 0.5" wp="BASELEG"/>
       <set value="V_CTL_AUTO_THROTTLE_MIN_CRUISE_THROTTLE" var="v_ctl_auto_throttle_cruise_throttle"/>
-      <circle radius="nav_radius" until="NavQdrCloseTo(DegOfRad(baseleg_out_qdr)-(nav_radius/fabs(nav_radius))*10) && 10 > fabs(estimator_z - WaypointAlt(WP_BASELEG))" wp="BASELEG"/>
+      <circle radius="nav_radius" until="NavQdrCloseTo(DegOfRad(baseleg_out_qdr)-(nav_radius/fabs(nav_radius))*10) && 10 > fabs(GetPosAlt() - WaypointAlt(WP_BASELEG))" wp="BASELEG"/>
     </block>
     <block name="final">
-      <exception cond="ground_alt + 10 > estimator_z" deroute="flare"/>
+      <exception cond="ground_alt + 10 > GetPosAlt()" deroute="flare"/>
       <go from="AF" hmode="route" vmode="glide" wp="TD"/>
     </block>
     <block name="flare">
@@ -155,11 +155,11 @@
       <go from="2" hmode="route" wp="STDBY"/>
     </block>
     <block name="Fly in Square">
-      <exception cond="! InsideSquare(estimator_x, estimator_y)" deroute="Come back wp 1"/>
+      <exception cond="! InsideSquare(GetPosX(), GetPosY())" deroute="Come back wp 1"/>
       <attitude alt="ground_alt+75" roll="0" vmode="alt"/>
     </block>
     <block name="Come back wp 1">
-      <exception cond="InsideSquare(estimator_x, estimator_y)" deroute="Fly in Square"/>
+      <exception cond="InsideSquare(GetPosX(), GetPosY())" deroute="Fly in Square"/>
       <go wp="1"/>
       <deroute block="Fly in Square"/>
     </block>
diff --git a/conf/flight_plans/corsica.xml b/conf/flight_plans/corsica.xml
index b429892412f..36064950a26 100644
--- a/conf/flight_plans/corsica.xml
+++ b/conf/flight_plans/corsica.xml
@@ -33,7 +33,7 @@
       <attitude roll="0" throttle="0" vmode="throttle"/>
     </block>
     <block name="Takeoff" strip_button="Takeoff (wp CLIMB)" strip_icon="takeoff.png">
-      <exception cond="estimator_z > ground_alt+25" deroute="Standby Menton"/>
+      <exception cond="GetPosAlt() > ground_alt+25" deroute="Standby Menton"/>
       <set value="0" var="kill_throttle"/>
       <set value="0" var="estimator_flight_time"/>
       <go throttle="1.0" vmode="throttle" wp="CLIMB" pitch="15"/>
@@ -63,10 +63,10 @@
     <block name="land">
       <call fun="nav_compute_baseleg(WP_AF, WP_TD, WP__BASELEG, nav_radius)"/>
       <circle radius="nav_radius" until="NavCircleCount() > 0.5" wp="_BASELEG"/>
-      <circle radius="nav_radius" until="And(NavQdrCloseTo(DegOfRad(baseleg_out_qdr)-10), 10 > fabs(estimator_z - WaypointAlt(WP__BASELEG)))" wp="_BASELEG"/>
+      <circle radius="nav_radius" until="And(NavQdrCloseTo(DegOfRad(baseleg_out_qdr)-10), 10 > fabs(GetPosAlt() - WaypointAlt(WP__BASELEG)))" wp="_BASELEG"/>
     </block>
     <block name="final">
-      <exception cond="ground_alt + 10 > estimator_z" deroute="flare"/>
+      <exception cond="ground_alt + 10 > GetPosAlt()" deroute="flare"/>
       <go from="AF" hmode="route" vmode="glide" wp="TD"/>
     </block>
     <block name="flare">
diff --git a/conf/flight_plans/creidlitz.xml b/conf/flight_plans/creidlitz.xml
index c53a001f24b..67f4f2487df 100644
--- a/conf/flight_plans/creidlitz.xml
+++ b/conf/flight_plans/creidlitz.xml
@@ -17,7 +17,7 @@
     <waypoint name="CLIMB" x="-102.3" y="-129.0"/>
   </waypoints>
   <exceptions>
-    <exception cond="estimator_z > 3400" deroute="Standby"/>
+    <exception cond="GetPosAlt() > 3400" deroute="Standby"/>
     <exception cond="datalink_time > 30" deroute="Standby"/>
   </exceptions>
   <blocks>
@@ -34,7 +34,7 @@
       <attitude roll="0" throttle="0" vmode="throttle"/>
     </block>
     <block name="Takeoff" strip_button="Takeoff (wp CLIMB)" strip_icon="takeoff.png">
-      <exception cond="estimator_z > ground_alt+25" deroute="Standby"/>
+      <exception cond="GetPosAlt() > ground_alt+25" deroute="Standby"/>
       <set value="0" var="kill_throttle"/>
       <set value="0" var="estimator_flight_time"/>
       <go from="HOME" throttle="1.0" vmode="throttle" wp="CLIMB"/>
@@ -46,7 +46,7 @@
       <circle radius="nav_radius" wp="PROFILE"/>
     </block>
     <block name="profile_down" strip_button="Profile Down" strip_icon="down_profile.png">
-      <circle pitch="RadOfDeg(-10)" radius="nav_radius" throttle="0" until="(ground_alt+150 > estimator_z)" vmode="throttle" wp="PROFILE"/>
+      <circle pitch="RadOfDeg(-10)" radius="nav_radius" throttle="0" until="(ground_alt+150 > GetPosAlt())" vmode="throttle" wp="PROFILE"/>
       <deroute block="Standby"/>
     </block>
     <block name="MOB" strip_button="Turn around here" strip_icon="mob.png">
diff --git a/conf/flight_plans/form_follow.xml b/conf/flight_plans/form_follow.xml
index 010cbc178d3..41355289ce6 100644
--- a/conf/flight_plans/form_follow.xml
+++ b/conf/flight_plans/form_follow.xml
@@ -35,8 +35,8 @@
     </block>
     <block name="land"> <!-- basic landing function out of formation -->
       <call fun="stop_formation()"/>
-      <exception cond="(GROUND_ALT+10> estimator_z)" deroute="stop"/>
-      <circle radius="50" until="225 > estimator_z" wp="IAF"/>
+      <exception cond="(GROUND_ALT+10> GetPosAlt())" deroute="stop"/>
+      <circle radius="50" until="225 > GetPosAlt()" wp="IAF"/>
       <go alt="215" wp="AF"/>
       <go from="AF" hmode="route" vmode="glide" wp="RWAY"/>
     </block>
diff --git a/conf/flight_plans/form_leader.xml b/conf/flight_plans/form_leader.xml
index c22cbf6813e..a98d81c56e9 100644
--- a/conf/flight_plans/form_leader.xml
+++ b/conf/flight_plans/form_leader.xml
@@ -45,8 +45,8 @@
       <call fun="stop_formation()"/>
     </block>
     <block name="land"> <!-- basic landing function out of formation -->
-      <exception cond="(GROUND_ALT+10> estimator_z)" deroute="stop"/>
-      <circle radius="50" until="WaypointAlt(WP_IAF) > estimator_z" wp="IAF"/>
+      <exception cond="(GROUND_ALT+10> GetPosAlt())" deroute="stop"/>
+      <circle radius="50" until="WaypointAlt(WP_IAF) > GetPosAlt()" wp="IAF"/>
       <go alt="WaypointAlt(WP_AF)" wp="AF"/>
       <go from="AF" hmode="route" vmode="glide" wp="RWAY"/>
     </block>
diff --git a/conf/flight_plans/fp_tp_auto.xml b/conf/flight_plans/fp_tp_auto.xml
index e676f007c65..c72620f3d98 100644
--- a/conf/flight_plans/fp_tp_auto.xml
+++ b/conf/flight_plans/fp_tp_auto.xml
@@ -44,7 +44,7 @@
       <attitude roll="0" throttle="0" vmode="throttle"/>
     </block>
     <block name="Takeoff" strip_icon="takeoff.png" strip_button="Takeoff (wp CLIMB)">
-      <exception cond="estimator_z > ground_alt+25" deroute="Standby"/>
+      <exception cond="GetPosAlt() > ground_alt+25" deroute="Standby"/>
       <set value="0" var="kill_throttle"/>
       <go wp="CLIMB"/>
     </block>
@@ -65,19 +65,19 @@
       <circle pitch="auto" radius="75" throttle="0.7" wp="1"/>
     </block>
     <block name="Climb 75% throttle">
-      <circle pitch="10" radius="50+(estimator_z-ground_alt)/2" throttle="0.75" until="(10 > PowerVoltage()) || (estimator_z > ground_alt+ 1350)" vmode="throttle" wp="1"/>
+      <circle pitch="10" radius="50+(GetPosAlt()-ground_alt)/2" throttle="0.75" until="(10 > PowerVoltage()) || (GetPosAlt() > ground_alt+ 1350)" vmode="throttle" wp="1"/>
     </block>
     <block name="Climb 0m/s">
       <circle climb="0" radius="nav_radius" until="10 > PowerVoltage()" vmode="climb" wp="1"/>
     </block>
     <block name="Climb 1m/s">
-      <circle climb="1" radius="50+(estimator_z-ground_alt)/2" until="10 > PowerVoltage()" vmode="climb" wp="1"/>
+      <circle climb="1" radius="50+(GetPosAlt()-ground_alt)/2" until="10 > PowerVoltage()" vmode="climb" wp="1"/>
     </block>
     <block name="Climb nav_climb m/s">
-      <circle climb="nav_climb" radius="nav_radius" until="(10 > PowerVoltage()) || (estimator_z > ground_alt+ 1350)" vmode="climb" wp="1"/>
+      <circle climb="nav_climb" radius="nav_radius" until="(10 > PowerVoltage()) || (GetPosAlt() > ground_alt+ 1350)" vmode="climb" wp="1"/>
     </block>
     <block name="Descent 0% throttle">
-      <circle pitch="-5" radius="50+(estimator_z-ground_alt)/2" throttle="0.0" until="ground_alt+50 > estimator_z" vmode="throttle" wp="1"/>
+      <circle pitch="-5" radius="50+(GetPosAlt()-ground_alt)/2" throttle="0.0" until="ground_alt+50 > GetPosAlt()" vmode="throttle" wp="1"/>
       <deroute block="Standby"/>
     </block>
     <block name="Route 1-2">
@@ -111,10 +111,10 @@
       <call fun="nav_compute_baseleg(WP_AF, WP_TD, WP_BASELEG, nav_radius)"/>
       <circle radius="nav_radius" until="NavCircleCount() > 0.5" wp="BASELEG"/>
       <set value="V_CTL_AUTO_THROTTLE_MIN_CRUISE_THROTTLE" var="v_ctl_auto_throttle_cruise_throttle"/>
-      <circle radius="nav_radius" until="NavQdrCloseTo(DegOfRad(baseleg_out_qdr)-(nav_radius/fabs(nav_radius))*10) && 10 > fabs(estimator_z - WaypointAlt(WP_BASELEG))" wp="BASELEG"/>
+      <circle radius="nav_radius" until="NavQdrCloseTo(DegOfRad(baseleg_out_qdr)-(nav_radius/fabs(nav_radius))*10) && 10 > fabs(GetPosAlt() - WaypointAlt(WP_BASELEG))" wp="BASELEG"/>
     </block>
     <block name="final">
-      <exception cond="ground_alt + 10 > estimator_z" deroute="flare"/>
+      <exception cond="ground_alt + 10 > GetPosAlt()" deroute="flare"/>
       <go from="AF" hmode="route" vmode="glide" wp="TD"/>
     </block>
     <block name="flare">
diff --git a/conf/flight_plans/generic.xml b/conf/flight_plans/generic.xml
index dafbcfced34..b1fc8ab8eac 100644
--- a/conf/flight_plans/generic.xml
+++ b/conf/flight_plans/generic.xml
@@ -24,13 +24,13 @@
       <circle radius="-75" wp="HOME"/>
     </block>
     <block name="climb 75">
-      <circle radius="50+(estimator_z-ground_alt)/2" wp="1" throttle="0.75" pitch="15" vmode="throttle" until="10>PowerVoltage()"/>
+      <circle radius="50+(GetPosAlt()-ground_alt)/2" wp="1" throttle="0.75" pitch="15" vmode="throttle" until="10>PowerVoltage()"/>
     </block>
     <block name="stack 1">
-      <circle radius="estimator_z/2" wp="1"/>
+      <circle radius="GetPosAlt()/2" wp="1"/>
     </block>
     <block name="descent 0" strip_button="Descent">
-      <circle radius="estimator_z/2" wp="1" throttle="0.0" pitch="-15" vmode="throttle"/>
+      <circle radius="GetPosAlt()/2" wp="1" throttle="0.0" pitch="-15" vmode="throttle"/>
     </block>
     <block name="route12">
       <go approaching_time="0" from="1" hmode="route" wp="2"/>
diff --git a/conf/flight_plans/grosslobke_demo.xml b/conf/flight_plans/grosslobke_demo.xml
index 31be09e05c2..6961d146faa 100644
--- a/conf/flight_plans/grosslobke_demo.xml
+++ b/conf/flight_plans/grosslobke_demo.xml
@@ -55,7 +55,7 @@
       <set value="TRUE" var="h_ctl_disabled"/>
       <set value="(-0.6*MAX_PPRZ)" var="h_ctl_aileron_setpoint"/>
       <set value="(0.9*MAX_PPRZ)" var="h_ctl_elevator_setpoint"/>
-      <while cond="estimator_z > ground_alt + 220"/>
+      <while cond="GetPosAlt() > ground_alt + 220"/>
       <set value="0" var="h_ctl_aileron_setpoint"/>
       <set value="0" var="h_ctl_elevator_setpoint"/>
     </block>
@@ -77,10 +77,10 @@
     <block name="land">
       <call fun="nav_compute_baseleg(WP_AF, WP_TD, WP__BASELEG, nav_radius)"/>
       <circle radius="nav_radius" until="NavCircleCount() > 0.5" wp="_BASELEG"/>
-      <circle radius="nav_radius" until="NavQdrCloseTo(DegOfRad(baseleg_out_qdr)-(nav_radius/fabs(nav_radius))*10) && 20 > fabs(estimator_z - WaypointAlt(WP__BASELEG))" wp="_BASELEG"/>
+      <circle radius="nav_radius" until="NavQdrCloseTo(DegOfRad(baseleg_out_qdr)-(nav_radius/fabs(nav_radius))*10) && 20 > fabs(GetPosAlt() - WaypointAlt(WP__BASELEG))" wp="_BASELEG"/>
     </block>
     <block name="final">
-      <exception cond="ground_alt + 10 > estimator_z" deroute="flare"/>
+      <exception cond="ground_alt + 10 > GetPosAlt()" deroute="flare"/>
       <go from="AF" hmode="route" vmode="glide" wp="TD"/>
     </block>
     <block name="flare">
@@ -114,7 +114,7 @@
     <!--block NAME="roll" strip_button="Roll">
      <set value="TRUE" var="h_ctl_disabled"/>
      <set value="(-0.75*MAX_PPRZ)" var="h_ctl_aileron_setpoint"/>
-     <while cond="3.> stage_time && (estimator_phi > 0 || -M_PI_2 > estimator_phi)"/>
+     <while cond="3.> stage_time && (stateGetNedToBodyEulers_f()->phi > 0 || -M_PI_2 > stateGetNedToBodyEulers_f()->phi)"/>
      <set value="FALSE" var="h_ctl_disabled"/>
      <return/>
     </block-->
diff --git a/conf/flight_plans/grosslobke_kreise.xml b/conf/flight_plans/grosslobke_kreise.xml
index 8a6d83dfb1b..170f85b5240 100644
--- a/conf/flight_plans/grosslobke_kreise.xml
+++ b/conf/flight_plans/grosslobke_kreise.xml
@@ -43,7 +43,7 @@
     <block NAME="roll" strip_button="Roll">
      <set value="TRUE" var="h_ctl_disabled"/>
      <set value="(-0.75*MAX_PPRZ)" var="h_ctl_aileron_setpoint"/>
-     <while cond="3.> stage_time && (estimator_phi > 0 || -M_PI_2 > estimator_phi)"/>
+     <while cond="3.> stage_time && (stateGetNedToBodyEulers_f()->phi > 0 || -M_PI_2 > stateGetNedToBodyEulers_f()->phi)"/>
      <set value="FALSE" var="h_ctl_disabled"/>
      <return/>
     </block>
diff --git a/conf/flight_plans/hsif.xml b/conf/flight_plans/hsif.xml
index 26825eb6e77..71ba46343a5 100644
--- a/conf/flight_plans/hsif.xml
+++ b/conf/flight_plans/hsif.xml
@@ -19,15 +19,15 @@
       <call fun="NavSetGroundReferenceHere()"/>
     </block>
     <block name="Takeoff">
-      <exception cond="estimator_z > ground_alt+25" deroute="HSIF"/>
+      <exception cond="GetPosAlt() > ground_alt+25" deroute="HSIF"/>
       <set value="0" var="kill_throttle"/>
       <set value="0" var="estimator_flight_time"/>
       <go from="HOME" pitch="15" throttle="1.0" vmode="throttle" wp="CLIMB"/>
     </block>
     <block name="HSIF">
-      <circle radius="200" until="estimator_x > 200" wp="2"/>
+      <circle radius="200" until="GetPosX() > 200" wp="2"/>
       <circle radius="-75" until="NavQdrCloseTo(330)" wp="3"/>
-      <circle radius="200" until="0 > estimator_x" wp="1"/>
+      <circle radius="200" until="0 > GetPosX()" wp="1"/>
       <circle radius="35" wp="STDBY"/>
     </block>
   </blocks>
diff --git a/conf/flight_plans/huit.xml b/conf/flight_plans/huit.xml
index 520e714a924..623c0b0af99 100644
--- a/conf/flight_plans/huit.xml
+++ b/conf/flight_plans/huit.xml
@@ -14,7 +14,7 @@
 <blocks>
   <block name="init">
     <while cond="(estimator_flight_time)"></while>
-    <heading course="QFU" vmode="climb" climb="2.0" until="(estimator_z > SECURITY_ALT)"/>
+    <heading course="QFU" vmode="climb" climb="2.0" until="(GetPosAlt() > SECURITY_ALT)"/>
   </block>
 
   <block name="balaye">
diff --git a/conf/flight_plans/ingolfsskali.xml b/conf/flight_plans/ingolfsskali.xml
index 2666e2b3073..bbc10eb850f 100644
--- a/conf/flight_plans/ingolfsskali.xml
+++ b/conf/flight_plans/ingolfsskali.xml
@@ -15,7 +15,7 @@
     <waypoint name="S2" x="300" y="400"/>
   </waypoints>
   <exceptions>
-    <exception cond="estimator_z > 3600"  deroute="wait"/>
+    <exception cond="GetPosAlt() > 3600"  deroute="wait"/>
     <exception cond="datalink_time > 30"  deroute="wait"/>
     <exception cond="10 > PowerVoltage()" deroute="wait"/>
   </exceptions>
@@ -66,11 +66,11 @@
       <circle radius="nav_radius" wp="CLIMB"/>
     </block>
     <block name="descent">
-      <circle radius="nav_radius" wp="CLIMB" vmode="throttle" throttle="0" pitch="RadOfDeg(my_nav_pitch)"  until="(ground_alt+150 > estimator_z)"/>
+      <circle radius="nav_radius" wp="CLIMB" vmode="throttle" throttle="0" pitch="RadOfDeg(my_nav_pitch)"  until="(ground_alt+150 > GetPosAlt())"/>
     <deroute block="wait"/>
     </block>
     <block name="wind">
-      <attitude pitch="RadOfDeg(my_nav_pitch)" roll="my_nav_roll" throttle="0.0" until="(ground_alt+150 > estimator_z)" vmode="throttle"/>
+      <attitude pitch="RadOfDeg(my_nav_pitch)" roll="my_nav_roll" throttle="0.0" until="(ground_alt+150 > GetPosAlt())" vmode="throttle"/>
       <deroute block="wait"/>
     </block>
     <block name="wait" strip_button="Wait">
@@ -79,7 +79,7 @@
     <block name="roll">
       <set value="TRUE" var="h_ctl_disabled"/>
       <set value="(-0.75*MAX_PPRZ)" var="h_ctl_aileron_setpoint"/>
-      <while cond="3.> stage_time && (estimator_phi > 0 || -M_PI_2 > estimator_phi)"/>
+      <while cond="3.> stage_time && (stateGetNedToBodyEulers_f()->phi > 0 || -M_PI_2 > stateGetNedToBodyEulers_f()->phi)"/>
       <set value="FALSE" var="h_ctl_disabled"/>
       <return/>
     </block>
diff --git a/conf/flight_plans/joystick.xml b/conf/flight_plans/joystick.xml
index c1efecaf3fa..971f2dd5442 100644
--- a/conf/flight_plans/joystick.xml
+++ b/conf/flight_plans/joystick.xml
@@ -34,7 +34,7 @@
       <attitude roll="0" throttle="0" vmode="throttle"/>
     </block>
     <block name="Takeoff" strip_button="Takeoff (wp CLIMB)" strip_icon="takeoff.png">
-      <exception cond="estimator_z > ground_alt+25" deroute="Standby"/>
+      <exception cond="GetPosAlt() > ground_alt+25" deroute="Standby"/>
       <set value="0" var="kill_throttle"/>
       <set value="0" var="estimator_flight_time"/>
       <go from="HOME" throttle="1.0" vmode="throttle" wp="CLIMB" pitch="15"/>
@@ -80,10 +80,10 @@
     <block name="land">
       <call fun="nav_compute_baseleg(WP_AF, WP_TD, WP__BASELEG, nav_radius)"/>
       <circle radius="nav_radius" until="NavCircleCount() > 0.5" wp="_BASELEG"/>
-      <circle radius="nav_radius" until="NavQdrCloseTo(DegOfRad(baseleg_out_qdr)-(nav_radius/fabs(nav_radius))*10) && 10 > fabs(estimator_z - WaypointAlt(WP__BASELEG))" wp="_BASELEG"/>
+      <circle radius="nav_radius" until="NavQdrCloseTo(DegOfRad(baseleg_out_qdr)-(nav_radius/fabs(nav_radius))*10) && 10 > fabs(GetPosAlt() - WaypointAlt(WP__BASELEG))" wp="_BASELEG"/>
     </block>
     <block name="final">
-      <exception cond="ground_alt + 10 > estimator_z" deroute="flare"/>
+      <exception cond="ground_alt + 10 > GetPosAlt()" deroute="flare"/>
       <go from="AF" hmode="route" vmode="glide" wp="TD"/>
     </block>
     <block name="flare">
diff --git a/conf/flight_plans/kalscott.xml b/conf/flight_plans/kalscott.xml
index b7401c953af..4fce807cded 100644
--- a/conf/flight_plans/kalscott.xml
+++ b/conf/flight_plans/kalscott.xml
@@ -26,47 +26,47 @@
       <attitude roll="0" throttle="0" vmode="throttle"/>
     </block>
     <block name="Takeoff" strip_button="Takeoff (wp CLIMB)" strip_icon="takeoff.png">
-      <exception cond="estimator_z > ground_alt+25" deroute="Circle"/>
+      <exception cond="GetPosAlt() > ground_alt+25" deroute="Circle"/>
       <set value="0" var="kill_throttle"/>
       <set value="0" var="estimator_flight_time"/>
       <go from="HOME" throttle="1.0" vmode="throttle" wp="CLIMB" pitch="15"/>
     </block>
     <block name="Circle" strip_button="Circle" strip_icon="home.png">
-      <circle radius="nav_radius" wp="CIRCLE" alt="ground_alt+200" until="estimator_z > ground_alt+195"/>
+      <circle radius="nav_radius" wp="CIRCLE" alt="ground_alt+200" until="GetPosAlt() > ground_alt+195"/>
       <circle radius="nav_radius" wp="CIRCLE" alt="ground_alt+200" until="NavCircleCount() > 2"/>
-      <circle radius="nav_radius" wp="CIRCLE" alt="ground_alt+300" until="estimator_z > ground_alt+295"/>
+      <circle radius="nav_radius" wp="CIRCLE" alt="ground_alt+300" until="GetPosAlt() > ground_alt+295"/>
       <circle radius="nav_radius" wp="CIRCLE" alt="ground_alt+300" until="NavCircleCount() > 2"/>
-      <circle radius="nav_radius" wp="CIRCLE" alt="ground_alt+400" until="estimator_z > ground_alt+395"/>
+      <circle radius="nav_radius" wp="CIRCLE" alt="ground_alt+400" until="GetPosAlt() > ground_alt+395"/>
       <circle radius="nav_radius" wp="CIRCLE" alt="ground_alt+400" until="NavCircleCount() > 2"/>
-      <circle radius="nav_radius" wp="CIRCLE" alt="ground_alt+300" until="ground_alt+305 > estimator_z"/>
+      <circle radius="nav_radius" wp="CIRCLE" alt="ground_alt+300" until="ground_alt+305 > GetPosAlt()"/>
       <circle radius="nav_radius" wp="CIRCLE" alt="ground_alt+300" until="NavCircleCount() > 2"/>
-      <circle radius="nav_radius" wp="CIRCLE" alt="ground_alt+200" until="ground_alt+205 > estimator_z"/>
+      <circle radius="nav_radius" wp="CIRCLE" alt="ground_alt+200" until="ground_alt+205 > GetPosAlt()"/>
       <circle radius="nav_radius" wp="CIRCLE" alt="ground_alt+200" until="NavCircleCount() > 2"/>
       <deroute block="Circle"/>
     </block>
    <block name="Racetrack E-W" strip_button="Racetrack E-W" strip_icon="oval.png">
-      <oval p1="1" p2="2" radius="nav_radius" alt="ground_alt+200" until="estimator_z > ground_alt+195"/>
+      <oval p1="1" p2="2" radius="nav_radius" alt="ground_alt+200" until="GetPosAlt() > ground_alt+195"/>
       <oval p1="1" p2="2" radius="nav_radius" alt="ground_alt+200" until="nav_oval_count > 2"/>
-      <oval p1="1" p2="2" radius="nav_radius" alt="ground_alt+300" until="estimator_z > ground_alt+295"/>
+      <oval p1="1" p2="2" radius="nav_radius" alt="ground_alt+300" until="GetPosAlt() > ground_alt+295"/>
       <oval p1="1" p2="2" radius="nav_radius" alt="ground_alt+300" until="nav_oval_count > 2"/>
-      <oval p1="1" p2="2" radius="nav_radius" alt="ground_alt+400" until="estimator_z > ground_alt+395"/>
+      <oval p1="1" p2="2" radius="nav_radius" alt="ground_alt+400" until="GetPosAlt() > ground_alt+395"/>
       <oval p1="1" p2="2" radius="nav_radius" alt="ground_alt+400" until="nav_oval_count > 2"/>
-      <oval p1="1" p2="2" radius="nav_radius" alt="ground_alt+300" until="ground_alt+305 > estimator_z"/>
+      <oval p1="1" p2="2" radius="nav_radius" alt="ground_alt+300" until="ground_alt+305 > GetPosAlt()"/>
       <oval p1="1" p2="2" radius="nav_radius" alt="ground_alt+300" until="nav_oval_count > 2"/>
-      <oval p1="1" p2="2" radius="nav_radius" alt="ground_alt+200" until="ground_alt+205 > estimator_z"/>
+      <oval p1="1" p2="2" radius="nav_radius" alt="ground_alt+200" until="ground_alt+205 > GetPosAlt()"/>
       <oval p1="1" p2="2" radius="nav_radius" alt="ground_alt+200" until="nav_oval_count > 2"/>
       <deroute block="Racetrack E-W"/>
     </block>
     <block name="Racetrack N-S" strip_button="Racetrack N-S" strip_icon="oval.png">
-      <oval p1="2" p2="3" radius="nav_radius" alt="ground_alt+200" until="estimator_z > ground_alt+195"/>
+      <oval p1="2" p2="3" radius="nav_radius" alt="ground_alt+200" until="GetPosAlt() > ground_alt+195"/>
       <oval p1="2" p2="3" radius="nav_radius" alt="ground_alt+200" until="nav_oval_count > 2"/>
-      <oval p1="2" p2="3" radius="nav_radius" alt="ground_alt+300" until="estimator_z > ground_alt+295"/>
+      <oval p1="2" p2="3" radius="nav_radius" alt="ground_alt+300" until="GetPosAlt() > ground_alt+295"/>
       <oval p1="2" p2="3" radius="nav_radius" alt="ground_alt+300" until="nav_oval_count > 2"/>
-      <oval p1="2" p2="3" radius="nav_radius" alt="ground_alt+400" until="estimator_z > ground_alt+395"/>
+      <oval p1="2" p2="3" radius="nav_radius" alt="ground_alt+400" until="GetPosAlt() > ground_alt+395"/>
       <oval p1="2" p2="3" radius="nav_radius" alt="ground_alt+400" until="nav_oval_count > 2"/>
-      <oval p1="2" p2="3" radius="nav_radius" alt="ground_alt+300" until="ground_alt+305 > estimator_z"/>
+      <oval p1="2" p2="3" radius="nav_radius" alt="ground_alt+300" until="ground_alt+305 > GetPosAlt()"/>
       <oval p1="2" p2="3" radius="nav_radius" alt="ground_alt+300" until="nav_oval_count > 2"/>
-      <oval p1="2" p2="3" radius="nav_radius" alt="ground_alt+200" until="ground_alt+305 > estimator_z"/>
+      <oval p1="2" p2="3" radius="nav_radius" alt="ground_alt+200" until="ground_alt+305 > GetPosAlt()"/>
       <oval p1="2" p2="3" radius="nav_radius" alt="ground_alt+200" until="nav_oval_count > 2"/>
       <deroute block="Racetrack N-S"/>
     </block>
diff --git a/conf/flight_plans/kv_svalbard.xml b/conf/flight_plans/kv_svalbard.xml
index 153104cc99e..e8b7ca83252 100644
--- a/conf/flight_plans/kv_svalbard.xml
+++ b/conf/flight_plans/kv_svalbard.xml
@@ -18,7 +18,7 @@
     <waypoint name="CLIMB" x="-114.5" y="162.3"/>
   </waypoints>
   <exceptions>
-    <exception cond="estimator_z > 3600"  deroute="Standby"/>
+    <exception cond="GetPosAlt() > 3600"  deroute="Standby"/>
     <exception cond="datalink_time > 30"  deroute="Standby"/>
     <exception cond="10 > PowerVoltage()" deroute="Standby"/>
   </exceptions>
@@ -36,7 +36,7 @@
       <attitude roll="0" throttle="0" vmode="throttle"/>
     </block>
     <block name="Takeoff" strip_button="Takeoff (wp CLIMB)" strip_icon="takeoff.png">
-      <exception cond="estimator_z > ground_alt+25" deroute="Standby"/>
+      <exception cond="GetPosAlt() > ground_alt+25" deroute="Standby"/>
       <set value="0" var="kill_throttle"/>
       <set value="0" var="estimator_flight_time"/>
       <go from="HOME" throttle="1.0" vmode="throttle" wp="CLIMB"/>
@@ -48,7 +48,7 @@
       <circle radius="nav_radius" wp="PROFILE"/>
     </block>
     <block name="profile_down" strip_button="Profile Down" strip_icon="down_profile.png">
-      <circle radius="nav_radius" wp="PROFILE" vmode="throttle" throttle="0" pitch="RadOfDeg(-10)"  until="(ground_alt+150 > estimator_z)"/>
+      <circle radius="nav_radius" wp="PROFILE" vmode="throttle" throttle="0" pitch="RadOfDeg(-10)"  until="(ground_alt+150 > GetPosAlt())"/>
     <deroute block="Standby"/>
     </block>
     <block name="MOB" strip_button="Turn around here" strip_icon="mob.png">
diff --git a/conf/flight_plans/landing.xml b/conf/flight_plans/landing.xml
index 953b26464b0..f1f2aed593e 100644
--- a/conf/flight_plans/landing.xml
+++ b/conf/flight_plans/landing.xml
@@ -18,10 +18,10 @@
     <block name="land">
       <call fun="nav_compute_baseleg(WP_AF, WP_TD, WP__BASELEG, nav_radius)"/>
       <circle radius="nav_radius" until="NavCircleCount() > 0.5" wp="_BASELEG"/>
-      <circle radius="nav_radius" until="And(NavQdrCloseTo(DegOfRad(baseleg_out_qdr)-(nav_radius/fabs(nav_radius))*10), 10 > fabs(estimator_z - WaypointAlt(WP__BASELEG)))" wp="_BASELEG"/>
+      <circle radius="nav_radius" until="And(NavQdrCloseTo(DegOfRad(baseleg_out_qdr)-(nav_radius/fabs(nav_radius))*10), 10 > fabs(GetPosAlt() - WaypointAlt(WP__BASELEG)))" wp="_BASELEG"/>
     </block>
     <block name="final">
-      <exception cond="ground_alt + 10 > estimator_z" deroute="flare"/>
+      <exception cond="ground_alt + 10 > GetPosAlt()" deroute="flare"/>
       <go from="AF" hmode="route" vmode="glide" wp="TD" alt="ground_alt"/>
     </block>
     <block name="flare">
diff --git a/conf/flight_plans/mav06.xml b/conf/flight_plans/mav06.xml
index 864c6ccd0ec..464950ca4eb 100644
--- a/conf/flight_plans/mav06.xml
+++ b/conf/flight_plans/mav06.xml
@@ -30,11 +30,11 @@
       <set value="compute_baseleg()" var="unit"/>
       <circle radius="BOMB_RADIUS" until="circle_count > 0.5" wp="BASELEG"/>
       <set value="V_CTL_AUTO_THROTTLE_MIN_CRUISE_THROTTLE" var="v_ctl_auto_throttle_cruise_throttle"/>
-      <circle radius="BOMB_RADIUS" until="Qdr(DegOfRad(bomb_start_qdr)-10) && baseleg_alt_tolerance > fabs(estimator_z - baseleg_alt)" wp="BASELEG"/>
+      <circle radius="BOMB_RADIUS" until="Qdr(DegOfRad(bomb_start_qdr)-10) && baseleg_alt_tolerance > fabs(GetPosAlt() - baseleg_alt)" wp="BASELEG"/>
     </block>
 
     <block name="final">
-      <exception cond="ground_alt + 10 > estimator_z" deroute="flare"/>
+      <exception cond="ground_alt + 10 > GetPosAlt()" deroute="flare"/>
       <go alt="baseleg_alt" approaching_time="16" from="AF" hmode="route" wp="TD"/>
       <go approaching_time="2" from="AF" hmode="route" throttle="0.1" vmode="throttle" wp="TD"/>
     </block>
@@ -50,7 +50,7 @@
     </block>
 
     <block name="climb">
-      <exception cond=" estimator_z > ground_alt +30" deroute="wait"/>
+      <exception cond=" GetPosAlt() > ground_alt +30" deroute="wait"/>
       <attitude pitch="15" roll="0" throttle="0.8" until="10> stage_time" vmode="throttle"/>
     </block>-->
     <block name="wait" strip_button="Wait">
@@ -89,7 +89,7 @@
     <block name="roll" strip_button="Roll">
       <set value="TRUE" var="h_ctl_disabled"/>
       <set value="(-0.75*MAX_PPRZ)" var="h_ctl_aileron_setpoint"/>
-      <while cond="3 > stage_time && (estimator_phi > 0 ||  -M_PI_2 > estimator_phi)"/>
+      <while cond="3 > stage_time && (stateGetNedToBodyEulers_f()->phi > 0 ||  -M_PI_2 > stateGetNedToBodyEulers_f()->phi)"/>
       <set value="FALSE" var="h_ctl_disabled"/>
       <return/>
     </block>-->
diff --git a/conf/flight_plans/mav07.xml b/conf/flight_plans/mav07.xml
index d4e3429ae04..176f4e2bb9d 100644
--- a/conf/flight_plans/mav07.xml
+++ b/conf/flight_plans/mav07.xml
@@ -57,7 +57,7 @@
     </sector>
   </sectors>
   <exceptions>
-    <exception cond="!InsideFonsorbes(estimator_x, estimator_y)" deroute="kill"/>
+    <exception cond="!InsideFonsorbes(GetPosX(), GetPosY())" deroute="kill"/>
     <exception cond="datalink_time > 35" deroute="standby"/>
   </exceptions>
   <blocks>
@@ -73,7 +73,7 @@
       </while>
     </block>
     <block name="takeoff">
-      <exception cond="estimator_z > ground_alt + 25" deroute="standby"/>
+      <exception cond="GetPosAlt() > ground_alt + 25" deroute="standby"/>
       <set value="0" var="kill_throttle"/>
       <go wp="TAKEOFF"/>
     </block>
@@ -130,10 +130,10 @@
     <block name="land">
       <call fun="nav_compute_baseleg(WP_AF, WP_TD, WP_BASELEG, nav_radius)"/>
       <circle radius="nav_radius" until="NavCircleCount() > 0.5" wp="BASELEG"/>
-      <circle radius="nav_radius" until="NavQdrCloseTo(DegOfRad(baseleg_out_qdr)-(nav_radius/fabs(nav_radius))*10) && 10 > fabs(estimator_z - WaypointAlt(WP_BASELEG))" wp="BASELEG"/>
+      <circle radius="nav_radius" until="NavQdrCloseTo(DegOfRad(baseleg_out_qdr)-(nav_radius/fabs(nav_radius))*10) && 10 > fabs(GetPosAlt() - WaypointAlt(WP_BASELEG))" wp="BASELEG"/>
     </block>
     <block name="final">
-      <exception cond="ground_alt + 10 > estimator_z" deroute="flare"/>
+      <exception cond="ground_alt + 10 > GetPosAlt()" deroute="flare"/>
       <call fun="compute_TOD(WP_AF, WP_TD, WP_TOD, GLIDE_AIRSPEED, GLIDE_VSPEED)"/>
       <go approaching_time="0" from="AF" hmode="route" wp="TOD"/>
       <go from="TOD" hmode="route" pitch="stage_time*(GLIDE_PITCH/3)" throttle="0" vmode="throttle" wp="TD"/>
@@ -144,11 +144,11 @@
     </block>
 
     <block name="T3 Square">
-      <exception cond="! InsideT3(estimator_x, estimator_y)" deroute="come back"/>
+      <exception cond="! InsideT3(GetPosX(), GetPosY())" deroute="come back"/>
       <attitude roll="0" vmode="alt" alt="WaypointAlt(WP_T3)"/>
     </block>
     <block name="come back">
-      <exception cond="InsideT3(estimator_x, estimator_y)" deroute="T3 Square"/>
+      <exception cond="InsideT3(GetPosX(), GetPosY())" deroute="T3 Square"/>
       <go wp="T3"/>
     </block>
   </blocks>
diff --git a/conf/flight_plans/mav08.xml b/conf/flight_plans/mav08.xml
index 084723612ce..35b5c415e14 100644
--- a/conf/flight_plans/mav08.xml
+++ b/conf/flight_plans/mav08.xml
@@ -42,7 +42,7 @@
       <attitude roll="0" throttle="0" vmode="throttle"/>
     </block>
     <block name="Takeoff" strip_button="Takeoff (wp CLIMB)" strip_icon="takeoff.png">
-      <exception cond="estimator_z > ground_alt+25" deroute="Standby"/>
+      <exception cond="GetPosAlt() > ground_alt+25" deroute="Standby"/>
       <set value="0" var="kill_throttle"/>
       <set value="0" var="estimator_flight_time"/>
       <go from="HOME" pitch="15" throttle="1.0" vmode="throttle" wp="CLIMB"/>
@@ -98,10 +98,10 @@
     <block name="land">
       <call fun="nav_compute_baseleg(WP_AF, WP_TD, WP__BASELEG, nav_radius)"/>
       <circle radius="nav_radius" until="NavCircleCount() > 0.5" wp="_BASELEG"/>
-      <circle radius="nav_radius" until="NavQdrCloseTo(DegOfRad(baseleg_out_qdr)-(nav_radius/fabs(nav_radius))*10) && 10 > fabs(estimator_z - WaypointAlt(WP__BASELEG))" wp="_BASELEG"/>
+      <circle radius="nav_radius" until="NavQdrCloseTo(DegOfRad(baseleg_out_qdr)-(nav_radius/fabs(nav_radius))*10) && 10 > fabs(GetPosAlt() - WaypointAlt(WP__BASELEG))" wp="_BASELEG"/>
     </block>
     <block name="final">
-      <exception cond="ground_alt + 10 > estimator_z" deroute="flare"/>
+      <exception cond="ground_alt + 10 > GetPosAlt()" deroute="flare"/>
       <go from="AF" hmode="route" vmode="glide" wp="TD"/>
     </block>
     <block name="flare">
diff --git a/conf/flight_plans/muret_for.xml b/conf/flight_plans/muret_for.xml
index 233cf0308a5..af469421792 100644
--- a/conf/flight_plans/muret_for.xml
+++ b/conf/flight_plans/muret_for.xml
@@ -10,7 +10,7 @@
     <block name="init">
       <while cond="(!launch)"/>
       <heading course="QFU" throttle="0.8" pitch="10" until="(estimator_flight_time > 8)" vmode="throttle"/>
-      <heading climb="3.0" course="QFU" pitch="10" until="(estimator_z > SECURITY_ALT)" vmode="climb"/>
+      <heading climb="3.0" course="QFU" pitch="10" until="(GetPosAlt() > SECURITY_ALT)" vmode="climb"/>
     </block>
     <block name="for">
       <for from="0" to="3" var="i">
diff --git a/conf/flight_plans/nordlys.xml b/conf/flight_plans/nordlys.xml
index ae8584808c6..d82892b5f7d 100644
--- a/conf/flight_plans/nordlys.xml
+++ b/conf/flight_plans/nordlys.xml
@@ -20,7 +20,7 @@
     <waypoint name="PROF_LAND" x="168.6" y="36.4"/>
   </waypoints>
   <exceptions>
-    <exception cond="estimator_z > 5000"  deroute="wait"/>
+    <exception cond="GetPosAlt() > 5000"  deroute="wait"/>
     <exception cond="datalink_time > 140"  deroute="wait"/>
     <exception cond="10 > PowerVoltage()" deroute="wait"/>
   </exceptions>
@@ -38,7 +38,7 @@
       <set value="TRUE" var="h_ctl_disabled"/>
       <set value="(-0.6*MAX_PPRZ)" var="h_ctl_aileron_setpoint"/>
       <set value="(0.9*MAX_PPRZ)" var="h_ctl_elevator_setpoint"/>
-      <while cond="estimator_z > ground_alt + 220"/>
+      <while cond="GetPosAlt() > ground_alt + 220"/>
       <set value="0" var="h_ctl_aileron_setpoint"/>
       <set value="0" var="h_ctl_elevator_setpoint"/>
     </block>
@@ -71,7 +71,7 @@
       <circle radius="nav_radius" wp="PROF_LAND"/>
     </block>
     <block name="profile_down land" strip_button="Profile Down Land" strip_icon="down_profile.png">
-      <circle pitch="RadOfDeg(-10)" radius="nav_radius" throttle="0" until="(ground_alt+100 > estimator_z)" vmode="throttle" wp="PROF_LAND"/>
+      <circle pitch="RadOfDeg(-10)" radius="nav_radius" throttle="0" until="(ground_alt+100 > GetPosAlt())" vmode="throttle" wp="PROF_LAND"/>
       <deroute block="wait"/>
     </block>
     <block name="Land Right AF-TD" strip_button="Land right (wp AF-TD)" strip_icon="land-right.png">
@@ -85,10 +85,10 @@
     <block name="land">
       <call fun="nav_compute_baseleg(WP_AF, WP_TD, WP__BASELEG, nav_radius)"/>
       <circle radius="nav_radius" until="NavCircleCount() > 0.5" wp="_BASELEG"/>
-      <circle radius="nav_radius" until="NavQdrCloseTo(DegOfRad(baseleg_out_qdr)-(nav_radius/fabs(nav_radius))*10) && 20 > fabs(estimator_z - WaypointAlt(WP__BASELEG))" wp="_BASELEG"/>
+      <circle radius="nav_radius" until="NavQdrCloseTo(DegOfRad(baseleg_out_qdr)-(nav_radius/fabs(nav_radius))*10) && 20 > fabs(GetPosAlt() - WaypointAlt(WP__BASELEG))" wp="_BASELEG"/>
     </block>
     <block name="final">
-      <exception cond="ground_alt + 10 > estimator_z" deroute="flare"/>
+      <exception cond="ground_alt + 10 > GetPosAlt()" deroute="flare"/>
       <go from="AF" hmode="route" vmode="glide" wp="TD"/>
     </block>
     <block name="flare">
diff --git a/conf/flight_plans/poles.xml b/conf/flight_plans/poles.xml
index 539367ea892..480a51b46eb 100644
--- a/conf/flight_plans/poles.xml
+++ b/conf/flight_plans/poles.xml
@@ -32,7 +32,7 @@
       <attitude roll="0" throttle="0" vmode="throttle"/>
     </block>
     <block key="t" name="Takeoff" strip_button="Takeoff (wp CLIMB)" strip_icon="takeoff.png">
-      <exception cond="estimator_z > ground_alt+25" deroute="Standby"/>
+      <exception cond="GetPosAlt() > ground_alt+25" deroute="Standby"/>
       <set value="0" var="kill_throttle"/>
       <set value="0" var="estimator_flight_time"/>
       <go from="HOME" pitch="15" throttle="1.0" vmode="throttle" wp="CLIMB"/>
@@ -70,10 +70,10 @@
     <block name="land">
       <call fun="nav_compute_baseleg(WP_AF, WP_TD, WP__BASELEG, nav_radius)"/>
       <circle radius="nav_radius" until="NavCircleCount() > 0.5" wp="_BASELEG"/>
-      <circle radius="nav_radius" until="And(NavQdrCloseTo(DegOfRad(baseleg_out_qdr)-(nav_radius/fabs(nav_radius))*10), 10 > fabs(estimator_z - WaypointAlt(WP__BASELEG)))" wp="_BASELEG"/>
+      <circle radius="nav_radius" until="And(NavQdrCloseTo(DegOfRad(baseleg_out_qdr)-(nav_radius/fabs(nav_radius))*10), 10 > fabs(GetPosAlt() - WaypointAlt(WP__BASELEG)))" wp="_BASELEG"/>
     </block>
     <block name="final">
-      <exception cond="ground_alt + 10 > estimator_z" deroute="flare"/>
+      <exception cond="ground_alt + 10 > GetPosAlt()" deroute="flare"/>
       <go from="AF" hmode="route" vmode="glide" wp="TD"/>
     </block>
     <block name="flare">
diff --git a/conf/flight_plans/sinsat.xml b/conf/flight_plans/sinsat.xml
index beb5132f5ab..8186ca27e64 100644
--- a/conf/flight_plans/sinsat.xml
+++ b/conf/flight_plans/sinsat.xml
@@ -7,7 +7,7 @@
     <block NAME="init">
       <while COND="(!launch)"/>
       <heading THROTTLE="0.8" PITCH="0.15" COURSE="QFU" UNTIL="(estimator_flight_time > 8)" VMODE="throttle"/>
-      <heading PITCH="0.15" CLIMB="3.0" COURSE="QFU" UNTIL="(estimator_z > SECURITY_ALT)" VMODE="climb"/>
+      <heading PITCH="0.15" CLIMB="3.0" COURSE="QFU" UNTIL="(GetPosAlt() > SECURITY_ALT)" VMODE="climb"/>
       <deroute BLOCK="circlehome"/>
     </block>
     <block NAME="circlehome">
diff --git a/conf/flight_plans/slayer_training.xml b/conf/flight_plans/slayer_training.xml
index 92d84fe58c5..86a3f046e77 100644
--- a/conf/flight_plans/slayer_training.xml
+++ b/conf/flight_plans/slayer_training.xml
@@ -36,7 +36,7 @@
       <deroute block="climb"/>
     </block>
     <block name="climb">
-      <attitude pitch="10" roll="0" throttle="0.9" until="estimator_z > ground_alt+30" vmode="throttle"/>
+      <attitude pitch="10" roll="0" throttle="0.9" until="GetPosAlt() > ground_alt+30" vmode="throttle"/>
     </block>
     <block name="standby" strip_button="STDBY">
       <circle radius="nav_radius" wp="STDBY"/>
@@ -75,10 +75,10 @@
       <call fun="nav_compute_baseleg(WP_AF, WP_TD, WP_BASELEG)"/>
       <circle radius="nav_radius" until="NavCircleCount() > 0.5" wp="BASELEG"/>
       <set value="V_CTL_AUTO_THROTTLE_MIN_CRUISE_THROTTLE" var="v_ctl_auto_throttle_cruise_throttle"/>
-      <circle radius="nav_radius" until="NavQdrCloseTo(DegOfRad(baseleg_out_qdr)-(nav_radius/fabs(nav_radius))*10) && 10 > fabs(estimator_z - WaypointAlt(WP_BASELEG))" wp="BASELEG"/>
+      <circle radius="nav_radius" until="NavQdrCloseTo(DegOfRad(baseleg_out_qdr)-(nav_radius/fabs(nav_radius))*10) && 10 > fabs(GetPosAlt() - WaypointAlt(WP_BASELEG))" wp="BASELEG"/>
     </block>
     <block name="final">
-      <exception cond="ground_alt + 10 > estimator_z" deroute="flare"/>
+      <exception cond="ground_alt + 10 > GetPosAlt()" deroute="flare"/>
       <go from="AF" hmode="route" vmode="glide" wp="TD"/>
     </block>
     <block name="flare">
diff --git a/conf/flight_plans/snav.xml b/conf/flight_plans/snav.xml
index 0dd2e92c734..71c3faa9478 100644
--- a/conf/flight_plans/snav.xml
+++ b/conf/flight_plans/snav.xml
@@ -34,7 +34,7 @@
       <attitude roll="0" throttle="0" vmode="throttle"/>
     </block>
     <block name="Takeoff" strip_button="Takeoff (wp CLIMB)" strip_icon="takeoff.png">
-      <exception cond="estimator_z > ground_alt+25" deroute="Standby"/>
+      <exception cond="GetPosAlt() > ground_alt+25" deroute="Standby"/>
       <set value="0" var="kill_throttle"/>
       <set value="0" var="estimator_flight_time"/>
       <go from="HOME" throttle="1.0" vmode="throttle" wp="CLIMB"/>
@@ -55,10 +55,10 @@
     <block name="land">
       <call fun="nav_compute_baseleg(WP_AF, WP_TD, WP__BASELEG, nav_radius)"/>
       <circle radius="nav_radius" until="NavCircleCount() > 0.5" wp="_BASELEG"/>
-      <circle radius="nav_radius" until="NavQdrCloseTo(DegOfRad(baseleg_out_qdr)-(nav_radius/fabs(nav_radius))*10) && 10 > fabs(estimator_z - WaypointAlt(WP__BASELEG))" wp="_BASELEG"/>
+      <circle radius="nav_radius" until="NavQdrCloseTo(DegOfRad(baseleg_out_qdr)-(nav_radius/fabs(nav_radius))*10) && 10 > fabs(GetPosAlt() - WaypointAlt(WP__BASELEG))" wp="_BASELEG"/>
     </block>
     <block name="final">
-      <exception cond="ground_alt + 10 > estimator_z" deroute="flare"/>
+      <exception cond="ground_alt + 10 > GetPosAlt()" deroute="flare"/>
       <go from="AF" hmode="route" vmode="glide" wp="TD"/>
     </block>
     <block name="flare">
diff --git a/conf/flight_plans/tcas.xml b/conf/flight_plans/tcas.xml
index e2b3045e20b..c30f6295f78 100644
--- a/conf/flight_plans/tcas.xml
+++ b/conf/flight_plans/tcas.xml
@@ -34,7 +34,7 @@
       <attitude roll="0" throttle="0" vmode="throttle"/>
     </block>
     <block name="Takeoff" strip_button="Takeoff (wp CLIMB)" strip_icon="takeoff.png">
-      <exception cond="estimator_z > ground_alt+25" deroute="Standby"/>
+      <exception cond="GetPosAlt() > ground_alt+25" deroute="Standby"/>
       <set value="0" var="kill_throttle"/>
       <set value="0" var="estimator_flight_time"/>
       <go from="HOME" throttle="1.0" vmode="throttle" wp="CLIMB"/>
@@ -72,10 +72,10 @@
     <block name="land">
       <call fun="nav_compute_baseleg(WP_AF, WP_TD, WP__BASELEG, nav_radius)"/>
       <circle radius="nav_radius" until="NavCircleCount() > 0.5" wp="_BASELEG"/>
-      <circle radius="nav_radius" until="NavQdrCloseTo(DegOfRad(baseleg_out_qdr)-(nav_radius/fabs(nav_radius))*10) && 10 > fabs(estimator_z - WaypointAlt(WP__BASELEG))" wp="_BASELEG"/>
+      <circle radius="nav_radius" until="NavQdrCloseTo(DegOfRad(baseleg_out_qdr)-(nav_radius/fabs(nav_radius))*10) && 10 > fabs(GetPosAlt() - WaypointAlt(WP__BASELEG))" wp="_BASELEG"/>
     </block>
     <block name="final">
-      <exception cond="ground_alt + 10 > estimator_z" deroute="flare"/>
+      <exception cond="ground_alt + 10 > GetPosAlt()" deroute="flare"/>
       <go from="AF" hmode="route" vmode="glide" wp="TD"/>
     </block>
     <block name="flare">
diff --git a/conf/flight_plans/versatile.xml b/conf/flight_plans/versatile.xml
index 571ab05e04e..9767e9cfded 100644
--- a/conf/flight_plans/versatile.xml
+++ b/conf/flight_plans/versatile.xml
@@ -44,7 +44,7 @@
       <attitude roll="0" throttle="0" vmode="throttle"/>
     </block>
     <block name="Takeoff" strip_icon="takeoff.png" strip_button="Takeoff (wp CLIMB)">
-      <exception cond="estimator_z > ground_alt+25" deroute="Standby"/>
+      <exception cond="GetPosAlt() > ground_alt+25" deroute="Standby"/>
       <set value="0" var="kill_throttle"/>
       <go wp="CLIMB"/>
     </block>
@@ -65,25 +65,25 @@
       <circle pitch="auto" radius="75" throttle="0.7" wp="1"/>
     </block>
     <block name="Climb 75% throttle">
-      <circle pitch="10" radius="50+(estimator_z-ground_alt)/2" throttle="0.75" until="(10 > PowerVoltage()) || (estimator_z > ground_alt+ 1350)" vmode="throttle" wp="1"/>
+      <circle pitch="10" radius="50+(GetPosAlt()-ground_alt)/2" throttle="0.75" until="(10 > PowerVoltage()) || (GetPosAlt() > ground_alt+ 1350)" vmode="throttle" wp="1"/>
     </block>
     <block name="Climb 0m/s">
       <circle climb="0" radius="nav_radius" until="10 > PowerVoltage()" vmode="climb" wp="1"/>
     </block>
     <block name="Climb 1m/s">
-      <circle climb="1" pitch="5" radius="50+(estimator_z-ground_alt)/2" until="10 > PowerVoltage()" vmode="climb" wp="1"/>
+      <circle climb="1" pitch="5" radius="50+(GetPosAlt()-ground_alt)/2" until="10 > PowerVoltage()" vmode="climb" wp="1"/>
     </block>
     <block name="Climb nav_climb m/s">
-      <circle climb="nav_climb" radius="nav_radius" until="(10 > PowerVoltage()) || (estimator_z > ground_alt+ 1350)" vmode="climb" wp="1"/>
+      <circle climb="nav_climb" radius="nav_radius" until="(10 > PowerVoltage()) || (GetPosAlt() > ground_alt+ 1350)" vmode="climb" wp="1"/>
     </block>
 
     <block name="Circle 0% throttle">
-      <circle pitch="fp_pitch" radius="nav_radius" throttle="0.0" until="ground_alt+50 > estimator_z" vmode="throttle" wp="1"/>
+      <circle pitch="fp_pitch" radius="nav_radius" throttle="0.0" until="ground_alt+50 > GetPosAlt()" vmode="throttle" wp="1"/>
       <deroute block="Standby"/>
     </block>
 
     <block name="Oval 0% throttle">
-      <oval p1="1" p2="2" pitch="fp_pitch" radius="nav_radius" throttle="0.0" until="ground_alt+50 > estimator_z" vmode="throttle"/>
+      <oval p1="1" p2="2" pitch="fp_pitch" radius="nav_radius" throttle="0.0" until="ground_alt+50 > GetPosAlt()" vmode="throttle"/>
       <deroute block="Standby"/>
     </block>
 
@@ -119,10 +119,10 @@
       <call fun="nav_compute_baseleg(WP_AF, WP_TD, WP_BASELEG, nav_radius)"/>
       <circle radius="nav_radius" until="NavCircleCount() > 0.5" wp="BASELEG"/>
       <set value="V_CTL_AUTO_THROTTLE_MIN_CRUISE_THROTTLE" var="v_ctl_auto_throttle_cruise_throttle"/>
-      <circle radius="nav_radius" until="NavQdrCloseTo(DegOfRad(baseleg_out_qdr)-(nav_radius/fabs(nav_radius))*10) && 10 > fabs(estimator_z - WaypointAlt(WP_BASELEG))" wp="BASELEG"/>
+      <circle radius="nav_radius" until="NavQdrCloseTo(DegOfRad(baseleg_out_qdr)-(nav_radius/fabs(nav_radius))*10) && 10 > fabs(GetPosAlt() - WaypointAlt(WP_BASELEG))" wp="BASELEG"/>
     </block>
     <block name="final">
-      <exception cond="ground_alt + 10 > estimator_z" deroute="flare"/>
+      <exception cond="ground_alt + 10 > GetPosAlt()" deroute="flare"/>
       <go from="AF" hmode="route" vmode="glide" wp="TD"/>
     </block>
     <block name="flare">
@@ -162,11 +162,11 @@
       <go from="2" hmode="route" wp="STDBY"/>
     </block>
     <block name="Fly in Square">
-      <exception cond="! InsideSquare(estimator_x, estimator_y)" deroute="Come back wp 1"/>
+      <exception cond="! InsideSquare(GetPosX(), GetPosY())" deroute="Come back wp 1"/>
       <attitude alt="ground_alt+75" roll="0" vmode="alt"/>
     </block>
     <block name="Come back wp 1">
-      <exception cond="InsideSquare(estimator_x, estimator_y)" deroute="Fly in Square"/>
+      <exception cond="InsideSquare(GetPosX(), GetPosY())" deroute="Fly in Square"/>
       <go wp="1"/>
       <deroute block="Fly in Square"/>
     </block>
diff --git a/conf/flight_plans/versatile_airspeed.xml b/conf/flight_plans/versatile_airspeed.xml
index 1a0aeafbc51..1427adf72c2 100644
--- a/conf/flight_plans/versatile_airspeed.xml
+++ b/conf/flight_plans/versatile_airspeed.xml
@@ -45,7 +45,7 @@
       <attitude roll="0" throttle="0" vmode="throttle"/>
     </block>
     <block name="Takeoff" strip_icon="takeoff.png" strip_button="Takeoff (wp CLIMB)">
-      <exception cond="estimator_z > ground_alt+25" deroute="Standby"/>
+      <exception cond="GetPosAlt() > ground_alt+25" deroute="Standby"/>
       <set value="0" var="kill_throttle"/>
       <set value="CLIMB_AIRSPEED" var="v_ctl_auto_airspeed_setpoint" />
       <go wp="CLIMB"/>
@@ -69,25 +69,25 @@
       <circle pitch="auto" radius="75" throttle="0.7" wp="1"/>
     </block>
     <block name="Climb 75% throttle">
-      <circle pitch="10" radius="50+(estimator_z-ground_alt)/2" throttle="0.75" until="(10 > PowerVoltage()) || (estimator_z > ground_alt+ 1350)" vmode="throttle" wp="1"/>
+      <circle pitch="10" radius="50+(GetPosAlt()-ground_alt)/2" throttle="0.75" until="(10 > PowerVoltage()) || (GetPosAlt() > ground_alt+ 1350)" vmode="throttle" wp="1"/>
     </block>
     <block name="Climb 0m/s">
       <circle climb="0" radius="nav_radius" until="10 > PowerVoltage()" vmode="climb" wp="1"/>
     </block>
     <block name="Climb 1m/s">
-      <circle climb="1" pitch="5" radius="50+(estimator_z-ground_alt)/2" until="10 > PowerVoltage()" vmode="climb" wp="1"/>
+      <circle climb="1" pitch="5" radius="50+(GetPosAlt()-ground_alt)/2" until="10 > PowerVoltage()" vmode="climb" wp="1"/>
     </block>
     <block name="Climb nav_climb m/s">
-      <circle climb="nav_climb" radius="nav_radius" until="(10 > PowerVoltage()) || (estimator_z > ground_alt+ 1350)" vmode="climb" wp="1"/>
+      <circle climb="nav_climb" radius="nav_radius" until="(10 > PowerVoltage()) || (GetPosAlt() > ground_alt+ 1350)" vmode="climb" wp="1"/>
     </block>
 
     <block name="Circle 0% throttle">
-      <circle pitch="fp_pitch" radius="nav_radius" throttle="0.0" until="ground_alt+50 > estimator_z" vmode="throttle" wp="1"/>
+      <circle pitch="fp_pitch" radius="nav_radius" throttle="0.0" until="ground_alt+50 > GetPosAlt()" vmode="throttle" wp="1"/>
       <deroute block="Standby"/>
     </block>
 
     <block name="Oval 0% throttle">
-      <oval p1="1" p2="2" pitch="fp_pitch" radius="nav_radius" throttle="0.0" until="ground_alt+50 > estimator_z" vmode="throttle"/>
+      <oval p1="1" p2="2" pitch="fp_pitch" radius="nav_radius" throttle="0.0" until="ground_alt+50 > GetPosAlt()" vmode="throttle"/>
       <deroute block="Standby"/>
     </block>
 
@@ -134,7 +134,7 @@
       <call fun="nav_compute_baseleg(WP_AF, WP_TD, WP_BASELEG, nav_radius)"/>
       <circle radius="nav_radius" until="NavCircleCount() > 0.5" wp="BASELEG"/>
       <set value="GLIDE_AIRSPEED" var="v_ctl_auto_airspeed_setpoint" />
-      <circle radius="nav_radius" until="NavQdrCloseTo(DegOfRad(baseleg_out_qdr)-(nav_radius/fabs(nav_radius))*10) && 10 > fabs(estimator_z - WaypointAlt(WP_BASELEG))" wp="BASELEG"/>
+      <circle radius="nav_radius" until="NavQdrCloseTo(DegOfRad(baseleg_out_qdr)-(nav_radius/fabs(nav_radius))*10) && 10 > fabs(GetPosAlt() - WaypointAlt(WP_BASELEG))" wp="BASELEG"/>
     </block>
     <block name="final">
       <go from="AF" hmode="route" vmode="glide" wp="TD" approaching_time="5" />
@@ -177,11 +177,11 @@
       <go from="2" hmode="route" wp="STDBY"/>
     </block>
     <block name="Fly in Square">
-      <exception cond="! InsideSquare(estimator_x, estimator_y)" deroute="Come back wp 1"/>
+      <exception cond="! InsideSquare(GetPosX(), GetPosY())" deroute="Come back wp 1"/>
       <attitude alt="ground_alt+75" roll="0" vmode="alt"/>
     </block>
     <block name="Come back wp 1">
-      <exception cond="InsideSquare(estimator_x, estimator_y)" deroute="Fly in Square"/>
+      <exception cond="InsideSquare(GetPosX(), GetPosY())" deroute="Fly in Square"/>
       <go wp="1"/>
       <deroute block="Fly in Square"/>
     </block>
diff --git a/conf/flight_plans/versatile_carto_fixe_muret.xml b/conf/flight_plans/versatile_carto_fixe_muret.xml
index 0c52f2d5267..d89466c3681 100644
--- a/conf/flight_plans/versatile_carto_fixe_muret.xml
+++ b/conf/flight_plans/versatile_carto_fixe_muret.xml
@@ -61,7 +61,7 @@ float varsweepsize=110;
     </block>
 
     <block name="Takeoff" strip_button="Takeoff (wp CLIMB)" strip_icon="takeoff.png">
-      <exception cond="estimator_z > ground_alt+25" deroute="Standby"/>
+      <exception cond="GetPosAlt() > ground_alt+25" deroute="Standby"/>
       <set value="0" var="kill_throttle"/>
       <go wp="CLIMB"/>
     </block>
@@ -112,7 +112,7 @@ float varsweepsize=110;
       <call fun="NavSetWaypointHere(WP_MOB)"/>
       <set value="DEFAULT_CIRCLE_RADIUS" var="nav_radius"/>
       <circle alt="ground_alt+105" radius="nav_radius" wp="MOB"/>
-      <exception cond="estimator_z > ground_alt+100" deroute="Carto survey S1-S2-S3"/>
+      <exception cond="GetPosAlt() > ground_alt+100" deroute="Carto survey S1-S2-S3"/>
     </block>
 
 
@@ -140,7 +140,7 @@ float varsweepsize=110;
       <call fun="NavSetWaypointHere(WP_MOB)"/>
       <set value="DEFAULT_CIRCLE_RADIUS" var="nav_radius"/>
       <circle alt="ground_alt+35" radius="nav_radius" wp="MOB"/>
-      <exception cond="ground_alt+40 > estimator_z   " deroute="Carto survey SP1-SP2-SP3 fin"/>
+      <exception cond="ground_alt+40 > GetPosAlt()   " deroute="Carto survey SP1-SP2-SP3 fin"/>
     </block>
 
     <block name="Carto survey SP1-SP2-SP3 fin" strip_button="Survey (wp S1-S2)" strip_icon="survey.png">
diff --git a/conf/flight_plans/xsens_cachejunction.xml b/conf/flight_plans/xsens_cachejunction.xml
index 2179cf40940..4a1859f9d19 100644
--- a/conf/flight_plans/xsens_cachejunction.xml
+++ b/conf/flight_plans/xsens_cachejunction.xml
@@ -24,7 +24,7 @@
   <exceptions/>
   <blocks>
     <block name="Takeoff" strip_button="Takeoff (wp CLIMB)" strip_icon="takeoff.png">
-      <exception cond="estimator_z > ground_alt+25" deroute="Standby"/>
+      <exception cond="GetPosAlt() > ground_alt+25" deroute="Standby"/>
       <set value="0" var="kill_throttle"/>
       <set value="0" var="estimator_flight_time"/>
       <go from="HOME" pitch="15" throttle="1.0" vmode="throttle" wp="CLIMB"/>
@@ -89,10 +89,10 @@
     <block name="land">
       <call fun="nav_compute_baseleg(WP_AF, WP_TD, WP__BASELEG, nav_radius)"/>
       <circle radius="nav_radius" until="NavCircleCount() > 0.5" wp="_BASELEG"/>
-      <circle radius="nav_radius" until="NavQdrCloseTo(DegOfRad(baseleg_out_qdr)-(nav_radius/fabs(nav_radius))*10) && 10 > fabs(estimator_z - WaypointAlt(WP__BASELEG))" wp="_BASELEG"/>
+      <circle radius="nav_radius" until="NavQdrCloseTo(DegOfRad(baseleg_out_qdr)-(nav_radius/fabs(nav_radius))*10) && 10 > fabs(GetPosAlt() - WaypointAlt(WP__BASELEG))" wp="_BASELEG"/>
     </block>
     <block name="final">
-      <exception cond="ground_alt + 10 > estimator_z" deroute="flare"/>
+      <exception cond="ground_alt + 10 > GetPosAlt()" deroute="flare"/>
       <go from="AF" hmode="route" vmode="glide" wp="TD"/>
     </block>
     <block name="flare">
diff --git a/sw/airborne/subsystems/nav.h b/sw/airborne/subsystems/nav.h
index 31ea51d0637..e719594fa75 100644
--- a/sw/airborne/subsystems/nav.h
+++ b/sw/airborne/subsystems/nav.h
@@ -194,4 +194,9 @@ bool_t nav_approaching_xy(float x, float y, float from_x, float from_y, float ap
 #define nav_SetNavRadius(x) { if (x==1) nav_radius = DEFAULT_CIRCLE_RADIUS; else if (x==-1) nav_radius = -DEFAULT_CIRCLE_RADIUS; else nav_radius = x; }
 
 #define NavKillThrottle() { kill_throttle = 1; }
+
+#define GetPosX() (stateGetPositionUtm_f()->north)
+#define GetPosY() (stateGetPositionUtm_f()->east)
+#define GetPosAlt() (stateGetPositionUtm_f()->alt)
+
 #endif /* NAV_H */