diff --git a/sw/lib/ocaml/mapWaypoints.ml b/sw/lib/ocaml/mapWaypoints.ml
index 957f02dbf8..38c5f820e0 100644
--- a/sw/lib/ocaml/mapWaypoints.ml
+++ b/sw/lib/ocaml/mapWaypoints.ml
@@ -203,7 +203,7 @@ object (self)
       try
         set_coordinates ();
         let wgs84 = self#pos in
-        let agl  = ea#value -. float (try Srtm.of_wgs84 wgs84 with _ -> 0) in
+        let agl  = ea#value -. (try float (Srtm.of_wgs84 wgs84) with _ -> ground_alt) in
         agl_lab#set_text (sprintf " AGL: %4.0fm" agl)
       with _ -> ()
     in
diff --git a/sw/simulator/sim.ml b/sw/simulator/sim.ml
index 059afe960e..44228c7e49 100644
--- a/sw/simulator/sim.ml
+++ b/sw/simulator/sim.ml
@@ -195,7 +195,7 @@ module Make(AircraftItl : AIRCRAFT_ITL) = struct
               Some s ->
                 begin
                   try s.Gps.alt -. float (Srtm.of_wgs84 s.Gps.wgs84) with
-                      _ -> s.Gps.alt
+                      _ -> s.Gps.alt -. !alt0
                 end
             | None -> 0. in
       FM.state_update !state FM.nominal_airspeed (!wind_x, !wind_y, !wind_z) agl fm_period