fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-21 16:33:33 +08:00
Code Issues Packages Projects Releases Wiki Activity

Add timestamp to ECL print statements, (#644)

Browse Source 
which is helpful for EKF replay debugging.
This commit is contained in:
kritz
2019-09-25 11:24:42 +02:00
committed by Julian Oes
parent bb88d4ce5e
commit 4a38f5a2f6
11 changed files with 61 additions and 45 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EKF/control.cpp
+20 -20
View File
@@ -71,7 +71,6 @@ void Ekf::controlFusionModes()
} else if (_control_status.flags.rng_hgt) {
ECL_INFO("EKF aligned, (range height, IMU buf: %i, OBS buf: %i)", (int)_imu_buffer_length, (int)_obs_buffer_length);
} else {
ECL_ERR("EKF aligned, (unknown height, IMU buf: %i, OBS buf: %i)", (int)_imu_buffer_length, (int)_obs_buffer_length);
}
@@ -181,14 +180,14 @@ void Ekf::controlExternalVisionFusion()
if (_params.fusion_mode & MASK_USE_EVPOS && !_control_status.flags.ev_pos) {
_control_status.flags.ev_pos = true;
resetPosition();
ECL_INFO("EKF commencing external vision position fusion");
ECL_INFO_TIMESTAMPED("EKF 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("EKF commencing external vision velocity fusion");
ECL_INFO_TIMESTAMPED("EKF commencing external vision velocity fusion");
}
if ((_params.fusion_mode & MASK_ROTATE_EV) && !(_params.fusion_mode & MASK_USE_EVYAW)
@@ -196,6 +195,7 @@ void Ekf::controlExternalVisionFusion()
// Reset transformation between EV and EKF navigation frames when starting fusion
resetExtVisRotMat();
_ev_rot_mat_initialised = true;
ECL_INFO_TIMESTAMPED("EKF external vision aligned");
}
}
}
@@ -254,7 +254,7 @@ void Ekf::controlExternalVisionFusion()
_control_status.flags.mag_3D = false;
}
ECL_INFO("EKF commencing external vision yaw fusion");
ECL_INFO_TIMESTAMPED("EKF commencing external vision yaw fusion");
}
}
@@ -406,7 +406,7 @@ void Ekf::controlExternalVisionFusion()
_control_status.flags.ev_pos = false;
_control_status.flags.ev_vel = false;
_control_status.flags.ev_yaw = false;
ECL_INFO("EKF External Vision Data Stopped");
ECL_INFO_TIMESTAMPED("EKF External Vision Data Stopped");
}
}
@@ -593,7 +593,7 @@ void Ekf::controlGpsFusion()
_control_status.flags.mag_3D = false;
}
ECL_INFO("EKF commencing GPS yaw fusion");
ECL_INFO_TIMESTAMPED("EKF commencing GPS yaw fusion");
}
}
@@ -641,7 +641,7 @@ void Ekf::controlGpsFusion()
}
if (_control_status.flags.gps) {
ECL_INFO("EKF commencing GPS fusion");
ECL_INFO_TIMESTAMPED("EKF commencing GPS fusion");
_time_last_gps = _time_last_imu;
}
}
@@ -659,7 +659,7 @@ void Ekf::controlGpsFusion()
if (_control_status.flags.ev_pos && !(_params.fusion_mode & MASK_ROTATE_EV)) {
resetPosition();
}
ECL_WARN("EKF GPS data quality poor - stopping use");
ECL_WARN_TIMESTAMPED("EKF GPS data quality poor - stopping use");
}
// handle the case when we now have GPS, but have not been using it for an extended period
@@ -684,7 +684,7 @@ void Ekf::controlGpsFusion()
resetVelocity();
resetPosition();
_velpos_reset_request = false;
ECL_WARN("EKF GPS fusion timeout - reset to GPS");
ECL_WARN_TIMESTAMPED("EKF GPS fusion timeout - reset to GPS");
// Reset the timeout counters
_time_last_pos_fuse = _time_last_imu;
@@ -743,12 +743,12 @@ void Ekf::controlGpsFusion()
} else if (_control_status.flags.gps && (_imu_sample_delayed.time_us - _gps_sample_delayed.time_us > (uint64_t)10e6)) {
_control_status.flags.gps = false;
ECL_WARN("EKF GPS data stopped");
ECL_WARN_TIMESTAMPED("EKF 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
_control_status.flags.gps = false;
ECL_WARN("EKF GPS data stopped, using only EV or OF");
ECL_WARN_TIMESTAMPED("EKF GPS data stopped, using only EV or OF");
}
}
@@ -828,7 +828,7 @@ void Ekf::controlHeightSensorTimeouts()
// request a reset
reset_height = true;
ECL_WARN("EKF baro hgt timeout - reset to GPS");
ECL_WARN_TIMESTAMPED("EKF baro hgt timeout - reset to GPS");
} else if (reset_to_baro) {
// set height sensor health
@@ -839,7 +839,7 @@ void Ekf::controlHeightSensorTimeouts()
// request a reset
reset_height = true;
ECL_WARN("EKF baro hgt timeout - reset to baro");
ECL_WARN_TIMESTAMPED("EKF baro hgt timeout - reset to baro");
} else {
// we have nothing we can reset to
@@ -879,7 +879,7 @@ void Ekf::controlHeightSensorTimeouts()
// request a reset
reset_height = true;
ECL_WARN("EKF gps hgt timeout - reset to baro");
ECL_WARN_TIMESTAMPED("EKF gps hgt timeout - reset to baro");
} else if (reset_to_gps) {
// reset the height mode
@@ -887,7 +887,7 @@ void Ekf::controlHeightSensorTimeouts()
// request a reset
reset_height = true;
ECL_WARN("EKF gps hgt timeout - reset to GPS");
ECL_WARN_TIMESTAMPED("EKF gps hgt timeout - reset to GPS");
} else {
// we have nothing to reset to
@@ -922,7 +922,7 @@ void Ekf::controlHeightSensorTimeouts()
// request a reset
reset_height = true;
ECL_WARN("EKF rng hgt timeout - reset to baro");
ECL_WARN_TIMESTAMPED("EKF rng hgt timeout - reset to baro");
} else if (reset_to_rng) {
// set height sensor health
@@ -933,7 +933,7 @@ void Ekf::controlHeightSensorTimeouts()
// request a reset
reset_height = true;
ECL_WARN("EKF rng hgt timeout - reset to rng hgt");
ECL_WARN_TIMESTAMPED("EKF rng hgt timeout - reset to rng hgt");
} else {
// we have nothing to reset to
@@ -967,7 +967,7 @@ void Ekf::controlHeightSensorTimeouts()
// request a reset
reset_height = true;
ECL_WARN("EKF ev hgt timeout - reset to baro");
ECL_WARN_TIMESTAMPED("EKF ev hgt timeout - reset to baro");
} else if (reset_to_ev) {
// reset the height mode
@@ -975,7 +975,7 @@ void Ekf::controlHeightSensorTimeouts()
// request a reset
reset_height = true;
ECL_WARN("EKF ev hgt timeout - reset to ev hgt");
ECL_WARN_TIMESTAMPED("EKF ev hgt timeout - reset to ev hgt");
} else {
// we have nothing to reset to
@@ -1682,7 +1682,7 @@ void Ekf::controlVelPosFusion()
_fuse_hpos_as_odom = false;
if (_time_last_fake_gps != 0) {
ECL_WARN("EKF stopping navigation");
ECL_WARN_TIMESTAMPED("EKF stopping navigation");
}
}