From 5d7282446599ea47319b4651652e99b6d5497b68 Mon Sep 17 00:00:00 2001
From: kirkscheper <kirkscheper@gmail.com>
Date: Thu, 7 Jan 2016 17:19:41 +0100
Subject: [PATCH] [gps] separated alt and alt_speed updates with new define,
 cleanup

---
 .../BR/ladybird_kit_bart_bluegiga_optitrack.xml     |  1 +
 sw/airborne/subsystems/gps/gps_datalink.c           | 13 +++++--------
 sw/airborne/subsystems/ins/ins_int.c                |  4 ++++
 3 files changed, 10 insertions(+), 8 deletions(-)

diff --git a/conf/airframes/BR/ladybird_kit_bart_bluegiga_optitrack.xml b/conf/airframes/BR/ladybird_kit_bart_bluegiga_optitrack.xml
index 342e682ae95..f9ff1440345 100644
--- a/conf/airframes/BR/ladybird_kit_bart_bluegiga_optitrack.xml
+++ b/conf/airframes/BR/ladybird_kit_bart_bluegiga_optitrack.xml
@@ -159,6 +159,7 @@
 
   <section name="INS" prefix="INS_">
     <define name="USE_GPS_ALT" value="1"/>
+    <define name="USE_GPS_ALT_SPEED" value="1"/>
   </section>
 
  <section name="GUIDANCE_V" prefix="GUIDANCE_V_">
diff --git a/sw/airborne/subsystems/gps/gps_datalink.c b/sw/airborne/subsystems/gps/gps_datalink.c
index 415d3029992..788a3569c87 100644
--- a/sw/airborne/subsystems/gps/gps_datalink.c
+++ b/sw/airborne/subsystems/gps/gps_datalink.c
@@ -27,10 +27,7 @@
  * GPS structure to the values received.
  */
 
-#include "messages.h"
-#include "generated/airframe.h"           // AC_ID
 #include "generated/flight_plan.h"        // reference lla NAV_XXX0
-#include "subsystems/datalink/downlink.h"
 
 #include "subsystems/gps.h"
 #include "subsystems/abi.h"
@@ -98,8 +95,8 @@ void parse_gps_datalink_small(uint8_t num_sv, uint32_t pos_xyz, uint32_t speed_x
   ecef_of_enu_vect_i(&gps.ecef_vel , &ltp_def , &enu_speed);
   SetBit(gps.valid_fields, GPS_VALID_VEL_ECEF_BIT);
 
-  gps.ned_vel.x = enu_speed.x;
-  gps.ned_vel.y = enu_speed.y;
+  gps.ned_vel.x = enu_speed.y;
+  gps.ned_vel.y = enu_speed.x;
   gps.ned_vel.z = -enu_speed.z;
   SetBit(gps.valid_fields, GPS_VALID_VEL_NED_BIT);
 
@@ -148,9 +145,9 @@ void parse_gps_datalink(uint8_t numsv, int32_t ecef_x, int32_t ecef_y, int32_t e
   gps.ecef_vel.z = ecef_zd;
   SetBit(gps.valid_fields, GPS_VALID_VEL_ECEF_BIT);
 
-  struct LtpDef_i ref_ltp;
-  ltp_def_from_ecef_i(&ref_ltp, &gps.ecef_pos);
-  ned_of_ecef_vect_i(&gps.ned_vel, &ref_ltp, &gps.ecef_vel);
+  gps.ned_vel.x = enu_speed.y;
+  gps.ned_vel.y = enu_speed.x;
+  gps.ned_vel.z = -enu_speed.z;
   SetBit(gps.valid_fields, GPS_VALID_VEL_NED_BIT);
 
   gps.course = course;
diff --git a/sw/airborne/subsystems/ins/ins_int.c b/sw/airborne/subsystems/ins/ins_int.c
index abbd2f42441..3fbad1560f6 100644
--- a/sw/airborne/subsystems/ins/ins_int.c
+++ b/sw/airborne/subsystems/ins/ins_int.c
@@ -94,6 +94,7 @@ PRINT_CONFIG_MSG("INS_SONAR_UPDATE_ON_AGL defaulting to FALSE")
 
 #endif // USE_SONAR
 
+#if USE_GPS
 #ifndef INS_VFF_R_GPS
 #define INS_VFF_R_GPS 2.0
 #endif
@@ -101,6 +102,7 @@ PRINT_CONFIG_MSG("INS_SONAR_UPDATE_ON_AGL defaulting to FALSE")
 #ifndef INS_VFF_VZ_R_GPS
 #define INS_VFF_VZ_R_GPS 2.0
 #endif
+#endif // USE_GPS
 
 /** maximum number of propagation steps without any updates in between */
 #ifndef INS_MAX_PROPAGATION_STEPS
@@ -378,6 +380,8 @@ void ins_int_update_gps(struct GpsState *gps_s)
 
 #if INS_USE_GPS_ALT
   vff_update_z_conf(((float)gps_pos_cm_ned.z) / 100.0, INS_VFF_R_GPS);
+#endif
+#if INS_USE_GPS_ALT_SPEED
   vff_update_vz_conf(((float)gps_speed_cm_s_ned.z) / 100.0, INS_VFF_VZ_R_GPS);
 #endif