GPS loss is handled by the flight plan

- removed going to failsafe from main.c - added exception in flight plan that will go into attitude mode for 10 seconds when gps is lost, to allow time for rc take-over - longer gps timeout 2 -> 5
paparazzi · Sep 6, 2016 · d6b050e · d6b050e
1 parent 89ec272
commit d6b050e
Show file tree

Hide file tree

Showing 5 changed files with 24 additions and 10 deletions.
diff --git a/conf/airframes/TUDELFT/tudelft_outback.xml b/conf/airframes/TUDELFT/tudelft_outback.xml
@@ -21,7 +21,9 @@
 
       <module name="telemetry" type="xbee_api"/>
       <module name="imu" type="mpu6000"/>
-      <module name="gps" type="ublox"/>
+      <module name="gps" type="ublox">
+        <define name="GPS_TIMEOUT" value="5"/>
+      </module>
       <module name="stabilization" type="int_quat"/>
       <module name="stabilization" type="rate"/>
       <module name="ahrs" type="int_cmpl_quat">

diff --git a/conf/flight_plans/TUDELFT/tudelft_kalmthout_outback.xml b/conf/flight_plans/TUDELFT/tudelft_kalmthout_outback.xml
@@ -34,6 +34,7 @@
   </modules>
   <exceptions>
     <exception cond="(!InsideSafety(GetPosX(), GetPosY()) && !(nav_block == IndexOfBlock('Wait GPS')) && !(nav_block == IndexOfBlock('Geo init')) )" deroute="Holding point"/>
+    <exception cond="(!GpsFixValid() && !(nav_block == IndexOfBlock('Hold Attitude')) && !(nav_block == IndexOfBlock('Takeoff')) && (exception_flag[0] == 0) && (autopilot_in_flight == true) )" deroute="gps loss"/>
   </exceptions>
   <blocks>
     <block name="Wait GPS">
@@ -126,5 +127,16 @@
     <block name="landed">
       <attitude pitch="0" roll="0" throttle="0" until="FALSE" vmode="throttle"/>
     </block>
+    <block name="gps loss">
+      <exception cond="stateGetPositionEnu_f()->z > 2.0" deroute="landed"/>
+      <exception cond="GpsFixValid()" deroute="Hover here"/>
+      <set var="exception_flag[0]" value="1"/>
+      <call fun="nav_throttle_curve_set(1)" loop="false"/>
+      <attitude pitch="0" roll="0" throttle="0.55" until="stage_time>20" vmode="throttle"/>
+    </block>
+    <block name="failsafe">
+      <call fun="NavSetFailsafe"/>
+      <attitude pitch="0" roll="0" throttle="0" until="FALSE" vmode="throttle"/>
+    </block>
   </blocks>
 </flight_plan>
diff --git a/sw/airborne/firmwares/rotorcraft/main.c b/sw/airborne/firmwares/rotorcraft/main.c
@@ -331,15 +331,6 @@ STATIC_INLINE void failsafe_check(void)
 #endif
 
 #if USE_GPS
-  if (autopilot_mode == AP_MODE_NAV &&
-      autopilot_motors_on &&
-#if NO_GPS_LOST_WITH_RC_VALID
-      radio_control.status != RC_OK &&
-#endif
-      GpsIsLost()) {
-    autopilot_set_mode(AP_MODE_FAILSAFE);
-  }
-
   if (autopilot_mode == AP_MODE_HOME &&
       autopilot_motors_on && GpsIsLost()) {
     autopilot_set_mode(AP_MODE_FAILSAFE);

diff --git a/sw/airborne/firmwares/rotorcraft/navigation.c b/sw/airborne/firmwares/rotorcraft/navigation.c
@@ -577,6 +577,11 @@ bool nav_set_heading_current(void)
   return false;
 }
 
+bool nav_set_failsafe(void) {
+  autopilot_set_mode(AP_MODE_FAILSAFE);
+  return false;
+}
+
 /************** Oval Navigation **********************************************/
 
 /** Navigation along a figure O. One side leg is defined by waypoints [p1] and

diff --git a/sw/airborne/firmwares/rotorcraft/navigation.h b/sw/airborne/firmwares/rotorcraft/navigation.h
@@ -101,6 +101,7 @@ extern bool nav_set_heading_towards(float x, float y);
 extern bool nav_set_heading_towards_waypoint(uint8_t wp);
 extern bool nav_set_heading_towards_target(void);
 extern bool nav_set_heading_current(void);
+extern bool nav_set_failsafe(void);
 
 /** default approaching_time for a wp */
 #ifndef CARROT
@@ -223,6 +224,9 @@ bool nav_check_wp_time(struct EnuCoor_i *wp, uint16_t stay_time);
   }
 
 #define NavSetManual nav_set_manual
+#define NavSetFailsafe { \
+  nav_set_failsafe(); \
+}
 
 #define NavStartDetectGround() ({ autopilot_detect_ground_once = true; false; })
 #define NavDetectGround() nav_detect_ground()