failsafe modes

Makefile.ac

@@ -70,7 +70,7 @@ $(CONTROL_C) : $(CONF)/$(AIRFRAME) $(CONF_XML)
 	$(Q)$(TOOLS)/gen_control.out c $< > /tmp/control.c
 	$(Q)mv /tmp/control.c $@
 
-$(FLIGHT_PLAN_H) : $(CONF)/$(FLIGHT_PLAN) $(CONF_XML)
+$(FLIGHT_PLAN_H) : $(CONF)/$(FLIGHT_PLAN) $(CONF_XML) $(TOOLS)/gen_flight_plan.out
 	@echo BUILD $@
 	$(Q)$(TOOLS)/gen_flight_plan.out $< > /tmp/fp.h
 	$(Q)mv /tmp/fp.h $@

conf/flight_plans/failsafe_modes.xml

@@ -0,0 +1,26 @@
+<!DOCTYPE procedure SYSTEM "procedure.dtd">
+
+<procedure>
+  <param name="kill_pitch"/>
+
+  <waypoints>
+    <waypoint NAME="BACK" X="0" Y="0"/>
+  </waypoints>
+
+  <exceptions>
+    <exception cond="And(block_time>20, dist2_to_home > 500*500)" deroute="home"/>
+    <exception cond="And(block_time>20, dist2_to_home > 600*600)" deroute="kill"/>
+    <exception cond="And(launch, gps_lost)" deroute="kill"/>
+  </exceptions>
+
+  <blocks>
+    <block name="home">
+      <circle wp="BACK" radius="50"/>
+    </block>
+
+    <block name="kill">
+      <go wp="BACK" vmode="gaz" gaz="0" pitch="kill_pitch"/>
+    </block>
+
+  </blocks>
+</procedure>

conf/flight_plans/flight_plan.dtd

@@ -15,7 +15,7 @@
 <!ELEMENT exceptions (exception*)>
 
 <!ELEMENT blocks (block+)>
-<!ELEMENT block (exception|while|heading|attitude|go|xyz|set|circle|deroute|stay|follow)*>
+<!ELEMENT block (exception|while|heading|attitude|go|xyz|set|circle|deroute|stay|follow|survey_rectangle)*>
 
 <!ELEMENT include (arg|with)*>
 <!ELEMENT arg EMPTY>
@@ -29,6 +29,7 @@
 <!ELEMENT xyz EMPTY>
 <!ELEMENT set EMPTY>
 <!ELEMENT circle EMPTY>
+<!ELEMENT survey_rectangle EMPTY>
 <!ELEMENT deroute EMPTY>
 <!ELEMENT stay EMPTY>
 <!ELEMENT follow EMPTY>
@@ -150,6 +151,12 @@ cam_ac_target CDATA #IMPLIED
 climb CDATA #IMPLIED
 until CDATA #IMPLIED>
 
+<!ATTLIST survey_rectangle
+grid CDATA #REQUIRED
+wp1 CDATA #REQUIRED
+wp2 CDATA #REQUIRED>
+
+
 <!ATTLIST stay
 wp CDATA #REQUIRED
 vmode CDATA #IMPLIED

conf/flight_plans/generic.xml

@@ -1,4 +1,6 @@
-<flight_plan SECURITY_HEIGHT="25" alt="450" ground_alt="400" lat0="43.4624" lon0="1.2727" max_dist_from_home="500" name="Generic" qfu="270">
+<!DOCTYPE flight_plan SYSTEM "flight_plan.dtd">
+
+<flight_plan SECURITY_HEIGHT="25" alt="450" ground_alt="400" lat0="43.4624" lon0="1.2727" max_dist_from_home="1000" name="Generic" qfu="270">
   <dl_settings>
     <dl_setting VAR="ir_roll_neutral" MAX="0.3" STEP="0.01" MIN="-0.3"/>
     <dl_setting VAR="ir_pitch_neutral" MAX="0.3" STEP="0.01" MIN="-0.3"/>
@@ -18,6 +20,7 @@
     <dl_setting MAX="2" MIN="0" STEP="1" VAR="pprz_mode"/>
     <dl_setting MAX="2" MIN="0" STEP="1" VAR="light_mode"/>
   </dl_settings>
+
   <waypoints>
     <waypoint NAME="HOME" X="0" Y="0"/>
     <waypoint NAME="1" X="200" Y="0"/>
@@ -25,6 +28,17 @@
     <waypoint alt="450." name="S1" x="-181.37499716" y="122.374998203"/>
     <waypoint alt="450." name="S2" x="136.75000196" y="308.874998301"/>
   </waypoints>
+
+  <include name="fs" procedure="failsafe_modes.xml">
+    <arg name="kill_pitch" value="-0.5"/>
+  </include>
+
+  <exceptions>
+    <exception cond="And(block_time>20, dist2_to_home > 500*500)" deroute="home"/>
+    <exception cond="And(block_time>20, dist2_to_home > 600*600)" deroute="kill"/>
+    <exception cond="And(launch, gps_lost)" deroute="kill"/>
+  </exceptions>
+
   <blocks>
     <block NAME="wait GPS">
       <while COND="!GPS_FIX_VALID(gps_mode)"/>
@@ -47,5 +61,14 @@
     <block NAME="survey">
       <survey_rectangle grid="70" wp1="S1" wp2="S2"/>
     </block>
+
+    <block name="home">
+      <circle wp="HOME" radius="50"/>
+    </block>
+
+    <block name="kill">
+      <go wp="HOME" vmode="gaz" gaz="0" pitch="-0.5"/>
+    </block>
+
   </blocks>
 </flight_plan>

conf/flight_plans/procedure.dtd

@@ -1,12 +1,14 @@
 <!-- Paparazzi procedure DTD -->
 
-<!ELEMENT procedure (param*,waypoints,blocks)>
+<!ELEMENT procedure (param*,waypoints,exceptions?,blocks)>
 
 <!ELEMENT param EMPTY>
 
-<!ELEMENT waypoints (waypoint+)>
+<!ELEMENT waypoints (waypoint*)>
 <!ELEMENT waypoint EMPTY>
 
+<!ELEMENT exceptions (exception+)>
+
 <!ELEMENT blocks (block+)>
 <!ELEMENT block (exception|while|heading|go|xyz|circle|deroute|stay)*>
 
@@ -52,6 +54,7 @@ from_qdr CDATA #IMPLIED
 from_dist CDATA #IMPLIED
 hmode CDATA #IMPLIED
 vmode CDATA #IMPLIED
+pitch CDATA #IMPLIED
 alt CDATA #IMPLIED
 until CDATA #IMPLIED
 approaching_time CDATA #IMPLIED

sw/airborne/autopilot.h

@@ -98,6 +98,7 @@ extern float slider_1_val, slider_2_val;
 extern bool_t launch;
 
 extern uint8_t light_mode;
+extern bool_t gps_lost;
 
 /** Assignment, returning _old_value != _value
  * Using GCC expression statements */

sw/airborne/main_ap.c

@@ -97,6 +97,8 @@ bool_t launch = FALSE;
 
 float energy; /** Fuel consumption */
 
+bool_t gps_lost = FALSE;
+
 
 #define LIGHT_MODE_OFF 0
 #define LIGHT_MODE_ON 1
@@ -278,7 +280,6 @@ static void navigation_task( void ) {
 #if defined FAILSAFE_DELAY_WITHOUT_GPS
   /** This section is used for the failsafe of GPS */
   static uint8_t last_pprz_mode;
-  static bool_t has_lost_gps = FALSE;
   /** Test if we lost the GPS */
   if (!GPS_FIX_VALID(gps_mode) ||
       (cpu_time - last_gps_msg_t > FAILSAFE_DELAY_WITHOUT_GPS)) {
@@ -289,13 +290,13 @@ static void navigation_task( void ) {
       last_pprz_mode = pprz_mode;
       pprz_mode = PPRZ_MODE_GPS_OUT_OF_ORDER;
       PERIODIC_SEND_PPRZ_MODE();
-      has_lost_gps = TRUE;
+      gps_lost = TRUE;
     }
   }
   /** If aircraft was in failsafe mode, come back in previous mode */
-  else if (has_lost_gps) {
+  else if (gps_lost) {
     pprz_mode = last_pprz_mode;
-    has_lost_gps = FALSE;
+    gps_lost = FALSE;
     PERIODIC_SEND_PPRZ_MODE();
   }
 #endif /* GPS && FAILSAFE_DELAY_WITHOUT_GPS */

sw/tools/fp_proc.ml

@@ -28,6 +28,11 @@ module G2D = Geometry_2d
 
 open Fp_syntax
 
+let rec list_split3 = function
+    [] -> ([], [], [])
+  | (x,y,z)::l ->
+      let (rx, ry, rz) = list_split3 l in (x::rx, y::ry, z::rz)
+
 let nop_stage = Xml.Element ("while", ["cond","FALSE"],[])
 
 
@@ -211,10 +216,11 @@ let parse_include = fun dir include_xml ->
   let f = Filename.concat dir (ExtXml.attrib include_xml "procedure") in
   let proc_name = ExtXml.attrib include_xml "name" in
   let prefix = fun x -> proc_name ^ "." ^ x in
+  let a = fun a b -> float_of_string (ExtXml.attrib_or_default include_xml a b) in
   let affine = { 
-    dx = ExtXml.float_attrib  include_xml "x";
+    dx = a "x" "0";
-    dy = ExtXml.float_attrib  include_xml "y";
+    dy = a "y" "0";
-    angle = ExtXml.float_attrib  include_xml "rotate"
+    angle = a "rotate" "0"
   } in
   let reroutes = 
     List.filter 
@@ -249,11 +255,12 @@ let parse_include = fun dir include_xml ->
     let env =  List.map (fun xml -> value xml env) params in
 
     let waypoints = Xml.children (ExtXml.child proc "waypoints")
+    and exceptions = Xml.children (ExtXml.child proc "exceptions")
     and blocks = Xml.children (ExtXml.child proc "blocks") in
 
     let waypoints = List.map (transform_waypoint prefix affine) waypoints in
     let blocks = List.map (transform_block prefix reroutes affine env) blocks in
-    (waypoints, blocks)
+    (waypoints, exceptions, blocks)
   with
     Dtd.Prove_error e -> dtd_error f (Dtd.prove_error e)
   | Dtd.Check_error e -> dtd_error f (Dtd.check_error e)
@@ -289,15 +296,18 @@ let process_includes = fun dir xml ->
   let includes, children =
    List.partition (fun x -> Xml.tag x = "include") (Xml.children xml) in
 
-  (* List of pairs of list (waypoints, blocks) *)
+  (* List of triples of lists (waypoints, exceptions, blocks) *)
   let waypoints_and_blocks = List.map (parse_include dir) includes in
 
-  let (inc_waypoints, inc_blocks) = List.split waypoints_and_blocks in
+  let (inc_waypoints, inc_exceptions, inc_blocks) = list_split3 waypoints_and_blocks in
   let inc_waypoints = List.flatten inc_waypoints
+  and inc_exceptions = List.flatten inc_exceptions
   and inc_blocks = List.flatten inc_blocks in
 
   let new_children = insert_children children
-      ["waypoints", inc_waypoints; "blocks", inc_blocks] in
+      ["waypoints", inc_waypoints; 
+       "exceptions", inc_exceptions; 
+       "blocks", inc_blocks] in
 
   Xml.Element (Xml.tag xml, Xml.attribs xml, new_children)
 

sw/tools/fp_syntax.ml

@@ -62,7 +62,7 @@ let variables = [
   "FALSE";
   "QFU";
   "gps_mode"; "gps_utm_east"; "gps_utm_north"; "gps_utm_zone";
-  "nav_utm_east0"; "nav_utm_north0"; "nav_utm_zone0"
+  "nav_utm_east0"; "nav_utm_north0"; "nav_utm_zone0"; "climb_level_gaz"; "gps_lost"
 
 ]
 

sw/tools/gen_flight_plan.ml

@@ -778,4 +778,6 @@ let _ =
   with
     Xml.Error e -> prerr_endline (Xml.error e); exit 1
   | Dtd.Prove_error e ->  prerr_endline (Dtd.prove_error e); exit 1
+  | Dtd.Check_error e ->  prerr_endline (Dtd.check_error e); exit 1
+  | Dtd.Parse_error e ->  prerr_endline (Dtd.parse_error e); exit 1