diff --git a/conf/conf_tests.xml b/conf/conf_tests.xml
index 543bd25c81..95ef0c4894 100644
--- a/conf/conf_tests.xml
+++ b/conf/conf_tests.xml
@@ -5,7 +5,7 @@
    airframe="airframes/AGGIEAIR/aggieair_ark_quad_lisa_mx.xml"
    radio="radios/AGGIEAIR/aggieair_taranis.xml"
    telemetry="telemetry/default_rotorcraft.xml"
-   flight_plan="flight_plans/rotorcraft_basic.xml"
+   flight_plan="flight_plans/rotorcraft_basic_geofence.xml"
    settings="settings/rotorcraft_basic.xml settings/control/rotorcraft_guidance.xml settings/nps.xml settings/control/stabilization_att_float_euler.xml"
    settings_modules="modules/gps.xml"
    gui_color="#ffff954c0000"
diff --git a/conf/flight_plans/AGGIEAIR/BasicTuning_Launcher.xml b/conf/flight_plans/AGGIEAIR/BasicTuning_Launcher.xml
index d6d4fa600a..5e18ceafb4 100644
--- a/conf/flight_plans/AGGIEAIR/BasicTuning_Launcher.xml
+++ b/conf/flight_plans/AGGIEAIR/BasicTuning_Launcher.xml
@@ -1,6 +1,6 @@
 <!DOCTYPE flight_plan SYSTEM "../flight_plan.dtd">
 
-<flight_plan alt="1550" ground_alt="1350" lat0="41.815562" lon0="-111.982437" max_dist_from_home="3000" name="BasicTuning" security_height="25" qfu="90">
+<flight_plan alt="1550" ground_alt="1350" lat0="41.815562" lon0="-111.982437" max_dist_from_home="3000" name="BasicTuning" security_height="25" home_mode_height="-200" qfu="90" geofence_sector="FlightArea" geofence_max_alt="2000" geofence_max_agl="500">
   <header>
 #include "subsystems/datalink/datalink.h"
 </header>
@@ -30,7 +30,7 @@
       <corner name="_S3"/>
       <corner name="_S4"/>
     </sector>
-    <sector name="FlightArea">
+    <sector name="FlightArea" color="red">
       <corner name="_P1"/>
       <corner name="_P2"/>
       <corner name="_P3"/>
diff --git a/conf/flight_plans/flight_plan.dtd b/conf/flight_plans/flight_plan.dtd
index b12837ccc0..d73ac94cf8 100644
--- a/conf/flight_plans/flight_plan.dtd
+++ b/conf/flight_plans/flight_plan.dtd
@@ -68,6 +68,7 @@ alt CDATA #REQUIRED
 qfu CDATA #IMPLIED
 home_mode_height CDATA #IMPLIED
 geofence_max_alt CDATA #IMPLIED
+geofence_max_agl CDATA #IMPLIED
 geofence_sector CDATA #IMPLIED>
 
 <!ATTLIST waypoints
diff --git a/sw/airborne/modules/nav/nav_geofence.h b/sw/airborne/modules/nav/nav_geofence.h
index 120ce3975c..388ad8171d 100644
--- a/sw/airborne/modules/nav/nav_geofence.h
+++ b/sw/airborne/modules/nav/nav_geofence.h
@@ -49,10 +49,18 @@ static inline bool datalink_lost(void)
 #endif /* GEOFENCE_DATALINK_LOST_TIME */
 
 
-#ifdef GEOFENCE_MAX_ALTITUDE// user defined geofence_max_altitude in the flight plan
+#if defined GEOFENCE_MAX_ALTITUDE || defined GEOFENCE_MAX_AGL// user defined geofence_max_altitude (or AGL) in the flight plan
 static inline bool higher_than_max_altitude(void)
 {
-  return (GetPosAlt() > GEOFENCE_MAX_ALTITUDE);
+  bool above_max_alt = false;
+#ifdef GEOFENCE_MAX_ALTITUDE
+  above_max_alt = above_max_alt || (GetPosAlt() > GEOFENCE_MAX_ALTITUDE);
+#endif /* GEOFENCE_MAX_ALTITUDE */
+
+#ifdef GEOFENCE_MAX_AGL
+  above_max_alt = above_max_alt || (GetPosAlt() > ( GetAltRef() + GEOFENCE_MAX_AGL));
+#endif /* GEOFENCE_MAX_AGL */
+  return above_max_alt;
 }
 #else // we dont have max altitude specified, so the condition is never true
 static inline bool higher_than_max_altitude(void)
diff --git a/sw/tools/generators/gen_flight_plan.ml b/sw/tools/generators/gen_flight_plan.ml
index 9544912531..1ac644a72d 100644
--- a/sw/tools/generators/gen_flight_plan.ml
+++ b/sw/tools/generators/gen_flight_plan.ml
@@ -931,6 +931,16 @@ let () =
       and mdfh = get_float "max_dist_from_home"
       and alt = ExtXml.attrib xml "alt" in
       security_height := get_float "security_height";
+      begin
+        try
+          if security_height < ref 0. then
+            begin
+              fprintf stderr "\nError: Security height cannot be negative (%.0f)\n" !security_height;
+              exit 1;
+            end
+        with
+          _ -> ()
+      end;
       ground_alt := get_float "ground_alt";
       let home_mode_height = try
                                max (get_float "home_mode_height") !security_height
@@ -1003,7 +1013,29 @@ let () =
             end
           else if geofence_max_alt < (float_of_string alt) then
             fprintf stderr "\nWarning: Geofence max altitude below default waypoint alt (%.0f < %.0f)\n" geofence_max_alt (float_of_string alt);
-          Xml2h.define "GEOFENCE_MAX_ALTITUDE" (sof geofence_max_alt)
+          Xml2h.define "GEOFENCE_MAX_ALTITUDE" (sof geofence_max_alt);
+          fprintf stderr "\nWarning: Geofence max altitude set to %.0f\n" geofence_max_alt;
+        with
+          _ -> ()
+      end;
+
+      begin 
+        try
+          let geofence_max_agl = get_float "geofence_max_agl" in
+          if geofence_max_agl < !security_height then
+            begin
+              fprintf stderr "\nError: Geofence max AGL below security height (%.0f < %.0f)\n" geofence_max_agl !security_height;
+              exit 1;
+            end
+          else if geofence_max_agl < home_mode_height then
+            begin
+              fprintf stderr "\nError: Geofence max AGL below home mode height (%.0f < %.0f)\n" geofence_max_agl home_mode_height;
+              exit 1;
+            end
+          else if (geofence_max_agl +. !ground_alt) < (float_of_string alt) then
+            fprintf stderr "\nWarning: Geofence max AGL below default waypoint AGL (%.0f < %.0f)\n" (geofence_max_agl +. !ground_alt) (float_of_string alt);
+          Xml2h.define "GEOFENCE_MAX_AGL" (sof geofence_max_agl);
+          fprintf stderr "\nWarning: Geofence max AGL set to %.0f\n" geofence_max_agl;
         with
           _ -> ()
       end;