Skip to content

Commit

Permalink
Remove EKF prefix from logged messages
Browse files Browse the repository at this point in the history
  • Loading branch information
@kamilritz @bresch
kamilritz authored and bresch committed Dec 12, 2019
1 parent 88c4929 commit 1bf09fd
Show file tree
Hide file tree
Showing 9 changed files with 54 additions and 53 deletions.
4 changes: 2 additions & 2 deletions EKF/airspeed_fusion.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -103,12 +103,12 @@ void Ekf::fuseAirspeed()
if (update_wind_only) {
resetWindStates();
resetWindCovariance();
ECL_ERR_TIMESTAMPED("EKF airspeed fusion badly conditioned - wind covariance reset");
ECL_ERR_TIMESTAMPED("airspeed fusion badly conditioned - wind covariance reset");

} else {
initialiseCovariance();
_state.wind_vel.setZero();
ECL_ERR_TIMESTAMPED("EKF airspeed fusion badly conditioned - full covariance reset");
ECL_ERR_TIMESTAMPED("airspeed fusion badly conditioned - full covariance reset");
}

return;
Expand Down
40 changes: 20 additions & 20 deletions EKF/control.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -191,22 +191,22 @@ void Ekf::controlExternalVisionFusion()
if (_params.fusion_mode & MASK_USE_EVPOS && !_control_status.flags.ev_pos) {
_control_status.flags.ev_pos = true;
resetPosition();
ECL_INFO_TIMESTAMPED("EKF commencing external vision position fusion");
ECL_INFO_TIMESTAMPED("commencing external vision position fusion");
}

// turn on use of external vision measurements for velocity
if (_params.fusion_mode & MASK_USE_EVVEL && !_control_status.flags.ev_vel) {
_control_status.flags.ev_vel = true;
resetVelocity();
ECL_INFO_TIMESTAMPED("EKF commencing external vision velocity fusion");
ECL_INFO_TIMESTAMPED("commencing external vision velocity fusion");
}

if ((_params.fusion_mode & MASK_ROTATE_EV) && !(_params.fusion_mode & MASK_USE_EVYAW)
&& !_ev_rot_mat_initialised) {
// Reset transformation between EV and EKF navigation frames when starting fusion
resetExtVisRotMat();
_ev_rot_mat_initialised = true;
ECL_INFO_TIMESTAMPED("EKF external vision aligned");
ECL_INFO_TIMESTAMPED("external vision aligned");
}
}
}
Expand Down Expand Up @@ -261,7 +261,7 @@ void Ekf::controlExternalVisionFusion()
stopMagHdgFusion();
stopMag3DFusion();

ECL_INFO_TIMESTAMPED("EKF commencing external vision yaw fusion");
ECL_INFO_TIMESTAMPED("commencing external vision yaw fusion");
}
}

Expand Down Expand Up @@ -394,7 +394,7 @@ void Ekf::controlExternalVisionFusion()

// Turn off EV fusion mode if no data has been received
stopEvFusion();
ECL_INFO_TIMESTAMPED("EKF External Vision Data Stopped");
ECL_INFO_TIMESTAMPED("External Vision Data Stopped");

}
}
Expand Down Expand Up @@ -577,7 +577,7 @@ void Ekf::controlGpsFusion()
stopMagHdgFusion();
stopMag3DFusion();

ECL_INFO_TIMESTAMPED("EKF commencing GPS yaw fusion");
ECL_INFO_TIMESTAMPED("commencing GPS yaw fusion");
}
}

Expand Down Expand Up @@ -628,7 +628,7 @@ void Ekf::controlGpsFusion()
}

if (_control_status.flags.gps) {
ECL_INFO_TIMESTAMPED("EKF commencing GPS fusion");
ECL_INFO_TIMESTAMPED("commencing GPS fusion");
_time_last_gps = _time_last_imu;
}
}
Expand All @@ -646,7 +646,7 @@ void Ekf::controlGpsFusion()
if (_control_status.flags.ev_pos && !(_params.fusion_mode & MASK_ROTATE_EV)) {
resetPosition();
}
ECL_WARN_TIMESTAMPED("EKF GPS data quality poor - stopping use");
ECL_WARN_TIMESTAMPED("GPS data quality poor - stopping use");
}

// handle the case when we now have GPS, but have not been using it for an extended period
Expand All @@ -671,7 +671,7 @@ void Ekf::controlGpsFusion()
resetVelocity();
resetPosition();
_velpos_reset_request = false;
ECL_WARN_TIMESTAMPED("EKF GPS fusion timeout - reset to GPS");
ECL_WARN_TIMESTAMPED("GPS fusion timeout - reset to GPS");

// Reset the timeout counters
_time_last_hor_pos_fuse = _time_last_imu;
Expand Down Expand Up @@ -739,12 +739,12 @@ void Ekf::controlGpsFusion()

} else if (_control_status.flags.gps && (_imu_sample_delayed.time_us - _gps_sample_delayed.time_us > (uint64_t)10e6)) {
stopGpsFusion();
ECL_WARN_TIMESTAMPED("EKF GPS data stopped");
ECL_WARN_TIMESTAMPED("GPS data stopped");
} else if (_control_status.flags.gps && (_imu_sample_delayed.time_us - _gps_sample_delayed.time_us > (uint64_t)1e6) && (_control_status.flags.opt_flow || _control_status.flags.ev_pos || _control_status.flags.ev_vel)) {
// Handle the case where we are fusing another position source along GPS,
// stop waiting for GPS after 1 s of lost signal
stopGpsFusion();
ECL_WARN_TIMESTAMPED("EKF GPS data stopped, using only EV or OF");
ECL_WARN_TIMESTAMPED("GPS data stopped, using only EV or OF");
}
}

Expand Down Expand Up @@ -824,7 +824,7 @@ void Ekf::controlHeightSensorTimeouts()

// request a reset
reset_height = true;
ECL_WARN_TIMESTAMPED("EKF baro hgt timeout - reset to GPS");
ECL_WARN_TIMESTAMPED("baro hgt timeout - reset to GPS");

} else if (reset_to_baro) {
// set height sensor health
Expand All @@ -835,7 +835,7 @@ void Ekf::controlHeightSensorTimeouts()

// request a reset
reset_height = true;
ECL_WARN_TIMESTAMPED("EKF baro hgt timeout - reset to baro");
ECL_WARN_TIMESTAMPED("baro hgt timeout - reset to baro");

} else {
// we have nothing we can reset to
Expand Down Expand Up @@ -875,15 +875,15 @@ void Ekf::controlHeightSensorTimeouts()

// request a reset
reset_height = true;
ECL_WARN_TIMESTAMPED("EKF gps hgt timeout - reset to baro");
ECL_WARN_TIMESTAMPED("gps hgt timeout - reset to baro");

} else if (reset_to_gps) {
// reset the height mode
setControlGPSHeight();

// request a reset
reset_height = true;
ECL_WARN_TIMESTAMPED("EKF gps hgt timeout - reset to GPS");
ECL_WARN_TIMESTAMPED("gps hgt timeout - reset to GPS");

} else {
// we have nothing to reset to
Expand All @@ -909,7 +909,7 @@ void Ekf::controlHeightSensorTimeouts()

// request a reset
reset_height = true;
ECL_WARN_TIMESTAMPED("EKF rng hgt timeout - reset to rng hgt");
ECL_WARN_TIMESTAMPED("rng hgt timeout - reset to rng hgt");

} else if (reset_to_baro) {
// set height sensor health
Expand All @@ -920,7 +920,7 @@ void Ekf::controlHeightSensorTimeouts()

// request a reset
reset_height = true;
ECL_WARN_TIMESTAMPED("EKF rng hgt timeout - reset to baro");
ECL_WARN_TIMESTAMPED("rng hgt timeout - reset to baro");

} else {
// we have nothing to reset to
Expand Down Expand Up @@ -954,15 +954,15 @@ void Ekf::controlHeightSensorTimeouts()

// request a reset
reset_height = true;
ECL_WARN_TIMESTAMPED("EKF ev hgt timeout - reset to baro");
ECL_WARN_TIMESTAMPED("ev hgt timeout - reset to baro");

} else if (reset_to_ev) {
// reset the height mode
setControlEVHeight();

// request a reset
reset_height = true;
ECL_WARN_TIMESTAMPED("EKF ev hgt timeout - reset to ev hgt");
ECL_WARN_TIMESTAMPED("ev hgt timeout - reset to ev hgt");

} else {
// we have nothing to reset to
Expand Down Expand Up @@ -1405,7 +1405,7 @@ void Ekf::controlFakePosFusion()
_fuse_hpos_as_odom = false;

if (_time_last_fake_pos != 0) {
ECL_WARN_TIMESTAMPED("EKF stopping navigation");
ECL_WARN_TIMESTAMPED("stopping navigation");
}

}
Expand Down
2 changes: 1 addition & 1 deletion EKF/covariance.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -853,7 +853,7 @@ void Ekf::fixCovarianceErrors()
P[15][15] = varZ;
_time_acc_bias_check = _time_last_imu;
_fault_status.flags.bad_acc_bias = false;
ECL_WARN_TIMESTAMPED("EKF invalid accel bias - resetting covariance");
ECL_WARN_TIMESTAMPED("invalid accel bias - resetting covariance");

} else {
// ensure the covariance values are symmetrical
Expand Down
20 changes: 10 additions & 10 deletions EKF/ekf_helper.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -54,15 +54,15 @@ 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");
ECL_INFO_TIMESTAMPED("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;

// use GPS accuracy to reset variances
setDiag(P, 4, 6, sq(_gps_sample_delayed.sacc));

} else if (_control_status.flags.opt_flow) {
ECL_INFO_TIMESTAMPED("EKF reset velocity to flow");
ECL_INFO_TIMESTAMPED("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);

Expand Down Expand Up @@ -97,7 +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");
ECL_INFO_TIMESTAMPED("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;
Expand All @@ -107,7 +107,7 @@ bool Ekf::resetVelocity()
_state.vel(2) = _ev_vel(2);
setDiag(P, 4, 6, sq(_ev_sample_delayed.velErr));
} else {
ECL_INFO_TIMESTAMPED("EKF reset velocity to zero");
ECL_INFO_TIMESTAMPED("reset velocity to zero");
// Used when falling back to non-aiding mode of operation
_state.vel(0) = 0.0f;
_state.vel(1) = 0.0f;
Expand Down Expand Up @@ -149,7 +149,7 @@ bool Ekf::resetPosition()
_hpos_prev_available = false;

if (_control_status.flags.gps) {
ECL_INFO_TIMESTAMPED("EKF reset position to GPS");
ECL_INFO_TIMESTAMPED("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);
Expand All @@ -159,7 +159,7 @@ 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");
ECL_INFO_TIMESTAMPED("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;
Expand All @@ -173,7 +173,7 @@ 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");
ECL_INFO_TIMESTAMPED("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
Expand All @@ -192,7 +192,7 @@ bool Ekf::resetPosition()
zeroRows(P,7,8);

} else {
ECL_INFO_TIMESTAMPED("EKF reset position to last known position");
ECL_INFO_TIMESTAMPED("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);
Expand Down Expand Up @@ -439,11 +439,11 @@ bool Ekf::realignYawGPS()

// correct yaw angle using GPS ground course if compass yaw bad or yaw is previously not aligned
if (badMagYaw || !_control_status.flags.yaw_align) {
ECL_WARN_TIMESTAMPED("EKF bad yaw corrected using GPS course");
ECL_WARN_TIMESTAMPED("bad yaw corrected using GPS course");

// declare the magnetometer as failed if a bad yaw has occurred more than once
if (_control_status.flags.mag_aligned_in_flight && (_num_bad_flight_yaw_events >= 2) && !_control_status.flags.mag_fault) {
ECL_WARN_TIMESTAMPED("EKF stopping magnetometer use");
ECL_WARN_TIMESTAMPED("stopping magnetometer use");
_control_status.flags.mag_fault = true;
}

Expand Down
20 changes: 10 additions & 10 deletions EKF/estimator_interface.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -104,7 +104,7 @@ void EstimatorInterface::setIMUData(const imuSample &imu_sample)
_drag_buffer_fail = !_drag_buffer.allocate(_obs_buffer_length);

if (_drag_buffer_fail) {
ECL_ERR_TIMESTAMPED("EKF drag buffer allocation failed");
ECL_ERR_TIMESTAMPED("drag buffer allocation failed");
return;
}
}
Expand Down Expand Up @@ -170,7 +170,7 @@ void EstimatorInterface::setMagData(uint64_t time_usec, float (&data)[3])
_mag_buffer_fail = !_mag_buffer.allocate(_obs_buffer_length);

if (_mag_buffer_fail) {
ECL_ERR_TIMESTAMPED("EKF mag buffer allocation failed");
ECL_ERR_TIMESTAMPED("mag buffer allocation failed");
return;
}
}
Expand Down Expand Up @@ -202,7 +202,7 @@ void EstimatorInterface::setGpsData(uint64_t time_usec, const gps_message &gps)
_gps_buffer_fail = !_gps_buffer.allocate(_obs_buffer_length);

if (_gps_buffer_fail) {
ECL_ERR_TIMESTAMPED("EKF GPS buffer allocation failed");
ECL_ERR_TIMESTAMPED("GPS buffer allocation failed");
return;
}
}
Expand Down Expand Up @@ -263,7 +263,7 @@ void EstimatorInterface::setBaroData(uint64_t time_usec, float data)
_baro_buffer_fail = !_baro_buffer.allocate(_obs_buffer_length);

if (_baro_buffer_fail) {
ECL_ERR_TIMESTAMPED("EKF baro buffer allocation failed");
ECL_ERR_TIMESTAMPED("baro buffer allocation failed");
return;
}
}
Expand Down Expand Up @@ -296,7 +296,7 @@ void EstimatorInterface::setAirspeedData(uint64_t time_usec, float true_airspeed
_airspeed_buffer_fail = !_airspeed_buffer.allocate(_obs_buffer_length);

if (_airspeed_buffer_fail) {
ECL_ERR_TIMESTAMPED("EKF airspeed buffer allocation failed");
ECL_ERR_TIMESTAMPED("airspeed buffer allocation failed");
return;
}
}
Expand Down Expand Up @@ -326,7 +326,7 @@ void EstimatorInterface::setRangeData(uint64_t time_usec, float data, int8_t qua
_range_buffer_fail = !_range_buffer.allocate(_obs_buffer_length);

if (_range_buffer_fail) {
ECL_ERR_TIMESTAMPED("EKF range finder buffer allocation failed");
ECL_ERR_TIMESTAMPED("range finder buffer allocation failed");
return;
}
}
Expand Down Expand Up @@ -356,7 +356,7 @@ void EstimatorInterface::setOpticalFlowData(uint64_t time_usec, flow_message *fl
_flow_buffer_fail = !_flow_buffer.allocate(_imu_buffer_length);

if (_flow_buffer_fail) {
ECL_ERR_TIMESTAMPED("EKF optical flow buffer allocation failed");
ECL_ERR_TIMESTAMPED("optical flow buffer allocation failed");
return;
}
}
Expand Down Expand Up @@ -428,7 +428,7 @@ void EstimatorInterface::setExtVisionData(uint64_t time_usec, ext_vision_message
_ev_buffer_fail = !_ext_vision_buffer.allocate(_obs_buffer_length);

if (_ev_buffer_fail) {
ECL_ERR_TIMESTAMPED("EKF external vision buffer allocation failed");
ECL_ERR_TIMESTAMPED("external vision buffer allocation failed");
return;
}
}
Expand Down Expand Up @@ -468,7 +468,7 @@ void EstimatorInterface::setAuxVelData(uint64_t time_usec, float (&data)[2], flo
_auxvel_buffer_fail = !_auxvel_buffer.allocate(_obs_buffer_length);

if (_auxvel_buffer_fail) {
ECL_ERR_TIMESTAMPED("EKF aux vel buffer allocation failed");
ECL_ERR_TIMESTAMPED("aux vel buffer allocation failed");
return;
}
}
Expand Down Expand Up @@ -517,7 +517,7 @@ bool EstimatorInterface::initialise_interface(uint64_t timestamp)
_output_buffer.allocate(_imu_buffer_length) &&
_output_vert_buffer.allocate(_imu_buffer_length))) {

ECL_ERR_TIMESTAMPED("EKF buffer allocation failed!");
ECL_ERR_TIMESTAMPED("buffer allocation failed!");
unallocate_buffers();
return false;
}
Expand Down
7 changes: 4 additions & 3 deletions EKF/gps_checks.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -90,13 +90,14 @@ bool Ekf::collect_gps(const gps_message &gps)
_gps_origin_epv = gps.epv;

// if the user has selected GPS as the primary height source, switch across to using it

if (_params.vdist_sensor_type == VDIST_SENSOR_GPS) {
ECL_INFO_TIMESTAMPED("EKF GPS checks passed (WGS-84 origin set, using GPS height)");
setControlGPSHeight();
ECL_INFO_TIMESTAMPED("GPS checks passed (WGS-84 origin set, using GPS height)");
setControlGPSHeight();
// zero the sensor offset
_hgt_sensor_offset = 0.0f;
} else {
ECL_INFO_TIMESTAMPED("EKF GPS checks passed (WGS-84 origin set)");
ECL_INFO_TIMESTAMPED("GPS checks passed (WGS-84 origin set)");
}
}

Expand Down
Loading

0 comments on commit 1bf09fd

Please sign in to comment.