fixed exception flags

paparazzi · Sep 13, 2015 · 387f51c · 387f51c
1 parent 944ed27
commit 387f51c
Show file tree

Hide file tree

Showing 6 changed files with 15 additions and 9 deletions.
diff --git 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
@@ -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
@@ -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
@@ -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;
 }
diff --git 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
@@ -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)