fix to use ins_alt_dot with baro wen USE_BAROMETER is defined

paparazzi · Aug 9, 2013 · 732bdcd · 732bdcd
1 parent d86f3d2
commit 732bdcd
Showing 1 changed file with 5 additions and 1 deletion.
diff --git a/sw/airborne/subsystems/ins/ins_alt_float.c b/sw/airborne/subsystems/ins/ins_alt_float.c
@@ -105,6 +105,10 @@ void ins_update_baro() {
       UTM_COPY(utm, *stateGetPositionUtm_f());
       utm.alt = ins_alt;
       stateSetPositionUtm_f(&utm);
+      struct NedCoor_f ned_vel;
+      memcpy(&ned_vel, stateGetSpeedNed_f(), sizeof(struct NedCoor_f));
+      ned_vel.z = -ins_alt_dot;
+      stateSetSpeedNed_f(&ned_vel);
     }
   }
 #endif
@@ -129,7 +133,7 @@ void ins_update_gps(void) {
   struct NedCoor_f ned_vel = {
     gps.ned_vel.x / 100.,
     gps.ned_vel.y / 100.,
-    gps.ned_vel.z / 100.
+    -ins_alt_dot
   };
   // set velocity
   stateSetSpeedNed_f(&ned_vel);