From 0f47cae1e1d6587866c8ba25090ee2daf69a5c11 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Fri, 8 Jan 2021 17:36:00 -0500 Subject: [PATCH] remove ECL_{INFO,WARN,ERN}_TIMESTAMPED macros - we already have mechanisms to print these messages timestamped --- EKF/control.cpp | 12 ++++++------ EKF/covariance.cpp | 2 +- EKF/ekf_helper.cpp | 32 ++++++++++++++++---------------- EKF/gps_checks.cpp | 2 +- EKF/gps_yaw_fusion.cpp | 2 +- EKF/mag_fusion.cpp | 2 +- ecl.h | 17 ----------------- 7 files changed, 26 insertions(+), 43 deletions(-) diff --git a/EKF/control.cpp b/EKF/control.cpp index 5f3e3cbf69..2a354f56a5 100644 --- a/EKF/control.cpp +++ b/EKF/control.cpp @@ -337,7 +337,7 @@ void Ekf::controlExternalVisionFusion() // Turn off EV fusion mode if no data has been received stopEvFusion(); _warning_events.flags.vision_data_stopped = true; - ECL_WARN_TIMESTAMPED("vision data stopped"); + ECL_WARN("vision data stopped"); } } @@ -554,7 +554,7 @@ void Ekf::controlGpsFusion() resetHorizontalPosition(); } _warning_events.flags.gps_quality_poor = true; - ECL_WARN_TIMESTAMPED("GPS quality poor - stopping use"); + ECL_WARN("GPS quality poor - stopping use"); } // handle case where we are not currently using GPS, but need to align yaw angle using EKF-GSF before @@ -662,7 +662,7 @@ void Ekf::controlGpsFusion() resetHorizontalPosition(); _velpos_reset_request = false; _warning_events.flags.gps_fusion_timout = true; - ECL_WARN_TIMESTAMPED("GPS fusion timeout - reset to GPS"); + ECL_WARN("GPS fusion timeout - reset to GPS"); // Reset the timeout counters _time_last_hor_pos_fuse = _time_last_imu; @@ -723,13 +723,13 @@ void Ekf::controlGpsFusion() } else if (_control_status.flags.gps && (_imu_sample_delayed.time_us - _gps_sample_delayed.time_us > (uint64_t)10e6)) { stopGpsFusion(); _warning_events.flags.gps_data_stopped = true; - ECL_WARN_TIMESTAMPED("GPS data stopped"); + ECL_WARN("GPS data stopped"); } else if (_control_status.flags.gps && (_imu_sample_delayed.time_us - _gps_sample_delayed.time_us > (uint64_t)1e6) && isOtherSourceOfHorizontalAidingThan(_control_status.flags.gps)) { // Handle the case where we are fusing another position source along GPS, // stop waiting for GPS after 1 s of lost signal stopGpsFusion(); _warning_events.flags.gps_data_stopped_using_alternate = true; - ECL_WARN_TIMESTAMPED("GPS data stopped, using only EV, OF or air data" ); + ECL_WARN("GPS data stopped, using only EV, OF or air data" ); } } @@ -1300,7 +1300,7 @@ void Ekf::controlFakePosFusion() if (_time_last_fake_pos != 0) { _warning_events.flags.stopping_navigation = true; - ECL_WARN_TIMESTAMPED("stopping navigation"); + ECL_WARN("stopping navigation"); } } diff --git a/EKF/covariance.cpp b/EKF/covariance.cpp index 5097730393..94bafced3e 100644 --- a/EKF/covariance.cpp +++ b/EKF/covariance.cpp @@ -972,7 +972,7 @@ void Ekf::fixCovarianceErrors(bool force_symmetry) _time_acc_bias_check = _time_last_imu; _fault_status.flags.bad_acc_bias = false; _warning_events.flags.invalid_accel_bias_cov_reset = true; - ECL_WARN_TIMESTAMPED("invalid accel bias - covariance reset"); + ECL_WARN("invalid accel bias - covariance reset"); } else if (force_symmetry) { // ensure the covariance values are symmetrical diff --git a/EKF/ekf_helper.cpp b/EKF/ekf_helper.cpp index ff43d0b263..24e8c878fa 100644 --- a/EKF/ekf_helper.cpp +++ b/EKF/ekf_helper.cpp @@ -66,7 +66,7 @@ void Ekf::resetVelocity() void Ekf::resetVelocityToGps() { _information_events.flags.reset_vel_to_gps = true; - ECL_INFO_TIMESTAMPED("reset velocity to GPS"); + ECL_INFO("reset velocity to GPS"); resetVelocityTo(_gps_sample_delayed.vel); P.uncorrelateCovarianceSetVariance<3>(4, sq(_gps_sample_delayed.sacc)); } @@ -74,7 +74,7 @@ void Ekf::resetVelocityToGps() void Ekf::resetHorizontalVelocityToOpticalFlow() { _information_events.flags.reset_vel_to_flow = true; - ECL_INFO_TIMESTAMPED("reset velocity to flow"); + ECL_INFO("reset velocity to flow"); // constrain height above ground to be above minimum possible const float heightAboveGndEst = fmaxf((_terrain_vpos - _state.pos(2)), _params.rng_gnd_clearance); @@ -105,7 +105,7 @@ void Ekf::resetHorizontalVelocityToOpticalFlow() void Ekf::resetVelocityToVision() { _information_events.flags.reset_vel_to_vision = true; - ECL_INFO_TIMESTAMPED("reset to vision velocity"); + ECL_INFO("reset to vision velocity"); resetVelocityTo(getVisionVelocityInEkfFrame()); P.uncorrelateCovarianceSetVariance<3>(4, getVisionVelocityVarianceInEkfFrame()); } @@ -113,7 +113,7 @@ void Ekf::resetVelocityToVision() void Ekf::resetHorizontalVelocityToZero() { _information_events.flags.reset_vel_to_zero = true; - ECL_INFO_TIMESTAMPED("reset velocity to zero"); + ECL_INFO("reset velocity to zero"); // Used when falling back to non-aiding mode of operation resetHorizontalVelocityTo(Vector2f{0.f, 0.f}); P.uncorrelateCovarianceSetVariance<2>(4, 25.0f); @@ -172,7 +172,7 @@ void Ekf::resetHorizontalPosition() } else if (_control_status.flags.opt_flow) { _information_events.flags.reset_pos_to_last_known = true; - ECL_INFO_TIMESTAMPED("reset position to last known position"); + ECL_INFO("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 @@ -187,7 +187,7 @@ void Ekf::resetHorizontalPosition() } else { _information_events.flags.reset_pos_to_last_known = true; - ECL_INFO_TIMESTAMPED("reset position to last known position"); + ECL_INFO("reset position to last known position"); // Used when falling back to non-aiding mode of operation resetHorizontalPositionTo(_last_known_posNE); P.uncorrelateCovarianceSetVariance<2>(7, sq(_params.pos_noaid_noise)); @@ -197,7 +197,7 @@ void Ekf::resetHorizontalPosition() void Ekf::resetHorizontalPositionToGps() { _information_events.flags.reset_pos_to_gps = true; - ECL_INFO_TIMESTAMPED("reset position to GPS"); + ECL_INFO("reset position to GPS"); resetHorizontalPositionTo(_gps_sample_delayed.pos); P.uncorrelateCovarianceSetVariance<2>(7, sq(_gps_sample_delayed.hacc)); } @@ -205,7 +205,7 @@ void Ekf::resetHorizontalPositionToGps() void Ekf::resetHorizontalPositionToVision() { _information_events.flags.reset_pos_to_vision = true; - ECL_INFO_TIMESTAMPED("reset position to ev position"); + ECL_INFO("reset position to ev position"); Vector3f _ev_pos = _ev_sample_delayed.pos; if (_params.fusion_mode & MASK_ROTATE_EV) { @@ -395,13 +395,13 @@ 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) { _warning_events.flags.bad_yaw_using_gps_course = true; - ECL_WARN_TIMESTAMPED("bad yaw, using GPS course"); + ECL_WARN("bad yaw, 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) { _warning_events.flags.stopping_mag_use = true; - ECL_WARN_TIMESTAMPED("stopping mag use"); + ECL_WARN("stopping mag use"); _control_status.flags.mag_fault = true; } @@ -1424,7 +1424,7 @@ void Ekf::startGpsFusion() } _information_events.flags.starting_gps_fusion = true; - ECL_INFO_TIMESTAMPED("starting GPS fusion"); + ECL_INFO("starting GPS fusion"); _control_status.flags.gps = true; } @@ -1470,7 +1470,7 @@ void Ekf::startEvPosFusion() _control_status.flags.ev_pos = true; resetHorizontalPosition(); _information_events.flags.starting_vision_pos_fusion = true; - ECL_INFO_TIMESTAMPED("starting vision pos fusion"); + ECL_INFO("starting vision pos fusion"); } void Ekf::startEvVelFusion() @@ -1478,7 +1478,7 @@ void Ekf::startEvVelFusion() _control_status.flags.ev_vel = true; resetVelocity(); _information_events.flags.starting_vision_vel_fusion = true; - ECL_INFO_TIMESTAMPED("starting vision vel fusion"); + ECL_INFO("starting vision vel fusion"); } void Ekf::startEvYawFusion() @@ -1500,7 +1500,7 @@ void Ekf::startEvYawFusion() stopMag3DFusion(); _information_events.flags.starting_vision_yaw_fusion = true; - ECL_INFO_TIMESTAMPED("starting vision yaw fusion"); + ECL_INFO("starting vision yaw fusion"); } void Ekf::stopEvFusion() @@ -1620,14 +1620,14 @@ bool Ekf::resetYawToEKFGSF() if (_params.mag_fusion_type == MAG_FUSE_TYPE_NONE) { _information_events.flags.yaw_aligned_to_imu_gps = true; - ECL_INFO_TIMESTAMPED("Yaw aligned using IMU and GPS"); + ECL_INFO("Yaw aligned using IMU and GPS"); } else { // stop using the magnetometer in the main EKF otherwise it's fusion could drag the yaw around // and cause another navigation failure _control_status.flags.mag_fault = true; _warning_events.flags.emergency_yaw_reset_mag_stopped = true; - ECL_WARN_TIMESTAMPED("Emergency yaw reset - mag use stopped"); + ECL_WARN("Emergency yaw reset - mag use stopped"); } return true; diff --git a/EKF/gps_checks.cpp b/EKF/gps_checks.cpp index 1527e4d59d..c2fdca102c 100644 --- a/EKF/gps_checks.cpp +++ b/EKF/gps_checks.cpp @@ -113,7 +113,7 @@ bool Ekf::collect_gps(const gps_message &gps) } _information_events.flags.gps_checks_passed = true; - ECL_INFO_TIMESTAMPED("GPS checks passed"); + ECL_INFO("GPS checks passed"); } else if (!_NED_origin_initialised) { // a rough 2D fix is still sufficient to lookup declination diff --git a/EKF/gps_yaw_fusion.cpp b/EKF/gps_yaw_fusion.cpp index 237bd52652..dbf3205318 100644 --- a/EKF/gps_yaw_fusion.cpp +++ b/EKF/gps_yaw_fusion.cpp @@ -128,7 +128,7 @@ void Ekf::fuseGpsYaw() // we reinitialise the covariance matrix and abort this fusion step initialiseCovariance(); - ECL_ERR_TIMESTAMPED("GPS yaw numerical error - covariance reset"); + ECL_ERR("GPS yaw numerical error - covariance reset"); return; } diff --git a/EKF/mag_fusion.cpp b/EKF/mag_fusion.cpp index 2b5d73d1d5..9d3a55aa2e 100644 --- a/EKF/mag_fusion.cpp +++ b/EKF/mag_fusion.cpp @@ -607,7 +607,7 @@ void Ekf::updateQuaternion(const float innovation, const float variance, const f // we reinitialise the covariance matrix and abort this fusion step initialiseCovariance(); - ECL_ERR_TIMESTAMPED("mag yaw fusion numerical error - covariance reset"); + ECL_ERR("mag yaw fusion numerical error - covariance reset"); return; } diff --git a/ecl.h b/ecl.h index 653cca788b..2cd4f49bd2 100644 --- a/ecl.h +++ b/ecl.h @@ -51,16 +51,6 @@ 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("%llu: " X, (unsigned long long)_imu_sample_delayed.time_us) -#define ECL_WARN_TIMESTAMPED(X) PX4_WARN("%llu: " X, (unsigned long long)_imu_sample_delayed.time_us) -#define ECL_ERR_TIMESTAMPED(X) PX4_ERR("%llu: " X, (unsigned long long)_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" @@ -73,9 +63,6 @@ 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 @@ -91,10 +78,6 @@ using ecl_abstime = uint64_t; #define ECL_WARN(X, ...) fprintf(stderr, X "\n", ##__VA_ARGS__) #define ECL_ERR(X, ...) fprintf(stderr, X "\n", ##__VA_ARGS__) -#define ECL_INFO_TIMESTAMPED(X) ECL_INFO("%llu: " X, (unsigned long long)_imu_sample_delayed.time_us) -#define ECL_WARN_TIMESTAMPED(X) ECL_WARN("%llu: " X, (unsigned long long)_imu_sample_delayed.time_us) -#define ECL_ERR_TIMESTAMPED(X) ECL_ERR("%llu: " X, (unsigned long long)_imu_sample_delayed.time_us) - #endif /* PX4_POSIX || PX4_NUTTX */ #include