[gps] separated alt and alt_speed updates with new define, cleanup

paparazzi · Jan 7, 2016 · 5d72824 · 5d72824
1 parent 56fd179
commit 5d72824
Show file tree

Hide file tree

Showing 3 changed files with 10 additions and 8 deletions.
diff --git 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
@@ -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
@@ -94,13 +94,15 @@ 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
 
 #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