dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

Add print statement to reset vel or pos function

This commit is contained in:
kamilritz 2019-10-15 14:53:14 +02:00 committed by Paul Riseborough
parent 5c038a3b43
commit beedf1ce4f
1 changed files with 12 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

15
EKF/ekf_helper.cpp
View File

@ -54,6 +54,7 @@ bool Ekf::resetVelocity()
// reset EKF states // reset EKF states
if (_control_status.flags.gps && _gps_check_fail_status.value==0) { 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 // this reset is only called if we have new gps data at the fusion time horizon
_state.vel = _gps_sample_delayed.vel; _state.vel = _gps_sample_delayed.vel;
@ -61,6 +62,7 @@ bool Ekf::resetVelocity()
setDiag(P, 4, 6, sq(_gps_sample_delayed.sacc)); setDiag(P, 4, 6, sq(_gps_sample_delayed.sacc));
} else if (_control_status.flags.opt_flow) { } else if (_control_status.flags.opt_flow) {
ECL_INFO_TIMESTAMPED("EKF reset velocity to flow");
// constrain height above ground to be above minimum possible // constrain height above ground to be above minimum possible
float heightAboveGndEst = fmaxf((_terrain_vpos - _state.pos(2)), _params.rng_gnd_clearance); 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 // reset the horizontal velocity variance using the optical flow noise variance
P[5][5] = P[4][4] = sq(range) * calcOptFlowMeasVar(); P[5][5] = P[4][4] = sq(range) * calcOptFlowMeasVar();
} else if (_control_status.flags.ev_vel) { } else if (_control_status.flags.ev_vel) {
ECL_INFO_TIMESTAMPED("EKF reset velocity to ev velocity");
Vector3f _ev_vel = _ev_sample_delayed.vel; Vector3f _ev_vel = _ev_sample_delayed.vel;
if(_params.fusion_mode & MASK_ROTATE_EV){ if(_params.fusion_mode & MASK_ROTATE_EV){
_ev_vel = _ev_rot_mat *_ev_sample_delayed.vel; _ev_vel = _ev_rot_mat *_ev_sample_delayed.vel;
@ -103,10 +106,8 @@ bool Ekf::resetVelocity()
_state.vel(1) = _ev_vel(1); _state.vel(1) = _ev_vel(1);
_state.vel(2) = _ev_vel(2); _state.vel(2) = _ev_vel(2);
setDiag(P, 4, 6, sq(_ev_sample_delayed.velErr)); setDiag(P, 4, 6, sq(_ev_sample_delayed.velErr));
} else if (_control_status.flags.ev_pos) {
_state.vel.setZero();
zeroOffDiag(P, 4, 6);
} else { } else {
ECL_INFO_TIMESTAMPED("EKF reset velocity to zero");
// Used when falling back to non-aiding mode of operation // Used when falling back to non-aiding mode of operation
_state.vel(0) = 0.0f; _state.vel(0) = 0.0f;
_state.vel(1) = 0.0f; _state.vel(1) = 0.0f;
@ -138,6 +139,7 @@ bool Ekf::resetVelocity()
// gps measurement then use for position initialisation // gps measurement then use for position initialisation
bool Ekf::resetPosition() bool Ekf::resetPosition()
{ {
// ECL_INFO_TIMESTAMPED("Reset Position");
// used to calculate the position change due to the reset // used to calculate the position change due to the reset
Vector2f posNE_before_reset; Vector2f posNE_before_reset;
posNE_before_reset(0) = _state.pos(0); posNE_before_reset(0) = _state.pos(0);
@ -147,6 +149,8 @@ bool Ekf::resetPosition()
_hpos_prev_available = false; _hpos_prev_available = false;
if (_control_status.flags.gps) { 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 // 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(0) = _gps_sample_delayed.pos(0);
_state.pos(1) = _gps_sample_delayed.pos(1); _state.pos(1) = _gps_sample_delayed.pos(1);
@ -155,6 +159,8 @@ bool Ekf::resetPosition()
setDiag(P, 7, 8, sq(_gps_sample_delayed.hacc)); setDiag(P, 7, 8, sq(_gps_sample_delayed.hacc));
} else if (_control_status.flags.ev_pos) { } 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 // this reset is only called if we have new ev data at the fusion time horizon
Vector3f _ev_pos = _ev_sample_delayed.pos; Vector3f _ev_pos = _ev_sample_delayed.pos;
if(_params.fusion_mode & MASK_ROTATE_EV){ if(_params.fusion_mode & MASK_ROTATE_EV){
@ -167,6 +173,8 @@ bool Ekf::resetPosition()
setDiag(P, 7, 8, sq(_ev_sample_delayed.posErr)); setDiag(P, 7, 8, sq(_ev_sample_delayed.posErr));
} else if (_control_status.flags.opt_flow) { } else if (_control_status.flags.opt_flow) {
ECL_INFO_TIMESTAMPED("EKF reset position to last known position");
if (!_control_status.flags.in_air) { if (!_control_status.flags.in_air) {
// we are likely starting OF for the first time so reset the horizontal position // we are likely starting OF for the first time so reset the horizontal position
_state.pos(0) = 0.0f; _state.pos(0) = 0.0f;
@ -184,6 +192,7 @@ bool Ekf::resetPosition()
zeroRows(P,7,8); zeroRows(P,7,8);
} else { } else {
ECL_INFO_TIMESTAMPED("EKF reset position to last known position");
// Used when falling back to non-aiding mode of operation // Used when falling back to non-aiding mode of operation
_state.pos(0) = _last_known_posNE(0); _state.pos(0) = _last_known_posNE(0);
_state.pos(1) = _last_known_posNE(1); _state.pos(1) = _last_known_posNE(1);