[paparazzi-commits] [4058] use hmsl data to correct ellipsoid alt

[Gautier Hattenberger]

Revision: 4058
          http://svn.sv.gnu.org/viewvc/?view=rev&root=paparazzi&revision=4058
Author:   gautier
Date:     2009-09-03 11:10:18 +0000 (Thu, 03 Sep 2009)
Log Message:
-----------
use hmsl data to correct ellipsoid alt

Modified Paths:
--------------
    paparazzi3/trunk/sw/airborne/booz/booz2_datalink.c
    paparazzi3/trunk/sw/airborne/booz/booz2_gps.c
    paparazzi3/trunk/sw/airborne/booz/booz2_gps.h
    paparazzi3/trunk/sw/airborne/booz/booz2_ins.c
    paparazzi3/trunk/sw/airborne/math/pprz_geodetic_int.h

Modified: paparazzi3/trunk/sw/airborne/booz/booz2_datalink.c
===================================================================
--- paparazzi3/trunk/sw/airborne/booz/booz2_datalink.c  2009-09-03 00:10:13 UTC 
(rev 4057)
+++ paparazzi3/trunk/sw/airborne/booz/booz2_datalink.c  2009-09-03 11:10:18 UTC 
(rev 4058)
@@ -85,13 +85,13 @@
       struct EnuCoor_i enu;
       lla.lat = INT32_RAD_OF_DEG(DL_MOVE_WP_lat(dl_buffer));
       lla.lon = INT32_RAD_OF_DEG(DL_MOVE_WP_lon(dl_buffer));
-      lla.alt = DL_MOVE_WP_alt(dl_buffer) + booz_ins_ltp_def.lla.alt;
+      lla.alt = DL_MOVE_WP_alt(dl_buffer) - booz_ins_ltp_def.hmsl + 
booz_ins_ltp_def.lla.alt;
       enu_of_lla_point_i(&enu,&booz_ins_ltp_def,&lla);
       enu.x = POS_BFP_OF_REAL(enu.x)/100;
       enu.y = POS_BFP_OF_REAL(enu.y)/100;
       enu.z = POS_BFP_OF_REAL(enu.z)/100;
       VECT3_ASSIGN(waypoints[wp_id], enu.x, enu.y, enu.z);
-      DOWNLINK_SEND_WP_MOVED_LTP(DefaultChannel, &wp_id, &enu.x, &enu.y, 
&enu.z);
+      DOWNLINK_SEND_WP_MOVED_ENU(DefaultChannel, &wp_id, &enu.x, &enu.y, 
&enu.z);
     }
     break;
 #endif /* USE_NAVIGATION */

Modified: paparazzi3/trunk/sw/airborne/booz/booz2_gps.c
===================================================================
--- paparazzi3/trunk/sw/airborne/booz/booz2_gps.c       2009-09-03 00:10:13 UTC 
(rev 4057)
+++ paparazzi3/trunk/sw/airborne/booz/booz2_gps.c       2009-09-03 11:10:18 UTC 
(rev 4058)
@@ -64,15 +64,19 @@
       booz_gps_state.num_sv     = UBX_NAV_SOL_numSV(ubx_msg_buf);
 #ifdef GPS_LED
       if (booz_gps_state.fix == BOOZ2_GPS_FIX_3D) {
-       LED_OFF(GPS_LED);
+        LED_OFF(GPS_LED);
       }
       else {
-       LED_TOGGLE(GPS_LED);
+        LED_TOGGLE(GPS_LED);
       }
 #endif
+    } else if (ubx_id == UBX_NAV_POSLLH_ID) {
+      booz_gps_state.lla_pos.lat = UBX_NAV_POSLLH_LAT(ubx_msg_buf);
+      booz_gps_state.lla_pos.lon = UBX_NAV_POSLLH_LON(ubx_msg_buf);
+      booz_gps_state.lla_pos.alt = UBX_NAV_POSLLH_HEIGHT(ubx_msg_buf) / 10;
+      booz_gps_state.hmsl        = UBX_NAV_POSLLH_HMSL(ubx_msg_buf) / 10;
     }
   }
-
 }
 
 /* UBX parsing */

Modified: paparazzi3/trunk/sw/airborne/booz/booz2_gps.h
===================================================================
--- paparazzi3/trunk/sw/airborne/booz/booz2_gps.h       2009-09-03 00:10:13 UTC 
(rev 4057)
+++ paparazzi3/trunk/sw/airborne/booz/booz2_gps.h       2009-09-03 11:10:18 UTC 
(rev 4058)
@@ -30,6 +30,8 @@
 struct Booz_gps_state {
   struct EcefCoor_i ecef_pos;    /* pos ECEF in cm        */
   struct EcefCoor_i ecef_vel;    /* speed ECEF in cm/s    */
+  struct LlaCoor_i lla_pos;      /* pos LLA               */
+  int32_t hmsl;                  /* above mean sea level  */
   uint32_t pacc;                 /* position accuracy     */
   uint32_t sacc;                 /* speed accuracy        */
   uint16_t pdop;                 /* dilution of precision */
@@ -47,6 +49,8 @@
 
 #include "ubx_protocol.h"
 
+#define GpsFixValid() (booz_gps_state.fix == BOOZ2_GPS_FIX_3D)
+
 #ifdef SITL
 extern bool_t booz_gps_available;
 #define GPS_LINKChAvailable() (FALSE)

Modified: paparazzi3/trunk/sw/airborne/booz/booz2_ins.c
===================================================================
--- paparazzi3/trunk/sw/airborne/booz/booz2_ins.c       2009-09-03 00:10:13 UTC 
(rev 4057)
+++ paparazzi3/trunk/sw/airborne/booz/booz2_ins.c       2009-09-03 11:10:18 UTC 
(rev 4058)
@@ -82,7 +82,7 @@
 
 
 void booz_ins_init() {
-#ifdef USE_NAV_INS_INIT
+#ifdef USE_INS_NAV_INIT
   booz_ins_ltp_initialised  = TRUE;
   struct EcefCoor_i nav_init;
   VECT3_ASSIGN(nav_init,NAV_ECEF_X0,NAV_ECEF_Y0,NAV_ECEF_Z0);
@@ -192,28 +192,30 @@
   if (booz_gps_state.fix == BOOZ2_GPS_FIX_3D) {
     if (!booz_ins_ltp_initialised) {
       ltp_def_from_ecef_i(&booz_ins_ltp_def, &booz_gps_state.ecef_pos);
+      booz_ins_ltp_def.lla.alt = booz_gps_state.lla_pos.alt;
+      booz_ins_ltp_def.hmsl = booz_gps_state.hmsl;
       booz_ins_ltp_initialised = TRUE;
     }
     ned_of_ecef_point_i(&booz_ins_gps_pos_cm_ned, &booz_ins_ltp_def, 
&booz_gps_state.ecef_pos);
     ned_of_ecef_vect_i(&booz_ins_gps_speed_cm_s_ned, &booz_ins_ltp_def, 
&booz_gps_state.ecef_vel);
 
 #ifdef USE_HFF
-       VECT2_ASSIGN(booz_ins_gps_pos_m_ned, booz_ins_gps_pos_cm_ned.x, 
booz_ins_gps_pos_cm_ned.y);
-       VECT2_SDIV(booz_ins_gps_pos_m_ned, booz_ins_gps_pos_m_ned, 100.);
-       VECT2_ASSIGN(booz_ins_gps_speed_m_s_ned, booz_ins_gps_speed_cm_s_ned.x, 
booz_ins_gps_speed_cm_s_ned.y);
+    VECT2_ASSIGN(booz_ins_gps_pos_m_ned, booz_ins_gps_pos_cm_ned.x, 
booz_ins_gps_pos_cm_ned.y);
+    VECT2_SDIV(booz_ins_gps_pos_m_ned, booz_ins_gps_pos_m_ned, 100.);
+    VECT2_ASSIGN(booz_ins_gps_speed_m_s_ned, booz_ins_gps_speed_cm_s_ned.x, 
booz_ins_gps_speed_cm_s_ned.y);
     VECT2_SDIV(booz_ins_gps_speed_m_s_ned, booz_ins_gps_speed_m_s_ned, 100.);
-       if (booz_ins_hff_realign) {
+    if (booz_ins_hff_realign) {
       booz_ins_hff_realign = FALSE;
 #ifdef SITL
-         struct FloatVect2 true_pos, true_speed;
-         VECT2_COPY(true_pos, fdm.ltpprz_pos);
-         VECT2_COPY(true_speed, fdm.ltpprz_ecef_vel);
-         b2_hff_realign(true_pos, true_speed);
+      struct FloatVect2 true_pos, true_speed;
+      VECT2_COPY(true_pos, fdm.ltpprz_pos);
+      VECT2_COPY(true_speed, fdm.ltpprz_ecef_vel);
+      b2_hff_realign(true_pos, true_speed);
 #else
-         b2_hff_realign(booz_ins_gps_pos_m_ned, booz_ins_gps_speed_m_s_ned);
+      b2_hff_realign(booz_ins_gps_pos_m_ned, booz_ins_gps_speed_m_s_ned);
 #endif
-       }
-       b2_hff_update_gps();
+    }
+    b2_hff_update_gps();
     booz_ins_ltp_accel.x = ACCEL_BFP_OF_REAL(b2_hff_state.xdotdot);
     booz_ins_ltp_accel.y = ACCEL_BFP_OF_REAL(b2_hff_state.ydotdot);
     booz_ins_ltp_speed.x = SPEED_BFP_OF_REAL(b2_hff_state.xdot);
@@ -228,7 +230,7 @@
     INT32_VECT2_SCALE_2(booz_ins_ltp_pos, booz_ins_gps_pos_cm_ned, 
INT32_POS_OF_CM_NUM, INT32_POS_OF_CM_DEN);
     INT32_VECT2_SCALE_2(booz_ins_ltp_speed, booz_ins_gps_speed_cm_s_ned, 
INT32_SPEED_OF_CM_S_NUM, INT32_SPEED_OF_CM_S_DEN);
 #ifdef USE_GPS_LAG_HACK
-       VECT2_COPY(d_pos, booz_ins_ltp_speed);
+    VECT2_COPY(d_pos, booz_ins_ltp_speed);
     INT32_VECT2_RSHIFT(d_pos, d_pos, 11);
     VECT2_ADD(booz_ins_ltp_pos, d_pos);
 #endif

Modified: paparazzi3/trunk/sw/airborne/math/pprz_geodetic_int.h
===================================================================
--- paparazzi3/trunk/sw/airborne/math/pprz_geodetic_int.h       2009-09-03 
00:10:13 UTC (rev 4057)
+++ paparazzi3/trunk/sw/airborne/math/pprz_geodetic_int.h       2009-09-03 
11:10:18 UTC (rev 4058)
@@ -42,6 +42,7 @@
   struct EcefCoor_i ecef;        /* Reference point in ecef */
   struct LlaCoor_i  lla;         /* Reference point in lla  */
   struct Int32Mat33 ltp_of_ecef; /* Rotation matrix         */
+  int32_t hmsl;                  /* Height above mean sea level */
 };
 
 extern void ltp_def_from_ecef_i(struct LtpDef_i* def, struct EcefCoor_i* ecef);