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

remove ECL_{INFO,WARN,ERN}_TIMESTAMPED macros

Browse Source 
- we already have mechanisms to print these messages timestamped
This commit is contained in:
Daniel Agar
2021-01-08 17:36:00 -05:00
parent c1f1415696
commit 0f47cae1e1
7 changed files with 26 additions and 43 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EKF/control.cpp
+6 -6
View File
@@ -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");
}
}
EKF/covariance.cpp
+1 -1
View File
@@ -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
EKF/ekf_helper.cpp
+16 -16
View File
@@ -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;
EKF/gps_checks.cpp
+1 -1
View File
@@ -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
EKF/gps_yaw_fusion.cpp
+1 -1
View File
@@ -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;
}
EKF/mag_fusion.cpp
+1 -1
View File
@@ -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;
}
ecl.h
-17
View File
@@ -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 <math.h>