Add timestamp to ECL print statements, (#644)

which is helpful for EKF replay debugging.

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;

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");
 				}
 
 			}

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

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

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;
 			}
 

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;
 	}

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)");
 		}
 	}
 

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;
 	}
 

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;
 	}
 

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;

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 */