From 387f51c1dedbb8866a3d89a649beb0464d3988b8 Mon Sep 17 00:00:00 2001
From: Ewoud Smeur <ewoud_smeur@msn.com>
Date: Sun, 13 Sep 2015 12:10:40 +0200
Subject: [PATCH] fixed exception flags

---
 conf/airframes/BR/conf.xml                            |  2 +-
 .../Tudelft/rotorcraft_survey_competition.xml         |  2 +-
 .../Tudelft/rotorcraft_survey_mission.xml             |  4 ++--
 sw/airborne/modules/exception_flags/exception_flags.c |  4 ++++
 sw/airborne/modules/exception_flags/exception_flags.h |  1 +
 sw/tools/generators/gen_flight_plan.ml                | 11 ++++++-----
 6 files changed, 15 insertions(+), 9 deletions(-)

diff --git a/conf/airframes/BR/conf.xml b/conf/airframes/BR/conf.xml
index 26d392aca05..4d933a31bb0 100644
--- a/conf/airframes/BR/conf.xml
+++ b/conf/airframes/BR/conf.xml
@@ -16,7 +16,7 @@
    airframe="airframes/BR/bebop_indi_frog.xml"
    radio="radios/dummy.xml"
    telemetry="telemetry/default_rotorcraft.xml"
-   flight_plan="flight_plans/Tudelft/rotorcraft_survey_delft.xml"
+   flight_plan="flight_plans/Tudelft/rotorcraft_survey_competition.xml"
    settings="settings/rotorcraft_basic.xml settings/control/rotorcraft_guidance.xml settings/estimation/ahrs_secondary.xml settings/estimation/ahrs_float_mlkf.xml settings/estimation/ahrs_int_cmpl_quat.xml settings/control/stabilization_att_indi.xml"
    settings_modules="modules/geo_mag.xml modules/air_data.xml modules/video_thread.xml modules/video_rtp_stream.xml modules/nav_survey_rectangle_rotorcraft.xml modules/digital_cam_video.xml modules/cv_colorfilter.xml"
    gui_color="#ffff0689b7a1"
diff --git a/conf/flight_plans/Tudelft/rotorcraft_survey_competition.xml b/conf/flight_plans/Tudelft/rotorcraft_survey_competition.xml
index 90863371692..7d49f6d07b4 100644
--- a/conf/flight_plans/Tudelft/rotorcraft_survey_competition.xml
+++ b/conf/flight_plans/Tudelft/rotorcraft_survey_competition.xml
@@ -1,6 +1,6 @@
 <!DOCTYPE flight_plan SYSTEM "../flight_plan.dtd">
 
-<flight_plan alt="30" ground_alt="0" lat0="50.789517" lon0="6.047089" max_dist_from_home="150" name="IMAV2015_competition_outdoor" security_height="2">
+<flight_plan alt="30" ground_alt="0" lat0="50.789517" lon0="6.047089" max_dist_from_home="400" name="IMAV2015_competition_outdoor" security_height="2">
 
   <waypoints>
     <waypoint name="HOME" x="0.0" y="0.0"/>
diff --git a/conf/flight_plans/Tudelft/rotorcraft_survey_mission.xml b/conf/flight_plans/Tudelft/rotorcraft_survey_mission.xml
index f69eca91fb5..a68d1bdda76 100644
--- a/conf/flight_plans/Tudelft/rotorcraft_survey_mission.xml
+++ b/conf/flight_plans/Tudelft/rotorcraft_survey_mission.xml
@@ -10,8 +10,8 @@
   </header>
 
   <exceptions>
-      <exception cond="electrical.bat_low" deroute="Standby"/>
-      <exception cond="electrical.bat_critical" deroute="land_here"/>
+      <exception cond="electrical.bat_low && (exception_flag[0] == 0)" deroute="Standby" exec="check_exception_flag(0)"/>
+      <exception cond="electrical.bat_critical && (exception_flag[1] == 0)" deroute="land_here" exec="check_exception_flag(1)"/>
   </exceptions>
 
   <blocks>
diff --git a/sw/airborne/modules/exception_flags/exception_flags.c b/sw/airborne/modules/exception_flags/exception_flags.c
index ba83b7ff585..6830bb9a402 100644
--- a/sw/airborne/modules/exception_flags/exception_flags.c
+++ b/sw/airborne/modules/exception_flags/exception_flags.c
@@ -32,4 +32,8 @@ bool_t exception_flag[10];
 
 void exception_flags_init(void) {
   memset(exception_flag,0,sizeof(exception_flag));
+}
+
+void check_exception_flag(uint8_t flag_num) {
+  exception_flag[flag_num] = 1;
 }
\ No newline at end of file
diff --git a/sw/airborne/modules/exception_flags/exception_flags.h b/sw/airborne/modules/exception_flags/exception_flags.h
index 471aed28a73..1e62abd8d4c 100644
--- a/sw/airborne/modules/exception_flags/exception_flags.h
+++ b/sw/airborne/modules/exception_flags/exception_flags.h
@@ -31,6 +31,7 @@
 #define EXCEPTION_FLAGS_H_
 
 extern void exception_flags_init(void);
+extern void check_exception_flag(uint8_t flag_num);
 
 extern bool_t exception_flag[10];
 
diff --git a/sw/tools/generators/gen_flight_plan.ml b/sw/tools/generators/gen_flight_plan.ml
index 699eece507a..f98ae09e0f0 100644
--- a/sw/tools/generators/gen_flight_plan.ml
+++ b/sw/tools/generators/gen_flight_plan.ml
@@ -158,15 +158,16 @@ let get_index_block = fun x ->
 
 let print_exception = fun x ->
   let c = parsed_attrib x "cond" in
+  let i = get_index_block (ExtXml.attrib x "deroute") in
   begin
   try
     let f =  ExtXml.attrib x "exec" in
-    lprintf "if (%s) { %s; }\n" c f
+    lprintf "if ((nav_block != %d) && %s) {%s; GotoBlock(%d); return; }\n" i c f i
   with
-    ExtXml.Error _ -> ()
-  end;
-  let i = get_index_block (ExtXml.attrib x "deroute") in
-  lprintf "if ((nav_block != %d) && %s) { GotoBlock(%d); return; }\n" i c i
+    ExtXml.Error _ -> (
+     lprintf "if ((nav_block != %d) && %s) { GotoBlock(%d); return; }\n" i c i
+    )
+  end
 
 
 let element = fun a b c -> Xml.Element (a, b, c)