[flight plan] add on_enter and on_exit functions for nav block (#3385)

The functions passed to the attributes 'on_enter' and 'on_exit' will be
called once every time the flight plan state machine enters or leaves a
block, even if the block change is requested by the operator or if it is
caused by an exception.
It allows to properly activate and then disable services related to a
specific block.
Another good side effect is that the block and stage index are now
private and can't be modified by external modules. Old variables
nav_block and nav_stage can still be used as read-only.

Based on the work from:
Baptiste Pollien, Christophe Garion, Gautier Hattenberger, Pierre Roux, Xavier Thirioux.
A Verified UAV Flight Plan Generator. 2023 IEEE/ACM 11th International Conference on Formal Methods in Software Engineering (FormaliSE),
May 2023, Melbourne, Australia. pp.130-140, 10.1109/FormaliSE58978.2023.00021

sw/airborne/firmwares/fixedwing/nav.c

@@ -531,8 +531,7 @@ static void send_survey(struct transport_tx *trans, struct link_device *dev)
  */
 void nav_init(void)
 {
-  nav_block = 0;
-  nav_stage = 0;
+  common_flight_plan_init();
   ground_alt = GROUND_ALT;
   nav_glide_pitch_trim = NAV_GLIDE_PITCH_TRIM;
   nav_radius = DEFAULT_CIRCLE_RADIUS;

sw/airborne/firmwares/rotorcraft/navigation.c

@@ -70,9 +70,7 @@ static inline void nav_set_altitude(void);
 void nav_init(void)
 {
   waypoints_init();
-
-  nav_block = 0;
-  nav_stage = 0;
+  common_flight_plan_init();
 
   nav.horizontal_mode = NAV_HORIZONTAL_MODE_WAYPOINT;
   nav.vertical_mode = NAV_VERTICAL_MODE_ALT;

sw/airborne/firmwares/rover/navigation.c

@@ -79,9 +79,7 @@ static void send_wp_moved(struct transport_tx *trans, struct link_device *dev)
 void nav_init(void)
 {
   waypoints_init();
-
-  nav_block = 0;
-  nav_stage = 0;
+  common_flight_plan_init();
 
   nav.mode = NAV_MODE_WAYPOINT;
 

sw/airborne/modules/com/generic_com.c

@@ -85,7 +85,7 @@ void generic_com_periodic(void)
   com_trans.buf[18] = charge;
   com_trans.buf[19] = (uint8_t)(command_get(COMMAND_THROTTLE) * 100 / MAX_PPRZ);
   com_trans.buf[20] = autopilot_get_mode();
-  com_trans.buf[21] = nav_block;
+  com_trans.buf[21] = get_nav_block();
   FillBufWith16bit(com_trans.buf, 22, autopilot.flight_time);
   i2c_transmit(&GENERIC_COM_I2C_DEV, &com_trans, GENERIC_COM_SLAVE_ADDR, NB_DATA);
 }

sw/airborne/modules/datalink/missionlib/mission_manager.c

@@ -93,9 +93,9 @@ void mavlink_mission_message_handler(const mavlink_message_t *msg)
 void mavlink_mission_periodic(void)
 {
   // FIXME: really use the SCRIPT_ITEM message to indicate current block?
-  if (mission_mgr.current_block != nav_block) {
-    mission_mgr.current_block = nav_block;
-    mavlink_msg_script_current_send(MAVLINK_COMM_0, nav_block);
+  if (mission_mgr.current_block != get_nav_block()) {
+    mission_mgr.current_block = get_nav_block();
+    mavlink_msg_script_current_send(MAVLINK_COMM_0, get_nav_block());
     MAVLinkSendMessage();
   }
   // check if we had a timeout on a transaction

sw/airborne/modules/nav/common_flight_plan.c

@@ -37,24 +37,104 @@ uint8_t nav_stage, nav_block;
 uint8_t last_block, last_stage;
 uint8_t last_wp UNUSED;
 
+static uint8_t private_nav_stage, private_nav_block;
+static uint8_t private_last_block, private_last_stage;
+
+void common_flight_plan_init(void) {
+  private_nav_stage = 0;
+  nav_stage = 0;
+  private_nav_block = 0;
+  nav_block = 0;
+  private_last_stage = 0;
+  last_stage = 0;
+  private_last_block = 0;
+  last_block = 0;
+  stage_time = 0;
+  block_time = 0;
+}
 
 void nav_init_block(void)
 {
-  if (nav_block >= NB_BLOCK) {
-    nav_block = NB_BLOCK - 1;
+  if (private_nav_block >= NB_BLOCK) {
+    private_nav_block = NB_BLOCK - 1;
+    nav_block = private_nav_block;
   }
-  nav_stage = 0;
+  private_nav_stage = 0;
+  nav_stage = private_nav_stage;
   block_time = 0;
   InitStage();
+  nav_on_enter_block(private_nav_block); // generated by flight plan
 }
 
 void nav_goto_block(uint8_t b)
 {
-  if (b != nav_block) { /* To avoid a loop in a the current block */
-    last_block = nav_block;
-    last_stage = nav_stage;
+  if (b != private_nav_block) {
+    /* To avoid a loop in a the current block */
+    private_last_block = private_nav_block;
+    last_block = private_last_block;
+    private_last_stage = private_nav_stage;
+    last_stage = private_last_stage;
   }
-  nav_block = b;
+  nav_on_exit_block(private_nav_block); // generated by flight plan
+  private_nav_block = b;
+  nav_block = private_nav_block;
   nav_init_block();
 }
 
+void nav_goto_next_block(void) {
+  if (private_nav_block < NB_BLOCK - 1) {
+    nav_goto_block(private_nav_block + 1);
+  } else {
+    nav_goto_block(private_nav_block);
+  }
+}
+
+void nav_goto_next_stage(void) {
+  private_nav_stage++;
+  nav_stage = private_nav_stage;
+  InitStage();
+}
+
+void nav_return(uint8_t reset) {
+  nav_on_exit_block(private_nav_block); // generated by flight plan
+  private_nav_block = private_last_block;
+  nav_block = private_nav_block;
+  if (reset == 1) {
+    private_nav_stage = 0;
+  } else {
+    private_nav_stage = private_last_stage;
+  }
+  nav_stage = private_nav_stage;
+  block_time = 0;
+  InitStage();
+  nav_on_enter_block(private_nav_block); // generated by flight plan
+}
+
+uint8_t get_nav_block() {
+  return private_nav_block;
+}
+uint8_t get_nav_stage() {
+  return private_nav_stage;
+}
+uint8_t get_last_block() {
+  return private_last_block;
+}
+uint8_t get_last_stage() {
+  return private_last_stage;
+}
+
+void set_nav_block(uint8_t b){
+  if (b >= NB_BLOCK) {
+    private_nav_block = NB_BLOCK - 1;
+  }
+  else {
+    private_nav_block = b;
+  }
+  nav_block = private_nav_block;
+}
+
+void set_nav_stage(uint8_t s) {
+  private_nav_stage = s;
+  nav_stage = s;
+}
+

sw/airborne/modules/nav/common_flight_plan.h

@@ -32,30 +32,53 @@
 /** In s */
 extern uint16_t stage_time, block_time;
 
+/* The modification of the following variables do not affect
+ * the behavior of the flight plan
+ */
 extern uint8_t nav_stage, nav_block;
 extern uint8_t last_block, last_stage;
 extern uint8_t last_wp __attribute__((unused));
 
-/** needs to be implemented by fixedwing and rotorcraft seperately */
-void nav_init_stage(void);
+/* init function */
+extern void common_flight_plan_init(void);
 
-void nav_init_block(void);
-void nav_goto_block(uint8_t block_id);
+/** needs to be implemented by fixedwing and rotorcraft seperately */
+extern void nav_init_stage(void);
+
+extern void nav_init_block(void);
+extern void nav_goto_block(uint8_t block_id);
+extern void nav_goto_next_block(void);
+extern void nav_goto_next_stage(void);
+extern void nav_return(uint8_t reset);
+
+/* Getter */
+extern uint8_t get_nav_block();
+extern uint8_t get_nav_stage();
+extern uint8_t get_last_block();
+extern uint8_t get_last_stage();
+
+/* Setter */
+extern void set_nav_block(uint8_t block_id);
+extern void set_nav_stage(uint8_t stage_id);
+
+/** Functions generated by the flight plan */
+extern void nav_on_enter_block(uint8_t block_id);
+extern void nav_on_exit_block(uint8_t block_id);
 
 #define InitStage() nav_init_stage();
 
-#define Block(x) case x: nav_block=x;
-#define NextBlock() nav_goto_block(nav_block + 1)
+#define Block(x) case x: set_nav_block(x);
+#define NextBlock() nav_goto_next_block()
 #define GotoBlock(b) nav_goto_block(b)
 
-#define Stage(s) case s: nav_stage=s;
-#define NextStage() { nav_stage++; InitStage(); } INTENTIONAL_FALLTHRU
-#define NextStageAndBreak() { nav_stage++; InitStage(); break; }
+#define Stage(s) case s: set_nav_stage(s);
+#define NextStage() { nav_goto_next_stage(); } INTENTIONAL_FALLTHRU
+#define NextStageAndBreak() { nav_goto_next_stage(); break; }
 #define NextStageAndBreakFrom(wp) { last_wp = wp; NextStageAndBreak(); }
 
 #define Label(x) label_ ## x:
 #define Goto(x) { goto label_ ## x; }
-#define Return(x) { nav_block=last_block; if (x==1) {nav_stage=0;} else {nav_stage=last_stage;} block_time=0;}
+#define Return(x) nav_return(x)
 
 /** Time in s since the entrance in the current block */
 #define NavBlockTime() (block_time)
@@ -64,3 +87,4 @@ void nav_goto_block(uint8_t block_id);
 #define Ptr(x) (&(x))
 
 #endif /* COMMON_FLIGHT_PLAN_H */
+

sw/tools/generators/gen_flight_plan.ml

@@ -918,6 +918,34 @@ let print_auto_init_bindings = fun out abi_msgs variables ->
   List.iter print_bindings variables;
   lprintf out "}\n\n"
 
+let print_enter_exit_functions = fun out blocks ->
+  let print_functions = fun name ->
+    lprintf out "void nav_%s_block(uint8_t block_id) {\n" name;
+    right ();
+    lprintf out "switch (block_id) {\n";
+    right ();
+    let block_id = ref (-1) in
+    List.iter (fun b ->
+      incr block_id;
+      try
+        let f_name = ExtXml.attrib b name in
+        lprintf out "case %d: // %s\n" !block_id (name_of b);
+        lprintf out "  %s;\n" f_name;
+        lprintf out "  break;\n"
+      with _ -> ()
+    ) blocks;
+    lprintf out "default:\n";
+    lprintf out "  break;\n";
+    left ();
+    lprintf out "}\n";
+    left ();
+    lprintf out "}\n\n"
+  in
+  (* print enter block functions *)
+  print_functions "on_enter";
+  (* print exit block functions *)
+  print_functions "on_exit"
+
 (**
  * Print flight plan header
  *)
@@ -1114,6 +1142,10 @@ let print_flight_plan_h = fun xml ref0 xml_file out_file ->
   lprintf out "\n";
   print_auto_init_bindings out abi_msgs variables;
 
+  (* print on_enter and on_exit functions *)
+  lprintf out "\n";
+  print_enter_exit_functions out blocks;
+
   (* print main flight plan state machine *)
   lprintf out "static inline void auto_nav(void) {\n";
   right ();