Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-06-01 11:06:04 +08:00
Code Issues Actions 18 Packages Projects Releases Wiki Activity

Set not yet supported estimator_innovations fields to zero

Browse Source
This commit is contained in:
kamilritz
2019-12-05 08:17:16 +01:00
committed by Julian Kent
parent 5d0965e83a
commit 24efc1b79c
1 changed files with 13 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/ekf2_main.cpp
+13
View File
@@ -1620,6 +1620,10 @@ void Ekf2::Run()
_ekf.getAirspeedInnov(innovations.airspeed); _ekf.getAirspeedInnov(innovations.airspeed);
_ekf.getBetaInnov(innovations.beta); _ekf.getBetaInnov(innovations.beta);
_ekf.getHaglInnov(innovations.hagl); _ekf.getHaglInnov(innovations.hagl);
// Not yet supported
innovations.aux_vvel = NAN;
innovations.fake_hpos[0] = innovations.fake_hpos[1] = innovations.fake_vpos = NAN;
innovations.fake_hvel[0] = innovations.fake_hvel[1] = innovations.fake_vvel = NAN;
// publish estimator innovation variance data // publish estimator innovation variance data
estimator_innovations_s innovation_var; estimator_innovations_s innovation_var;
@@ -1638,6 +1642,11 @@ void Ekf2::Run()
_ekf.getAirspeedInnovVar(innovation_var.airspeed); _ekf.getAirspeedInnovVar(innovation_var.airspeed);
_ekf.getBetaInnovVar(innovation_var.beta); _ekf.getBetaInnovVar(innovation_var.beta);
_ekf.getHaglInnovVar(innovation_var.hagl); _ekf.getHaglInnovVar(innovation_var.hagl);
// Not yet supported
innovation_var.aux_vvel = NAN;
innovation_var.fake_hpos[0] = innovation_var.fake_hpos[1] = innovation_var.fake_vpos = NAN;
innovation_var.fake_hvel[0] = innovation_var.fake_hvel[1] = innovation_var.fake_vvel = NAN;
// publish estimator innovation test ratio data // publish estimator innovation test ratio data
estimator_innovations_s test_ratios; estimator_innovations_s test_ratios;
@@ -1656,6 +1665,10 @@ void Ekf2::Run()
_ekf.getAirspeedInnovRatio(test_ratios.airspeed); _ekf.getAirspeedInnovRatio(test_ratios.airspeed);
_ekf.getBetaInnovRatio(test_ratios.beta); _ekf.getBetaInnovRatio(test_ratios.beta);
_ekf.getHaglInnovRatio(test_ratios.hagl); _ekf.getHaglInnovRatio(test_ratios.hagl);
// Not yet supported
test_ratios.aux_vvel = NAN;
test_ratios.fake_hpos[0] = test_ratios.fake_hpos[1] = test_ratios.fake_vpos = NAN;
test_ratios.fake_hvel[0] = test_ratios.fake_hvel[1] = test_ratios.fake_vvel = NAN;
// calculate noise filtered velocity innovations which are used for pre-flight checking // calculate noise filtered velocity innovations which are used for pre-flight checking
if (_vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_STANDBY) { if (_vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_STANDBY) {