From 4a38f5a2f65d0852394d568a639e736ea5372939 Mon Sep 17 00:00:00 2001 From: kritz Date: Wed, 25 Sep 2019 11:24:42 +0200 Subject: [PATCH] Add timestamp to ECL print statements, (#644) which is helpful for EKF replay debugging. --- EKF/airspeed_fusion.cpp | 4 ++-- EKF/control.cpp | 40 ++++++++++++++++++------------------- EKF/covariance.cpp | 2 +- EKF/ekf.cpp | 2 +- EKF/ekf_helper.cpp | 4 ++-- EKF/estimator_interface.cpp | 20 +++++++++---------- EKF/gps_checks.cpp | 4 ++-- EKF/gps_yaw_fusion.cpp | 2 +- EKF/mag_fusion.cpp | 8 ++++---- EKF/sideslip_fusion.cpp | 4 ++-- ecl.h | 16 +++++++++++++++ 11 files changed, 61 insertions(+), 45 deletions(-) diff --git a/EKF/airspeed_fusion.cpp b/EKF/airspeed_fusion.cpp index 730f132024..a5b322b42b 100644 --- a/EKF/airspeed_fusion.cpp +++ b/EKF/airspeed_fusion.cpp @@ -103,12 +103,12 @@ void Ekf::fuseAirspeed() if (update_wind_only) { resetWindStates(); resetWindCovariance(); - ECL_ERR("EKF airspeed fusion badly conditioned - wind covariance reset"); + ECL_ERR_TIMESTAMPED("EKF airspeed fusion badly conditioned - wind covariance reset"); } else { initialiseCovariance(); _state.wind_vel.setZero(); - ECL_ERR("EKF airspeed fusion badly conditioned - full covariance reset"); + ECL_ERR_TIMESTAMPED("EKF airspeed fusion badly conditioned - full covariance reset"); } return; diff --git a/EKF/control.cpp b/EKF/control.cpp index 76e9443bfa..6db7dc1dc2 100644 --- a/EKF/control.cpp +++ b/EKF/control.cpp @@ -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"); } } diff --git a/EKF/covariance.cpp b/EKF/covariance.cpp index 7af7e39332..76e56e5d10 100644 --- a/EKF/covariance.cpp +++ b/EKF/covariance.cpp @@ -852,7 +852,7 @@ void Ekf::fixCovarianceErrors() P[15][15] = varZ; _time_acc_bias_check = _time_last_imu; _fault_status.flags.bad_acc_bias = false; - ECL_WARN("EKF invalid accel bias - resetting covariance"); + ECL_WARN_TIMESTAMPED("EKF invalid accel bias - resetting covariance"); } else { // ensure the covariance values are symmetrical diff --git a/EKF/ekf.cpp b/EKF/ekf.cpp index 2df552d665..7e6ffe23ac 100644 --- a/EKF/ekf.cpp +++ b/EKF/ekf.cpp @@ -274,7 +274,7 @@ bool Ekf::initialiseFilter() const baroSample &baro_newest = _baro_buffer.get_newest(); _baro_hgt_offset = baro_newest.hgt; _state.pos(2) = -math::max(_rng_filt_state * _R_rng_to_earth_2_2, _params.rng_gnd_clearance); - ECL_INFO("EKF using range finder height - commencing alignment"); + ECL_INFO_TIMESTAMPED("EKF using range finder height - commencing alignment"); } else if (_control_status.flags.ev_hgt) { // if we are using external vision data for height, then the vertical position state needs to be reset diff --git a/EKF/ekf_helper.cpp b/EKF/ekf_helper.cpp index 19d2f3aa21..eafd8b767a 100644 --- a/EKF/ekf_helper.cpp +++ b/EKF/ekf_helper.cpp @@ -442,11 +442,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("EKF bad yaw corrected using GPS course"); + ECL_WARN_TIMESTAMPED("EKF 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_align_complete && (_num_bad_flight_yaw_events >= 2) && !_control_status.flags.mag_fault) { - ECL_WARN("EKF stopping magnetometer use"); + ECL_WARN_TIMESTAMPED("EKF stopping magnetometer use"); _control_status.flags.mag_fault = true; } diff --git a/EKF/estimator_interface.cpp b/EKF/estimator_interface.cpp index 7b01c84645..e10130d7db 100644 --- a/EKF/estimator_interface.cpp +++ b/EKF/estimator_interface.cpp @@ -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("EKF drag buffer allocation failed"); + ECL_ERR_TIMESTAMPED("EKF drag buffer allocation failed"); return; } } @@ -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("EKF mag buffer allocation failed"); + ECL_ERR_TIMESTAMPED("EKF mag buffer allocation failed"); return; } } @@ -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("EKF GPS buffer allocation failed"); + ECL_ERR_TIMESTAMPED("EKF GPS buffer allocation failed"); return; } } @@ -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("EKF baro buffer allocation failed"); + ECL_ERR_TIMESTAMPED("EKF baro buffer allocation failed"); return; } } @@ -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("EKF airspeed buffer allocation failed"); + ECL_ERR_TIMESTAMPED("EKF airspeed buffer allocation failed"); return; } } @@ -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("EKF range finder buffer allocation failed"); + ECL_ERR_TIMESTAMPED("EKF range finder buffer allocation failed"); return; } } @@ -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("EKF optical flow buffer allocation failed"); + ECL_ERR_TIMESTAMPED("EKF optical flow buffer allocation failed"); return; } } @@ -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("EKF external vision buffer allocation failed"); + ECL_ERR_TIMESTAMPED("EKF external vision buffer allocation failed"); return; } } @@ -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("EKF aux vel buffer allocation failed"); + ECL_ERR_TIMESTAMPED("EKF aux vel buffer allocation failed"); return; } } @@ -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("EKF buffer allocation failed!"); + ECL_ERR_TIMESTAMPED("EKF buffer allocation failed!"); unallocate_buffers(); return false; } diff --git a/EKF/gps_checks.cpp b/EKF/gps_checks.cpp index d0abd3c878..43f39ee0e2 100644 --- a/EKF/gps_checks.cpp +++ b/EKF/gps_checks.cpp @@ -91,14 +91,14 @@ bool Ekf::collect_gps(const gps_message &gps) // if the user has selected GPS as the primary height source, switch across to using it if (_primary_hgt_source == VDIST_SENSOR_GPS) { - ECL_INFO("EKF GPS checks passed (WGS-84 origin set, using GPS height)"); + ECL_INFO_TIMESTAMPED("EKF GPS checks passed (WGS-84 origin set, using GPS height)"); _control_status.flags.baro_hgt = false; _control_status.flags.gps_hgt = true; _control_status.flags.rng_hgt = false; // zero the sensor offset _hgt_sensor_offset = 0.0f; } else { - ECL_INFO("EKF GPS checks passed (WGS-84 origin set)"); + ECL_INFO_TIMESTAMPED("EKF GPS checks passed (WGS-84 origin set)"); } } diff --git a/EKF/gps_yaw_fusion.cpp b/EKF/gps_yaw_fusion.cpp index 007a40f08d..13d13415c4 100644 --- a/EKF/gps_yaw_fusion.cpp +++ b/EKF/gps_yaw_fusion.cpp @@ -174,7 +174,7 @@ void Ekf::fuseGpsAntYaw() // we reinitialise the covariance matrix and abort this fusion step initialiseCovariance(); - ECL_ERR("EKF GPS yaw fusion numerical error - covariance reset"); + ECL_ERR_TIMESTAMPED("EKF GPS yaw fusion numerical error - covariance reset"); return; } diff --git a/EKF/mag_fusion.cpp b/EKF/mag_fusion.cpp index 89e1eb8323..355384b398 100644 --- a/EKF/mag_fusion.cpp +++ b/EKF/mag_fusion.cpp @@ -101,7 +101,7 @@ void Ekf::fuseMag() // we need to re-initialise covariances and abort this fusion step resetMagCovariance(); - ECL_ERR("EKF magX fusion numerical error - covariance reset"); + ECL_ERR_TIMESTAMPED("EKF magX fusion numerical error - covariance reset"); return; } @@ -120,7 +120,7 @@ void Ekf::fuseMag() // we need to re-initialise covariances and abort this fusion step resetMagCovariance(); - ECL_ERR("EKF magY fusion numerical error - covariance reset"); + ECL_ERR_TIMESTAMPED("EKF magY fusion numerical error - covariance reset"); return; } @@ -139,7 +139,7 @@ void Ekf::fuseMag() // we need to re-initialise covariances and abort this fusion step resetMagCovariance(); - ECL_ERR("EKF magZ fusion numerical error - covariance reset"); + ECL_ERR_TIMESTAMPED("EKF magZ fusion numerical error - covariance reset"); return; } @@ -677,7 +677,7 @@ void Ekf::fuseHeading() // we reinitialise the covariance matrix and abort this fusion step initialiseCovariance(); - ECL_ERR("EKF mag yaw fusion numerical error - covariance reset"); + ECL_ERR_TIMESTAMPED("EKF mag yaw fusion numerical error - covariance reset"); return; } diff --git a/EKF/sideslip_fusion.cpp b/EKF/sideslip_fusion.cpp index 583e5b4f8d..cdeb269c5a 100644 --- a/EKF/sideslip_fusion.cpp +++ b/EKF/sideslip_fusion.cpp @@ -133,12 +133,12 @@ void Ekf::fuseSideslip() if (update_wind_only) { resetWindStates(); resetWindCovariance(); - ECL_ERR("EKF synthetic sideslip fusion badly conditioned - wind covariance reset"); + ECL_ERR_TIMESTAMPED("EKF synthetic sideslip fusion badly conditioned - wind covariance reset"); } else { initialiseCovariance(); _state.wind_vel.setZero(); - ECL_ERR("EKF synthetic sideslip fusion badly conditioned - full covariance reset"); + ECL_ERR_TIMESTAMPED("EKF synthetic sideslip fusion badly conditioned - full covariance reset"); } return; diff --git a/ecl.h b/ecl.h index d9244ef129..656a697077 100644 --- a/ecl.h +++ b/ecl.h @@ -51,6 +51,16 @@ using ecl_abstime = hrt_abstime; #define ECL_WARN PX4_WARN #define ECL_ERR PX4_ERR +#if defined(__PX4_POSIX) +#define ECL_INFO_TIMESTAMPED(X) PX4_INFO("%lu: " X, _imu_sample_delayed.time_us) +#define ECL_WARN_TIMESTAMPED(X) PX4_WARN("%lu: " X, _imu_sample_delayed.time_us) +#define ECL_ERR_TIMESTAMPED(X) PX4_ERR("%lu: " X, _imu_sample_delayed.time_us) +#else +#define ECL_INFO_TIMESTAMPED PX4_INFO +#define ECL_WARN_TIMESTAMPED PX4_WARN +#define ECL_ERR_TIMESTAMPED PX4_ERR +#endif + #elif defined(__PAPARAZZI) #include "std.h" @@ -63,6 +73,9 @@ using ecl_abstime = uint64_t; #define ECL_INFO(...) #define ECL_WARN(...) #define ECL_ERR(...) +#define ECL_INFO_TIMESTAMPED PX4_INFO +#define ECL_WARN_TIMESTAMPED PX4_WARN +#define ECL_ERR_TIMESTAMPED PX4_ERR #else @@ -77,6 +90,9 @@ using ecl_abstime = uint64_t; #define ECL_INFO printf #define ECL_WARN printf #define ECL_ERR printf +#define ECL_INFO_TIMESTAMPED printf +#define ECL_WARN_TIMESTAMPED printf +#define ECL_ERR_TIMESTAMPED printf #endif /* PX4_POSIX || PX4_NUTTX */