diff --git a/conf/flight_plans/flight_plan.dtd b/conf/flight_plans/flight_plan.dtd
index f5bd1a97d6..02a375c752 100644
--- a/conf/flight_plans/flight_plan.dtd
+++ b/conf/flight_plans/flight_plan.dtd
@@ -310,4 +310,5 @@ default_value CDATA #IMPLIED>
 
 <!ATTLIST header>
 
-<!ATTLIST return>
+<!ATTLIST return
+reset_stage CDATA #IMPLIED>
diff --git a/sw/airborne/subsystems/navigation/common_flight_plan.c b/sw/airborne/subsystems/navigation/common_flight_plan.c
index 4653fd656c..f9bf31c719 100644
--- a/sw/airborne/subsystems/navigation/common_flight_plan.c
+++ b/sw/airborne/subsystems/navigation/common_flight_plan.c
@@ -54,5 +54,6 @@ void nav_goto_block(uint8_t b)
     last_block = nav_block;
     last_stage = nav_stage;
   }
-  GotoBlock(b);
+  nav_block = b;
+  nav_init_block();
 }
diff --git a/sw/airborne/subsystems/navigation/common_flight_plan.h b/sw/airborne/subsystems/navigation/common_flight_plan.h
index e472987362..59f738b8c1 100644
--- a/sw/airborne/subsystems/navigation/common_flight_plan.h
+++ b/sw/airborne/subsystems/navigation/common_flight_plan.h
@@ -44,8 +44,8 @@ void nav_goto_block(uint8_t block_id);
 #define InitStage() nav_init_stage();
 
 #define Block(x) case x: nav_block=x;
-#define NextBlock() { nav_block++; nav_init_block(); }
-#define GotoBlock(b) { nav_block=b; nav_init_block(); }
+#define NextBlock() nav_goto_block(nav_block + 1)
+#define GotoBlock(b) nav_goto_block(b)
 
 #define Stage(s) case s: nav_stage=s;
 #define NextStage() { nav_stage++; InitStage(); }
@@ -54,7 +54,7 @@ void nav_goto_block(uint8_t block_id);
 
 #define Label(x) label_ ## x:
 #define Goto(x) { goto label_ ## x; }
-#define Return() { nav_block=last_block; nav_stage=last_stage; block_time=0;}
+#define Return(x) { nav_block=last_block; if (x==1) {nav_stage=0;} else {nav_stage=last_stage;} block_time=0;}
 
 #define And(x, y) ((x) && (y))
 #define Or(x, y) ((x) || (y))
diff --git a/sw/tools/generators/gen_flight_plan.ml b/sw/tools/generators/gen_flight_plan.ml
index 2a163cd9bd..b183244ddd 100644
--- a/sw/tools/generators/gen_flight_plan.ml
+++ b/sw/tools/generators/gen_flight_plan.ml
@@ -324,7 +324,7 @@ let rec print_stage = fun index_of_waypoints x ->
     match String.lowercase (Xml.tag x) with
         "return" ->
           stage ();
-          lprintf "Return();\n";
+          lprintf "Return(%s);\n" (ExtXml.attrib_or_default x "reset_stage" "0");
           lprintf "break;\n"
       | "goto" ->
         stage ();