more readable start and stop functions

conf/autopilot/autopilot.dtd

@@ -5,8 +5,10 @@
 <!ELEMENT control_block (call*)>
 <!ELEMENT exceptions (exception*)>
 <!ELEMENT includes (include*,define*)>
-<!ELEMENT mode (select*,control*,exception*)>
+<!ELEMENT mode (select*,on_enter?,control*,exception*,on_exit?)>
 <!ELEMENT select EMPTY>
+<!ELEMENT on_enter (call)*>
+<!ELEMENT on_exit (call)*>
 <!ELEMENT control (call|call_block)*>
 <!ELEMENT exception EMPTY>
 <!ELEMENT include EMPTY>
@@ -37,8 +39,6 @@ name CDATA #REQUIRED>
 <!ATTLIST mode
 name CDATA #REQUIRED
 shortname CDATA #IMPLIED
-start CDATA #IMPLIED
-stop CDATA #IMPLIED
 gcs_name CDATA #IMPLIED
 settings CDATA #IMPLIED>
 
@@ -50,6 +50,9 @@ settings CDATA #IMPLIED>
 cond CDATA #REQUIRED
 exception CDATA #IMPLIED>
 
+<!ATTLIST on_enter>
+<!ATTLIST on_exit>
+  
 <!ATTLIST control
 freq CDATA #IMPLIED>
 

conf/autopilot/rotorcraft_autopilot.xml

@@ -55,8 +55,11 @@
       <exception cond="kill_switch_is_on()" deroute="KILL"/>
     </exceptions>
 
-    <mode name="ATTITUDE_DIRECT" start="stabilization_attitude_enter()" shortname="ATT">
+    <mode name="ATTITUDE_DIRECT" shortname="ATT">
       <select cond="RCMode0()"/>
+      <on_enter>
+        <call fun="stabilization_attitude_enter()"/>
+      </on_enter>
       <control freq="NAV_FREQ">
         <call fun="nav_periodic_task()"/>
       </control>
@@ -68,8 +71,12 @@
       <exception cond="RCLost()" deroute="FAILSAFE"/>
     </mode>
 
-    <mode name="ATTITUDE_Z_HOLD" start="stabilization_attitude_enter()|guidance_v_z_enter()" shortname="A_ZH">
+    <mode name="ATTITUDE_Z_HOLD" shortname="A_ZH">
       <select cond="RCMode1()"/>
+      <on_enter>
+        <call fun="stabilization_attitude_enter()"/>
+        <call fun="guidance_v_z_enter()"/>
+      </on_enter>
       <control freq="NAV_FREQ">
         <call fun="nav_periodic_task()"/>
       </control>
@@ -81,8 +88,13 @@
       <exception cond="RCLost()" deroute="FAILSAFE"/>
     </mode>
 
-    <mode name="NAV" start="guidance_h_nav_enter()|stabilization_attitude_enter()|guidance_v_z_enter()">
+    <mode name="NAV">
       <select cond="RCMode2() && DLModeNav()" exception="HOME"/>
+      <on_enter>
+        <call fun="guidance_h_nav_enter()"/>
+        <call fun="stabilization_attitude_enter()"/>
+        <call fun="guidance_v_z_enter()"/>
+      </on_enter>
       <control freq="NAV_FREQ">
         <call fun="nav_periodic_task()"/>
       </control>
@@ -95,8 +107,14 @@
       <exception cond="GpsIsLost() && autopilot_in_flight" deroute="FAILSAFE"/>
     </mode>
 
-    <mode name="GUIDED" start="guidance_h.mode=GUIDANCE_H_MODE_GUIDED|guidance_h_hover_enter()|stabilization_attitude_enter()|guidance_v_mode=GUIDANCE_V_MODE_GUIDED|guidance_v_guided_enter()">
+    <mode name="GUIDED">
       <select cond="RCMode2() && DLModeGuided()" exception="HOME"/>
+      <on_enter>
+        <call fun="guidance_h_hover_enter()"/>
+        <call fun="stabilization_attitude_enter()"/>
+        <call fun="guidance_v_mode = GUIDANCE_V_MODE_GUIDED"/>
+        <call fun="guidance_v_guided_enter()"/>
+      </on_enter>
       <control freq="NAV_FREQ">
         <call fun="nav_periodic_task()"/>
       </control>
@@ -109,7 +127,12 @@
       <exception cond="GpsIsLost() && autopilot_in_flight" deroute="FAILSAFE"/>
     </mode>
 
-    <mode name="HOME" start="guidance_h_nav_enter()|stabilization_attitude_enter()|guidance_v_z_enter()">
+    <mode name="HOME">
+      <on_enter>
+        <call fun="guidance_h_nav_enter()"/>
+        <call fun="stabilization_attitude_enter()"/>
+        <call fun="guidance_v_z_enter()"/>
+      </on_enter>
       <control freq="NAV_FREQ">
         <call fun="nav_home()"/>
       </control>
@@ -123,7 +146,12 @@
     </mode>
 
     <!-- Safe landing -->
-    <mode name="FAILSAFE" start="stabilization_attitude_set_failsafe_setpoint()|guidance_v_mode_changed(GUIDANCE_V_MODE_CLIMB)|guidance_v_zd_sp = SPEED_BFP_OF_REAL(FAILSAFE_DESCENT_SPEED)" shortname="FAIL">
+    <mode name="FAILSAFE" shortname="FAIL">
+      <on_enter>
+        <call fun="stabilization_attitude_set_failsafe_setpoint()"/>
+        <call fun="guidance_v_mode_changed(GUIDANCE_V_MODE_CLIMB)"/>
+        <call fun="guidance_v_zd_sp = SPEED_BFP_OF_REAL(FAILSAFE_DESCENT_SPEED)"/>
+      </on_enter>
       <control>
         <call fun="stabilization_attitude_set_failsafe_setpoint()"/>
         <call fun="stabilization_attitude_run(autopilot_in_flight)"/>
@@ -135,9 +163,15 @@
     </mode>
 
     <!-- Kill throttle mode -->
-    <mode name="KILL" start="autopilot_in_flight = false | autopilot_in_flight_counter = 0 | autopilot_set_motors_on(false) | stabilization_cmd[COMMAND_THRUST] = 0">
+    <mode name="KILL">
       <select cond="$DEFAULT_MODE"/>
       <select cond="kill_switch_is_on()"/>
+      <on_enter>
+        <call fun="autopilot_in_flight = false"/>
+        <call fun="autopilot_in_flight_counter = 0"/>
+        <call fun="autopilot_set_motors_on(false)"/>
+        <call fun="stabilization_cmd[COMMAND_THRUST] = 0"/>
+      </on_enter>
       <control>
         <call fun="SetCommands(commands_failsafe)"/>
       </control>

sw/tools/generators/gen_autopilot.ml

@@ -210,7 +210,7 @@ let print_set_mode = fun modes name out_h ->
   let print_case = fun mode f ->
     lprintf out_h "case %s :\n" (print_mode_name name (Xml.attrib mode "name"));
     right ();
-    List.iter (fun x -> lprintf out_h "%s;\n" x) (Str.split (Str.regexp "|") f);
+    List.iter (fun x -> lprintf out_h "%s;\n" (ExtXml.attrib x "fun")) (Xml.children f);
     lprintf out_h "break;\n";
     left ();
   in
@@ -220,7 +220,7 @@ let print_set_mode = fun modes name out_h ->
     right ();
     List.iter (fun m ->
       try
-        print_case m (Xml.attrib m t)
+        print_case m (ExtXml.child m t)
       with _ -> ()
     ) modes;
     lprintf out_h "default:\n";
@@ -235,10 +235,10 @@ let print_set_mode = fun modes name out_h ->
   right ();
   lprintf out_h "if (new_mode == private_autopilot_mode_%s) return;\n\n" name; (* set mode if different from current mode *)
   (* Print stop functions for each modes *)
-  print_switch ("private_autopilot_mode_"^name) modes "stop";
+  print_switch ("private_autopilot_mode_"^name) modes "on_exit";
   fprintf out_h "\n";
   (* Print start functions for each modes *)
-  print_switch "new_mode" modes "start";
+  print_switch "new_mode" modes "on_enter";
   fprintf out_h "\n";
   lprintf out_h "last_autopilot_mode_%s = private_autopilot_mode_%s;\n" name name;
   lprintf out_h "private_autopilot_mode_%s = new_mode;\n" name;