From beedf1ce4f9d7007a734301546573712f65d4157 Mon Sep 17 00:00:00 2001 From: kamilritz Date: Tue, 15 Oct 2019 14:53:14 +0200 Subject: [PATCH] Add print statement to reset vel or pos function --- EKF/ekf_helper.cpp | 15 ++++++++++++--- 1 file changed, 12 insertions(+), 3 deletions(-) diff --git a/EKF/ekf_helper.cpp b/EKF/ekf_helper.cpp index a54a886335..1401bf2d36 100644 --- a/EKF/ekf_helper.cpp +++ b/EKF/ekf_helper.cpp @@ -54,6 +54,7 @@ bool Ekf::resetVelocity() // reset EKF states if (_control_status.flags.gps && _gps_check_fail_status.value==0) { + ECL_INFO_TIMESTAMPED("EKF reset velocity to GPS"); // this reset is only called if we have new gps data at the fusion time horizon _state.vel = _gps_sample_delayed.vel; @@ -61,6 +62,7 @@ bool Ekf::resetVelocity() setDiag(P, 4, 6, sq(_gps_sample_delayed.sacc)); } else if (_control_status.flags.opt_flow) { + ECL_INFO_TIMESTAMPED("EKF reset velocity to flow"); // constrain height above ground to be above minimum possible float heightAboveGndEst = fmaxf((_terrain_vpos - _state.pos(2)), _params.rng_gnd_clearance); @@ -95,6 +97,7 @@ bool Ekf::resetVelocity() // reset the horizontal velocity variance using the optical flow noise variance P[5][5] = P[4][4] = sq(range) * calcOptFlowMeasVar(); } else if (_control_status.flags.ev_vel) { + ECL_INFO_TIMESTAMPED("EKF reset velocity to ev velocity"); Vector3f _ev_vel = _ev_sample_delayed.vel; if(_params.fusion_mode & MASK_ROTATE_EV){ _ev_vel = _ev_rot_mat *_ev_sample_delayed.vel; @@ -103,10 +106,8 @@ bool Ekf::resetVelocity() _state.vel(1) = _ev_vel(1); _state.vel(2) = _ev_vel(2); setDiag(P, 4, 6, sq(_ev_sample_delayed.velErr)); - } else if (_control_status.flags.ev_pos) { - _state.vel.setZero(); - zeroOffDiag(P, 4, 6); } else { + ECL_INFO_TIMESTAMPED("EKF reset velocity to zero"); // Used when falling back to non-aiding mode of operation _state.vel(0) = 0.0f; _state.vel(1) = 0.0f; @@ -138,6 +139,7 @@ bool Ekf::resetVelocity() // gps measurement then use for position initialisation bool Ekf::resetPosition() { + // ECL_INFO_TIMESTAMPED("Reset Position"); // used to calculate the position change due to the reset Vector2f posNE_before_reset; posNE_before_reset(0) = _state.pos(0); @@ -147,6 +149,8 @@ bool Ekf::resetPosition() _hpos_prev_available = false; if (_control_status.flags.gps) { + ECL_INFO_TIMESTAMPED("EKF reset position to GPS"); + // this reset is only called if we have new gps data at the fusion time horizon _state.pos(0) = _gps_sample_delayed.pos(0); _state.pos(1) = _gps_sample_delayed.pos(1); @@ -155,6 +159,8 @@ bool Ekf::resetPosition() setDiag(P, 7, 8, sq(_gps_sample_delayed.hacc)); } else if (_control_status.flags.ev_pos) { + ECL_INFO_TIMESTAMPED("EKF reset position to ev position"); + // this reset is only called if we have new ev data at the fusion time horizon Vector3f _ev_pos = _ev_sample_delayed.pos; if(_params.fusion_mode & MASK_ROTATE_EV){ @@ -167,6 +173,8 @@ bool Ekf::resetPosition() setDiag(P, 7, 8, sq(_ev_sample_delayed.posErr)); } else if (_control_status.flags.opt_flow) { + ECL_INFO_TIMESTAMPED("EKF reset position to last known position"); + if (!_control_status.flags.in_air) { // we are likely starting OF for the first time so reset the horizontal position _state.pos(0) = 0.0f; @@ -184,6 +192,7 @@ bool Ekf::resetPosition() zeroRows(P,7,8); } else { + ECL_INFO_TIMESTAMPED("EKF reset position to last known position"); // Used when falling back to non-aiding mode of operation _state.pos(0) = _last_known_posNE(0); _state.pos(1) = _last_known_posNE(1);