diff --git a/Makefile.ac b/Makefile.ac
index 7d399b9d96..91f5385c2f 100644
--- a/Makefile.ac
+++ b/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 $@
diff --git a/conf/flight_plans/failsafe_modes.xml b/conf/flight_plans/failsafe_modes.xml
new file mode 100644
index 0000000000..786d11d445
--- /dev/null
+++ b/conf/flight_plans/failsafe_modes.xml
@@ -0,0 +1,26 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/conf/flight_plans/flight_plan.dtd b/conf/flight_plans/flight_plan.dtd
index 6a64115465..d37e381805 100644
--- a/conf/flight_plans/flight_plan.dtd
+++ b/conf/flight_plans/flight_plan.dtd
@@ -15,7 +15,7 @@
-
+
@@ -29,6 +29,7 @@
+
@@ -150,6 +151,12 @@ cam_ac_target CDATA #IMPLIED
climb CDATA #IMPLIED
until CDATA #IMPLIED>
+
+
+
+
+
+
@@ -18,6 +20,7 @@
+
@@ -25,6 +28,17 @@
+
+
+
+
+
+
+
+
+
+
+
@@ -47,5 +61,14 @@
+
+
+
+
+
+
+
+
+
diff --git a/conf/flight_plans/procedure.dtd b/conf/flight_plans/procedure.dtd
index bf920034af..67b675efc1 100644
--- a/conf/flight_plans/procedure.dtd
+++ b/conf/flight_plans/procedure.dtd
@@ -1,12 +1,14 @@
-
+
-
+
+
+
@@ -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
diff --git a/sw/airborne/autopilot.h b/sw/airborne/autopilot.h
index f2264d3731..dec24926a6 100644
--- a/sw/airborne/autopilot.h
+++ b/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 */
diff --git a/sw/airborne/main_ap.c b/sw/airborne/main_ap.c
index a69d284ed4..6a1b243bc4 100644
--- a/sw/airborne/main_ap.c
+++ b/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 */
diff --git a/sw/tools/fp_proc.ml b/sw/tools/fp_proc.ml
index c5f221e773..0195dda000 100644
--- a/sw/tools/fp_proc.ml
+++ b/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";
- dy = ExtXml.float_attrib include_xml "y";
- angle = ExtXml.float_attrib include_xml "rotate"
+ dx = a "x" "0";
+ dy = a "y" "0";
+ 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)
diff --git a/sw/tools/fp_syntax.ml b/sw/tools/fp_syntax.ml
index 2695df8e25..cf74fcad94 100644
--- a/sw/tools/fp_syntax.ml
+++ b/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"
]
diff --git a/sw/tools/gen_flight_plan.ml b/sw/tools/gen_flight_plan.ml
index 987576ec03..b02496f97d 100644
--- a/sw/tools/gen_flight_plan.ml
+++ b/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