[paparazzi-commits] [5852] digital_cam module and xsens-fixedwing flying

[Christophe De Wagter]

Revision: 5852
          http://svn.sv.gnu.org/viewvc/?view=rev&root=paparazzi&revision=5852
Author:   dewagter
Date:     2010-09-13 20:46:47 +0000 (Mon, 13 Sep 2010)
Log Message:
-----------
digital_cam module and xsens-fixedwing flying

Modified Paths:
--------------
    paparazzi3/trunk/conf/boards/classix.makefile
    paparazzi3/trunk/conf/modules/ins_xsens_MTiG_fixedwing.xml
    paparazzi3/trunk/sw/airborne/OSAMNav.c
    paparazzi3/trunk/sw/airborne/gps.h
    paparazzi3/trunk/sw/airborne/main_ap.c
    paparazzi3/trunk/sw/airborne/modules/ins/ins_xsens.c

Added Paths:
-----------
    paparazzi3/trunk/conf/modules/digital_cam.xml
    paparazzi3/trunk/sw/airborne/modules/digital_cam/
    paparazzi3/trunk/sw/airborne/modules/digital_cam/dc.c
    paparazzi3/trunk/sw/airborne/modules/digital_cam/dc.h

Removed Paths:
-------------
    paparazzi3/trunk/sw/airborne/dc.c
    paparazzi3/trunk/sw/airborne/dc.h

Modified: paparazzi3/trunk/conf/boards/classix.makefile
===================================================================
--- paparazzi3/trunk/conf/boards/classix.makefile       2010-09-11 13:55:20 UTC 
(rev 5851)
+++ paparazzi3/trunk/conf/boards/classix.makefile       2010-09-13 20:46:47 UTC 
(rev 5852)
@@ -5,7 +5,7 @@
 #
 # TODO: move all to new directories
 # ARCH=lpc21
-ARCH=arm7
+ARCH=arm7tdmi
 ARCHI=arm7
 
 

Added: paparazzi3/trunk/conf/modules/digital_cam.xml
===================================================================
--- paparazzi3/trunk/conf/modules/digital_cam.xml                               
(rev 0)
+++ paparazzi3/trunk/conf/modules/digital_cam.xml       2010-09-13 20:46:47 UTC 
(rev 5852)
@@ -0,0 +1,21 @@
+<!DOCTYPE module SYSTEM "./module.dtd">
+
+<module name="digital_cam">
+  <header>
+    <file name="dc.h"/>
+  </header>
+  <init fun="dc_init()"/>
+  <periodic fun="dc_periodic()" freq="4" autorun="TRUE"/>
+  <makefile>
+    <raw>
+
+# ap.CFLAGS += -DGPS_TRIGGERED_FUNCTION="dc_shoot_on_gps" 
+# ap.CFLAGS += -DDC_GPS_TRIGGER_START=1 
+# ap.CFLAGS += -DDC_GPS_TRIGGER_STOP=3 
+
+    </raw>
+    <flag name="DIGITAL_CAM" />
+    <file name="dc.c"/>
+  </makefile>
+</module>
+

Modified: paparazzi3/trunk/conf/modules/ins_xsens_MTiG_fixedwing.xml
===================================================================
--- paparazzi3/trunk/conf/modules/ins_xsens_MTiG_fixedwing.xml  2010-09-11 
13:55:20 UTC (rev 5851)
+++ paparazzi3/trunk/conf/modules/ins_xsens_MTiG_fixedwing.xml  2010-09-13 
20:46:47 UTC (rev 5852)
@@ -14,6 +14,7 @@
     <flag name="INS_LINK" value="Uart1"/>
     <flag name="UART1_BAUD" value="B115200"/>
     <flag name="USE_GPS_XSENS"/>
+    <flag name="USE_GPS_XSENS_RAW_DATA" />
     <file name="ins_xsens.c"/>
   </makefile>
 </module>

Modified: paparazzi3/trunk/sw/airborne/OSAMNav.c
===================================================================
--- paparazzi3/trunk/sw/airborne/OSAMNav.c      2010-09-11 13:55:20 UTC (rev 
5851)
+++ paparazzi3/trunk/sw/airborne/OSAMNav.c      2010-09-13 20:46:47 UTC (rev 
5852)
@@ -1056,11 +1056,12 @@
                NavVerticalAutoThrottleMode(0); /* No pitch */
                NavVerticalAltitudeMode(waypoints[From_WP].a, 0);
                
-               nav_circle_XY(FLCircle.x, FLCircle.y, FLRadius);        
-       
+               nav_circle_XY(FLCircle.x, FLCircle.y, FLRadius);
+                       
                if(NavCircleCount() > 0.2 && NavQdrCloseTo(DegOfRad(FLQDR)))
                {
                        CFLStatus = FLLine;
+                       LINE_START_FUNCTION;
                        nav_init_stage();
                }
                break;  
@@ -1069,11 +1070,14 @@
 
                NavVerticalAutoThrottleMode(0); /* No pitch */
                NavVerticalAltitudeMode(waypoints[From_WP].a, 0);
+               
                nav_route_xy(FLFROMWP.x,FLFROMWP.y,FLTOWP.x,FLTOWP.y);
+               
 
                if(nav_approaching_xy(FLTOWP.x,FLTOWP.y,FLFROMWP.x,FLFROMWP.y, 
0))
                {
                        CFLStatus = FLFinished;
+                       LINE_STOP_FUNCTION;
                        nav_init_stage();
                }                       
        break;  

Deleted: paparazzi3/trunk/sw/airborne/dc.c
===================================================================
--- paparazzi3/trunk/sw/airborne/dc.c   2010-09-11 13:55:20 UTC (rev 5851)
+++ paparazzi3/trunk/sw/airborne/dc.c   2010-09-13 20:46:47 UTC (rev 5852)
@@ -1,9 +0,0 @@
-#include "dc.h"
-
-uint8_t dc_timer;
-uint8_t dc_periodic_shutter;
-uint8_t dc_shutter_timer;
-uint8_t dc_utm_threshold;
-uint16_t dc_photo_nr = 0;
-
-uint8_t dc_shoot = 0;

Deleted: paparazzi3/trunk/sw/airborne/dc.h
===================================================================
--- paparazzi3/trunk/sw/airborne/dc.h   2010-09-11 13:55:20 UTC (rev 5851)
+++ paparazzi3/trunk/sw/airborne/dc.h   2010-09-13 20:46:47 UTC (rev 5852)
@@ -1,178 +0,0 @@
-/*
- * Paparazzi $Id$
- *  
- * Copyright (C) 2003-2008  Pascal Brisset, Antoine Drouin
- *
- * This file is part of paparazzi.
- *
- * paparazzi is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2, or (at your option)
- * any later version.
- *
- * paparazzi is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with paparazzi; see the file COPYING.  If not, write to
- * the Free Software Foundation, 59 Temple Place - Suite 330,
- * Boston, MA 02111-1307, USA. 
- *
- */
-
-/** \file dc.h
- *  \brief Digital Camera Control
- * 
- * Provides the control of the shutter and the zoom of a digital camera
- * through standard binary IOs of the board.
- *
- * Configuration: 
- *  Since the API of led.h is used, connected pins must be defined as led
- *  numbers (usually in the airframe file):
- *   <define name="DC_SHUTTER_LED" value="6"/>
- *   <define name="DC_ZOOM_LED" value="7"/>
- *  Related bank and pin must also be defined:
- *   <define name="LED_6_BANK" value="0"/>
- *   <define name="LED_6_PIN" value="2"/>
- *  The required initialization (dc_init()) and periodic (4Hz) process
- *  (dc_periodic()) are called if the DIGITAL_CAM flag is set:
- *   ap.CFLAGS += -DDIGITAL_CAM
- *
- * Usage (from the flight plan, the settings or any airborne code):
- *  - dc_Shutter(_) sets the DC_SHUTTER_LED pin output to 1 for 0.5s and sends
- *   a DC_SHOT message
- *  - dc_Zoom(_) sets the DC_ZOOM_LED pin output to 1 for 0.5s
- *  - dc_Periodic(s) activates a periodic call to dc_Shutter() every s seconds
- */
-
-#ifndef DC_H
-#define DC_H
-
-#include "std.h"
-#include "led.h"
-#include "airframe.h"
-#include "ap_downlink.h"
-#include "estimator.h"
-#include "gps.h"
-
-extern uint8_t dc_timer;
-
-extern uint8_t dc_periodic_shutter;
-/* In s. If non zero, period of automatic calls to dc_shutter() */
-extern uint8_t dc_shutter_timer;
-/* In s. Related counter */
-
-extern uint8_t dc_utm_threshold;
-/* In m. If non zero, automatic shots when greater than utm_north % 100 */
-
-/* Picture Number starting from zero */
-extern uint16_t dc_photo_nr;
-extern uint8_t dc_shoot;
-
-#ifndef DC_PUSH
-#define DC_PUSH LED_ON
-#endif
-
-#ifndef DC_RELEASE
-#define DC_RELEASE LED_OFF
-#endif
-
-#define SHUTTER_DELAY 2  /* 4Hz -> 0.5s */
-
-static inline uint8_t dc_shutter( void ) {
-  dc_timer = SHUTTER_DELAY; 
-  DC_PUSH(DC_SHUTTER_LED);
-
-  int16_t phi = DegOfRad(estimator_phi*10.0f);
-  int16_t theta = DegOfRad(estimator_theta*10.0f);
-  DOWNLINK_SEND_DC_SHOT(DefaultChannel, &dc_photo_nr, &gps_utm_east, 
&gps_utm_north, &estimator_z, &gps_utm_zone, &phi, &theta,  &gps_course, 
&gps_gspeed, &gps_itow);
-
-  dc_photo_nr++;
-
-  return 0;
-}
-
-static inline uint8_t dc_zoom( void ) {
-  dc_timer = SHUTTER_DELAY; 
-#ifdef DC_ZOOM_LED
-  DC_PUSH(DC_ZOOM_LED);
-#endif
-  return 0;
-}
-
-#define dc_Shutter(_) ({ dc_shutter(); 0; })
-#define dc_Zoom(_) ({ dc_zoom(); 0; })
-#define dc_Periodic(s) ({ dc_periodic_shutter = s; dc_shutter_timer = s; 0; })
-
-
-#define dc_init() { /* initialized as leds */ dc_periodic_shutter = 0; } /* 
Output */
-
-
-#ifndef DC_GPS_TRIGGER_START 
-#define DC_GPS_TRIGGER_START 1
-#endif
-#ifndef DC_GPS_TRIGGER_STOP 
-#define DC_GPS_TRIGGER_STOP 3
-#endif
-
-static inline void dc_shoot_on_gps( void ) {
-  static uint8_t gps_msg_counter = 0;
-
-  if (dc_shoot > 0)
-  {
-
-    if (gps_msg_counter == 0)
-    {
-      DC_PUSH(DC_SHUTTER_LED);
-      int16_t phi = DegOfRad(estimator_phi*10.0f);
-      int16_t theta = DegOfRad(estimator_theta*10.0f);
-      float gps_z = ((float)gps_alt) / 100.0f;
-      DOWNLINK_SEND_DC_SHOT(DefaultChannel, &dc_photo_nr, &gps_utm_east, 
&gps_utm_north, &gps_z, &gps_utm_zone, &phi, &theta,  &gps_course, &gps_gspeed, 
&gps_itow);
-      dc_photo_nr++;
-    }
-    else if (gps_msg_counter == DC_GPS_TRIGGER_START)
-    {
-      DC_RELEASE(DC_SHUTTER_LED);
-    }
-  
-    gps_msg_counter++;
-    if (gps_msg_counter >= DC_GPS_TRIGGER_STOP)
-      gps_msg_counter = 0;
-  }
-}      
-
-/* 4Hz */
-static inline void dc_periodic( void ) {
-  if (dc_timer) {
-    dc_timer--;
-  } else {
-    DC_RELEASE(DC_SHUTTER_LED);
-#ifdef DC_ZOOM_LED
-    DC_RELEASE(DC_ZOOM_LED);
-#endif
-  }
-
-  if (dc_periodic_shutter) {
-    RunOnceEvery(4, 
-    {
-      if (dc_shutter_timer) {
-       dc_shutter_timer--;
-      } else {
-       dc_shutter();
-       dc_shutter_timer = dc_periodic_shutter;
-      }
-    });
-  }
-}
-
-static inline void dc_shot_on_utm_north_close_to_100m_grid( void ) {
-  if (dc_utm_threshold && !dc_timer) {
-    uint32_t dist_to_100m_grid = (gps_utm_north / 100) % 100;
-    if (dist_to_100m_grid < dc_utm_threshold || 100 - dist_to_100m_grid < 
dc_utm_threshold)
-      dc_shutter();
-  }
-}
-
-#endif // DC_H

Modified: paparazzi3/trunk/sw/airborne/gps.h
===================================================================
--- paparazzi3/trunk/sw/airborne/gps.h  2010-09-11 13:55:20 UTC (rev 5851)
+++ paparazzi3/trunk/sw/airborne/gps.h  2010-09-13 20:46:47 UTC (rev 5852)
@@ -119,14 +119,10 @@
 #define GpsToggleLed() {}
 #endif
 
-#ifdef GPS
+#if defined(GPS) || defined(USE_GPS_XSENS) || defined(SITL) 
 #  define GpsTimeoutError (cpu_time_sec - last_gps_msg_t > 
FAILSAFE_DELAY_WITHOUT_GPS)
 #else
-#  ifdef SITL
-#    define GpsTimeoutError (cpu_time_sec - last_gps_msg_t > 
FAILSAFE_DELAY_WITHOUT_GPS)
-#  else
-#    define GpsTimeoutError 1
-#  endif
+#  define GpsTimeoutError 1
 #endif
 
 #define UseGpsPosNoSend(_callback) {                   \

Modified: paparazzi3/trunk/sw/airborne/main_ap.c
===================================================================
--- paparazzi3/trunk/sw/airborne/main_ap.c      2010-09-11 13:55:20 UTC (rev 
5851)
+++ paparazzi3/trunk/sw/airborne/main_ap.c      2010-09-13 20:46:47 UTC (rev 
5852)
@@ -83,10 +83,6 @@
 #include "i2c.h"
 #endif
 
-#ifdef DIGITAL_CAM
-#include "dc.h"
-#endif
-
 #ifdef DPICCO
 #include "i2c.h"
 #include "dpicco.h"
@@ -515,9 +511,6 @@
     break;
     }
 
-#ifdef DIGITAL_CAM
-    dc_periodic();
-#endif
     break;
 
 #ifdef USE_GPIO
@@ -753,10 +746,6 @@
   adc_generic_init();
 #endif
 
-#ifdef DIGITAL_CAM
-  dc_init();
-#endif
-
 #ifdef DPICCO
   i2c0_init();
   dpicco_init();

Copied: paparazzi3/trunk/sw/airborne/modules/digital_cam/dc.c (from rev 5851, 
paparazzi3/trunk/sw/airborne/dc.c)
===================================================================
--- paparazzi3/trunk/sw/airborne/modules/digital_cam/dc.c                       
        (rev 0)
+++ paparazzi3/trunk/sw/airborne/modules/digital_cam/dc.c       2010-09-13 
20:46:47 UTC (rev 5852)
@@ -0,0 +1,37 @@
+#include "dc.h"
+
+uint8_t dc_timer;
+uint8_t dc_periodic_shutter;
+uint8_t dc_shutter_timer;
+uint8_t dc_utm_threshold;
+uint16_t dc_photo_nr = 0;
+
+uint8_t dc_shoot = 0;
+
+
+#ifndef DOWNLINK_DEVICE
+#define DOWNLINK_DEVICE DOWNLINK_AP_DEVICE
+#endif
+#include "uart.h"
+#include "messages.h"
+#include "downlink.h"
+
+
+static void dc_send_shot_position(void)
+{
+      int16_t phi = DegOfRad(estimator_phi*10.0f);
+      int16_t theta = DegOfRad(estimator_theta*10.0f);
+      float gps_z = ((float)gps_alt) / 100.0f;
+      DOWNLINK_SEND_DC_SHOT(DefaultChannel, &dc_photo_nr, &gps_utm_east, 
&gps_utm_north, &gps_z, &gps_utm_zone, &phi, &theta,  &gps_course, &gps_gspeed, 
&gps_itow);
+      dc_photo_nr++;   
+}
+
+uint8_t dc_shutter( void )
+{
+  dc_timer = SHUTTER_DELAY; 
+  DC_PUSH(DC_SHUTTER_LED);
+  dc_send_shot_position();
+
+  return 0;
+}
+

Copied: paparazzi3/trunk/sw/airborne/modules/digital_cam/dc.h (from rev 5851, 
paparazzi3/trunk/sw/airborne/dc.h)
===================================================================
--- paparazzi3/trunk/sw/airborne/modules/digital_cam/dc.h                       
        (rev 0)
+++ paparazzi3/trunk/sw/airborne/modules/digital_cam/dc.h       2010-09-13 
20:46:47 UTC (rev 5852)
@@ -0,0 +1,176 @@
+/*
+ * Paparazzi $Id$
+ *  
+ * Copyright (C) 2003-2008  Pascal Brisset, Antoine Drouin
+ *
+ * This file is part of paparazzi.
+ *
+ * paparazzi is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2, or (at your option)
+ * any later version.
+ *
+ * paparazzi is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with paparazzi; see the file COPYING.  If not, write to
+ * the Free Software Foundation, 59 Temple Place - Suite 330,
+ * Boston, MA 02111-1307, USA. 
+ *
+ */
+
+/** \file dc.h
+ *  \brief Digital Camera Control
+ * 
+ * Provides the control of the shutter and the zoom of a digital camera
+ * through standard binary IOs of the board.
+ *
+ * Configuration: 
+ *  Since the API of led.h is used, connected pins must be defined as led
+ *  numbers (usually in the airframe file):
+ *   <define name="DC_SHUTTER_LED" value="6"/>
+ *   <define name="DC_ZOOM_LED" value="7"/>
+ *  Related bank and pin must also be defined:
+ *   <define name="LED_6_BANK" value="0"/>
+ *   <define name="LED_6_PIN" value="2"/>
+ *  The required initialization (dc_init()) and periodic (4Hz) process
+ *  (dc_periodic()) are called if the DIGITAL_CAM flag is set:
+ *   ap.CFLAGS += -DDIGITAL_CAM
+ *
+ * Usage (from the flight plan, the settings or any airborne code):
+ *  - dc_Shutter(_) sets the DC_SHUTTER_LED pin output to 1 for 0.5s and sends
+ *   a DC_SHOT message
+ *  - dc_Zoom(_) sets the DC_ZOOM_LED pin output to 1 for 0.5s
+ *  - dc_Periodic(s) activates a periodic call to dc_Shutter() every s seconds
+ */
+
+#ifndef DC_H
+#define DC_H
+
+#include "std.h"
+#include "led.h"
+#include "airframe.h"
+#include "estimator.h"
+#include "gps.h"
+
+
+extern uint8_t dc_timer;
+
+extern uint8_t dc_periodic_shutter;
+/* In s. If non zero, period of automatic calls to dc_shutter() */
+extern uint8_t dc_shutter_timer;
+/* In s. Related counter */
+
+extern uint8_t dc_utm_threshold;
+/* In m. If non zero, automatic shots when greater than utm_north % 100 */
+
+/* Picture Number starting from zero */
+extern uint16_t dc_photo_nr;
+extern uint8_t dc_shoot;
+
+#ifndef DC_PUSH
+#define DC_PUSH LED_ON
+#endif
+
+#ifndef DC_RELEASE
+#define DC_RELEASE LED_OFF
+#endif
+
+#define SHUTTER_DELAY 2  /* 4Hz -> 0.5s */
+
+uint8_t dc_shutter( void );
+
+static inline uint8_t dc_zoom( void ) {
+  dc_timer = SHUTTER_DELAY; 
+#ifdef DC_ZOOM_LED
+  DC_PUSH(DC_ZOOM_LED);
+#endif
+  return 0;
+}
+
+#define dc_Shutter(_) ({ dc_shutter(); 0; })
+#define dc_Zoom(_) ({ dc_zoom(); 0; })
+#define dc_Periodic(s) ({ dc_periodic_shutter = s; dc_shutter_timer = s; 0; })
+
+#ifndef DC_PERIODIC_SHUTTER
+#define DC_PERIODIC_SHUTTER 0
+#endif
+
+#define dc_init() { /* initialized as leds */ dc_periodic_shutter = 
DC_PERIODIC_SHUTTER; DC_PUSH(DC_SHUTTER_LED);} /* Output */
+
+
+#ifndef DC_GPS_TRIGGER_START 
+#define DC_GPS_TRIGGER_START 1
+#endif
+#ifndef DC_GPS_TRIGGER_STOP 
+#define DC_GPS_TRIGGER_STOP 3
+#endif
+
+static void dc_send_shot_position(void);
+
+static inline void dc_shoot_on_gps( void ) {
+  static uint8_t gps_msg_counter = 0;
+
+  if (dc_shoot > 0)
+  {
+
+    if (gps_msg_counter == 0)
+    {
+      DC_PUSH(DC_SHUTTER_LED);
+
+      dc_send_shot_position();
+    }
+    else if (gps_msg_counter == DC_GPS_TRIGGER_START)
+    {
+      DC_RELEASE(DC_SHUTTER_LED);
+    }
+  
+    gps_msg_counter++;
+    if (gps_msg_counter >= DC_GPS_TRIGGER_STOP)
+      gps_msg_counter = 0;
+  }
+}      
+
+/* 4Hz */
+static inline void dc_periodic( void ) {
+  if (dc_timer) {
+    dc_timer--;
+  } else {
+    DC_RELEASE(DC_SHUTTER_LED);
+#ifdef DC_ZOOM_LED
+    DC_RELEASE(DC_ZOOM_LED);
+#endif
+  }
+
+  if (dc_shoot > 0)
+  {
+    if (dc_periodic_shutter) {
+      RunOnceEvery(2, 
+      {
+        if (dc_shutter_timer) {
+         dc_shutter_timer--;
+        } else {
+         dc_shutter();
+         dc_shutter_timer = dc_periodic_shutter;
+        }
+      });
+    }
+  }
+  else
+  {
+    dc_shutter_timer = 0;
+  }
+}
+
+static inline void dc_shot_on_utm_north_close_to_100m_grid( void ) {
+  if (dc_utm_threshold && !dc_timer) {
+    uint32_t dist_to_100m_grid = (gps_utm_north / 100) % 100;
+    if (dist_to_100m_grid < dc_utm_threshold || 100 - dist_to_100m_grid < 
dc_utm_threshold)
+      dc_shutter();
+  }
+}
+
+#endif // DC_H

Modified: paparazzi3/trunk/sw/airborne/modules/ins/ins_xsens.c
===================================================================
--- paparazzi3/trunk/sw/airborne/modules/ins/ins_xsens.c        2010-09-11 
13:55:20 UTC (rev 5851)
+++ paparazzi3/trunk/sw/airborne/modules/ins/ins_xsens.c        2010-09-13 
20:46:47 UTC (rev 5852)
@@ -189,7 +189,12 @@
 void handle_ins_msg( void) {
   EstimatorSetAtt(ins_phi, ins_psi, ins_theta);
   EstimatorSetRate(ins_p,ins_q);
+  if (gps_mode != 0x03)
+    gps_gspeed = 0;
   EstimatorSetSpeedPol(gps_gspeed, ins_psi, ins_vz);
+  // EstimatorSetAlt(ins_z);
+  estimator_update_state_gps();
+  reset_gps_watchdog();
 }
 
 void parse_ins_msg( void ) {
@@ -231,7 +236,6 @@
 #ifdef USE_GPS_XSENS
         gps_week = 0; // FIXME
         gps_itow = XSENS_DATA_RAWGPS_itow(xsens_msg_buf,offset) * 10;
-#ifdef USE_GPS_XSENS_RAW_DATA
         gps_lat = XSENS_DATA_RAWGPS_lat(xsens_msg_buf,offset);
         gps_lon = XSENS_DATA_RAWGPS_lon(xsens_msg_buf,offset);
         /* Set the real UTM zone */
@@ -248,7 +252,6 @@
         ins_vy = (INS_FORMAT)XSENS_DATA_RAWGPS_vel_n(xsens_msg_buf,offset) / 
100.;
         ins_vz = (INS_FORMAT)XSENS_DATA_RAWGPS_vel_d(xsens_msg_buf,offset) / 
100.;
         gps_climb = -XSENS_DATA_RAWGPS_vel_d(xsens_msg_buf,offset) / 10;
-#endif
         gps_Pacc = XSENS_DATA_RAWGPS_hacc(xsens_msg_buf,offset) / 100;
         gps_Sacc = XSENS_DATA_RAWGPS_sacc(xsens_msg_buf,offset) / 100;
         gps_PDOP = 5;
@@ -324,6 +327,7 @@
         offset += l * XSENS_DATA_Auxiliary_LENGTH / 2;
       }
       if (XSENS_MASK_Position(xsens_output_mode)) {
+#ifdef USE_GPS_XSENS_RAW_DATA
 #ifdef USE_GPS_XSENS
         float lat = XSENS_DATA_Position_lat(xsens_msg_buf,offset);
         float lon = XSENS_DATA_Position_lon(xsens_msg_buf,offset);
@@ -335,12 +339,12 @@
         ins_y = latlong_utm_y;
         gps_utm_east  = ins_x * 100;
         gps_utm_north = ins_y * 100;
-        ins_z = -XSENS_DATA_Position_alt(xsens_msg_buf,offset);
+        ins_z = XSENS_DATA_Position_alt(xsens_msg_buf,offset);
 #ifndef USE_VFILTER
         gps_alt = ins_z * 100;
 #endif
-       reset_gps_watchdog();
 #endif
+#endif
         offset += XSENS_DATA_Position_LENGTH;
       }
       if (XSENS_MASK_Velocity(xsens_output_mode)) {