[paparazzi-commits] [4180] switch to failsafe if GPS is lost in nav mode

[Pascal Brisset]

Revision: 4180
          http://svn.sv.gnu.org/viewvc/?view=rev&root=paparazzi&revision=4180
Author:   hecto
Date:     2009-09-18 14:19:09 +0000 (Fri, 18 Sep 2009)
Log Message:
-----------
 switch to failsafe if GPS is lost in nav mode
 update datalink_time
 

Modified Paths:
--------------
    paparazzi3/trunk/sw/airborne/booz/booz2_main.c

Modified: paparazzi3/trunk/sw/airborne/booz/booz2_main.c
===================================================================
--- paparazzi3/trunk/sw/airborne/booz/booz2_main.c      2009-09-18 14:15:23 UTC 
(rev 4179)
+++ paparazzi3/trunk/sw/airborne/booz/booz2_main.c      2009-09-18 14:19:09 UTC 
(rev 4180)
@@ -184,7 +184,9 @@
     {                                                                  \
       /*BoozControlSurfacesSetFromCommands();*/                                
\
     },                                                                 \
-    {},                                 \
+    { if (radio_control.status != RADIO_CONTROL_OK && booz2_autopilot_mode == 
BOOZ2_AP_MODE_NAV && GpsIsLost())    \
+       booz2_autopilot_set_mode(BOOZ2_AP_MODE_FAILSAFE);               \
+    },                                                                  \
     {},                                                                        
\
     {},                                                                        
\
     {},                                                                        
\
@@ -192,6 +194,8 @@
     {}                                                                 \
     );                                                                 \
 
+  booz_gps_periodic();
+
 #ifdef USE_CAM
   RunOnceEvery(50,booz2_cam_periodic());
 #endif
@@ -205,7 +209,7 @@
 #endif
 
   if (booz2_autopilot_in_flight) {
-    RunOnceEvery(512, { booz2_autopilot_flight_time++; });
+    RunOnceEvery(512, { booz2_autopilot_flight_time++; datalink_time++; });
   }
 
   //  t1 = T0TC;