[subsystem ins] only update vert filter with baro measurement when no…

…t realigning, did a wrong update in that case
paparazzi · May 8, 2012 · 6988e2c · 6988e2c
1 parent 2669d8f
commit 6988e2c
Showing 1 changed file with 5 additions and 3 deletions.
diff --git a/sw/airborne/subsystems/ins.c b/sw/airborne/subsystems/ins.c
@@ -182,8 +182,6 @@ void ins_update_baro() {
       ins_qfe = baro.absolute;
       ins_baro_initialised = TRUE;
     }
-    ins_baro_alt = ((baro.absolute - ins_qfe) * INS_BARO_SENS_NUM)/INS_BARO_SENS_DEN;
-    float alt_float = POS_FLOAT_OF_BFP(ins_baro_alt);
     if (ins_vf_realign) {
       ins_vf_realign = FALSE;
       ins_qfe = baro.absolute;
@@ -198,7 +196,11 @@ void ins_update_baro() {
       ins_enu_speed.z = -ins_ltp_speed.z;
       ins_enu_accel.z = -ins_ltp_accel.z;
     }
-    vff_update(alt_float);
+    else { /* not realigning, so normal update with baro measurement */
+      ins_baro_alt = ((baro.absolute - ins_qfe) * INS_BARO_SENS_NUM)/INS_BARO_SENS_DEN;
+      float alt_float = POS_FLOAT_OF_BFP(ins_baro_alt);
+      vff_update(alt_float);
+    }
   }
 #endif
 }