diff --git a/EKF/airspeed_fusion.cpp b/EKF/airspeed_fusion.cpp index a5b322b42b1d..4e45ae3c48fc 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_TIMESTAMPED("EKF airspeed fusion badly conditioned - wind covariance reset"); + ECL_ERR_TIMESTAMPED("airspeed fusion badly conditioned - wind covariance reset"); } else { initialiseCovariance(); _state.wind_vel.setZero(); - ECL_ERR_TIMESTAMPED("EKF airspeed fusion badly conditioned - full covariance reset"); + ECL_ERR_TIMESTAMPED("airspeed fusion badly conditioned - full covariance reset"); } return; diff --git a/EKF/control.cpp b/EKF/control.cpp index cf2c40b1afce..a9e0e2d33519 100644 --- a/EKF/control.cpp +++ b/EKF/control.cpp @@ -191,14 +191,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_TIMESTAMPED("EKF commencing external vision position fusion"); + ECL_INFO_TIMESTAMPED("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_TIMESTAMPED("EKF commencing external vision velocity fusion"); + ECL_INFO_TIMESTAMPED("commencing external vision velocity fusion"); } if ((_params.fusion_mode & MASK_ROTATE_EV) && !(_params.fusion_mode & MASK_USE_EVYAW) @@ -206,7 +206,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"); + ECL_INFO_TIMESTAMPED("external vision aligned"); } } } @@ -261,7 +261,7 @@ void Ekf::controlExternalVisionFusion() stopMagHdgFusion(); stopMag3DFusion(); - ECL_INFO_TIMESTAMPED("EKF commencing external vision yaw fusion"); + ECL_INFO_TIMESTAMPED("commencing external vision yaw fusion"); } } @@ -394,7 +394,7 @@ void Ekf::controlExternalVisionFusion() // Turn off EV fusion mode if no data has been received stopEvFusion(); - ECL_INFO_TIMESTAMPED("EKF External Vision Data Stopped"); + ECL_INFO_TIMESTAMPED("External Vision Data Stopped"); } } @@ -577,7 +577,7 @@ void Ekf::controlGpsFusion() stopMagHdgFusion(); stopMag3DFusion(); - ECL_INFO_TIMESTAMPED("EKF commencing GPS yaw fusion"); + ECL_INFO_TIMESTAMPED("commencing GPS yaw fusion"); } } @@ -628,7 +628,7 @@ void Ekf::controlGpsFusion() } if (_control_status.flags.gps) { - ECL_INFO_TIMESTAMPED("EKF commencing GPS fusion"); + ECL_INFO_TIMESTAMPED("commencing GPS fusion"); _time_last_gps = _time_last_imu; } } @@ -646,7 +646,7 @@ void Ekf::controlGpsFusion() if (_control_status.flags.ev_pos && !(_params.fusion_mode & MASK_ROTATE_EV)) { resetPosition(); } - ECL_WARN_TIMESTAMPED("EKF GPS data quality poor - stopping use"); + ECL_WARN_TIMESTAMPED("GPS data quality poor - stopping use"); } // handle the case when we now have GPS, but have not been using it for an extended period @@ -671,7 +671,7 @@ void Ekf::controlGpsFusion() resetVelocity(); resetPosition(); _velpos_reset_request = false; - ECL_WARN_TIMESTAMPED("EKF GPS fusion timeout - reset to GPS"); + ECL_WARN_TIMESTAMPED("GPS fusion timeout - reset to GPS"); // Reset the timeout counters _time_last_hor_pos_fuse = _time_last_imu; @@ -739,12 +739,12 @@ void Ekf::controlGpsFusion() } else if (_control_status.flags.gps && (_imu_sample_delayed.time_us - _gps_sample_delayed.time_us > (uint64_t)10e6)) { stopGpsFusion(); - ECL_WARN_TIMESTAMPED("EKF GPS data stopped"); + ECL_WARN_TIMESTAMPED("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 stopGpsFusion(); - ECL_WARN_TIMESTAMPED("EKF GPS data stopped, using only EV or OF"); + ECL_WARN_TIMESTAMPED("GPS data stopped, using only EV or OF"); } } @@ -824,7 +824,7 @@ void Ekf::controlHeightSensorTimeouts() // request a reset reset_height = true; - ECL_WARN_TIMESTAMPED("EKF baro hgt timeout - reset to GPS"); + ECL_WARN_TIMESTAMPED("baro hgt timeout - reset to GPS"); } else if (reset_to_baro) { // set height sensor health @@ -835,7 +835,7 @@ void Ekf::controlHeightSensorTimeouts() // request a reset reset_height = true; - ECL_WARN_TIMESTAMPED("EKF baro hgt timeout - reset to baro"); + ECL_WARN_TIMESTAMPED("baro hgt timeout - reset to baro"); } else { // we have nothing we can reset to @@ -875,7 +875,7 @@ void Ekf::controlHeightSensorTimeouts() // request a reset reset_height = true; - ECL_WARN_TIMESTAMPED("EKF gps hgt timeout - reset to baro"); + ECL_WARN_TIMESTAMPED("gps hgt timeout - reset to baro"); } else if (reset_to_gps) { // reset the height mode @@ -883,7 +883,7 @@ void Ekf::controlHeightSensorTimeouts() // request a reset reset_height = true; - ECL_WARN_TIMESTAMPED("EKF gps hgt timeout - reset to GPS"); + ECL_WARN_TIMESTAMPED("gps hgt timeout - reset to GPS"); } else { // we have nothing to reset to @@ -909,7 +909,7 @@ void Ekf::controlHeightSensorTimeouts() // request a reset reset_height = true; - ECL_WARN_TIMESTAMPED("EKF rng hgt timeout - reset to rng hgt"); + ECL_WARN_TIMESTAMPED("rng hgt timeout - reset to rng hgt"); } else if (reset_to_baro) { // set height sensor health @@ -920,7 +920,7 @@ void Ekf::controlHeightSensorTimeouts() // request a reset reset_height = true; - ECL_WARN_TIMESTAMPED("EKF rng hgt timeout - reset to baro"); + ECL_WARN_TIMESTAMPED("rng hgt timeout - reset to baro"); } else { // we have nothing to reset to @@ -954,7 +954,7 @@ void Ekf::controlHeightSensorTimeouts() // request a reset reset_height = true; - ECL_WARN_TIMESTAMPED("EKF ev hgt timeout - reset to baro"); + ECL_WARN_TIMESTAMPED("ev hgt timeout - reset to baro"); } else if (reset_to_ev) { // reset the height mode @@ -962,7 +962,7 @@ void Ekf::controlHeightSensorTimeouts() // request a reset reset_height = true; - ECL_WARN_TIMESTAMPED("EKF ev hgt timeout - reset to ev hgt"); + ECL_WARN_TIMESTAMPED("ev hgt timeout - reset to ev hgt"); } else { // we have nothing to reset to @@ -1405,7 +1405,7 @@ void Ekf::controlFakePosFusion() _fuse_hpos_as_odom = false; if (_time_last_fake_pos != 0) { - ECL_WARN_TIMESTAMPED("EKF stopping navigation"); + ECL_WARN_TIMESTAMPED("stopping navigation"); } } diff --git a/EKF/covariance.cpp b/EKF/covariance.cpp index a478de1313e0..d75fe2a5e36d 100644 --- a/EKF/covariance.cpp +++ b/EKF/covariance.cpp @@ -853,7 +853,7 @@ void Ekf::fixCovarianceErrors() P[15][15] = varZ; _time_acc_bias_check = _time_last_imu; _fault_status.flags.bad_acc_bias = false; - ECL_WARN_TIMESTAMPED("EKF invalid accel bias - resetting covariance"); + ECL_WARN_TIMESTAMPED("invalid accel bias - resetting covariance"); } else { // ensure the covariance values are symmetrical diff --git a/EKF/ekf_helper.cpp b/EKF/ekf_helper.cpp index 03b5f48bca6d..5d19ce375a16 100644 --- a/EKF/ekf_helper.cpp +++ b/EKF/ekf_helper.cpp @@ -54,7 +54,7 @@ bool Ekf::resetVelocity() // reset EKF states if (_control_status.flags.gps && _gps_check_fail_status.value==0) { - ECL_INFO_TIMESTAMPED("EKF reset velocity to GPS"); + ECL_INFO_TIMESTAMPED("reset velocity to GPS"); // this reset is only called if we have new gps data at the fusion time horizon _state.vel = _gps_sample_delayed.vel; @@ -62,7 +62,7 @@ bool Ekf::resetVelocity() setDiag(P, 4, 6, sq(_gps_sample_delayed.sacc)); } else if (_control_status.flags.opt_flow) { - ECL_INFO_TIMESTAMPED("EKF reset velocity to flow"); + ECL_INFO_TIMESTAMPED("reset velocity to flow"); // constrain height above ground to be above minimum possible float heightAboveGndEst = fmaxf((_terrain_vpos - _state.pos(2)), _params.rng_gnd_clearance); @@ -97,7 +97,7 @@ bool Ekf::resetVelocity() // reset the horizontal velocity variance using the optical flow noise variance P[5][5] = P[4][4] = sq(range) * calcOptFlowMeasVar(); } else if (_control_status.flags.ev_vel) { - ECL_INFO_TIMESTAMPED("EKF reset velocity to ev velocity"); + ECL_INFO_TIMESTAMPED("reset velocity to ev velocity"); Vector3f _ev_vel = _ev_sample_delayed.vel; if(_params.fusion_mode & MASK_ROTATE_EV){ _ev_vel = _ev_rot_mat *_ev_sample_delayed.vel; @@ -107,7 +107,7 @@ bool Ekf::resetVelocity() _state.vel(2) = _ev_vel(2); setDiag(P, 4, 6, sq(_ev_sample_delayed.velErr)); } else { - ECL_INFO_TIMESTAMPED("EKF reset velocity to zero"); + ECL_INFO_TIMESTAMPED("reset velocity to zero"); // Used when falling back to non-aiding mode of operation _state.vel(0) = 0.0f; _state.vel(1) = 0.0f; @@ -149,7 +149,7 @@ bool Ekf::resetPosition() _hpos_prev_available = false; if (_control_status.flags.gps) { - ECL_INFO_TIMESTAMPED("EKF reset position to GPS"); + ECL_INFO_TIMESTAMPED("reset position to GPS"); // this reset is only called if we have new gps data at the fusion time horizon _state.pos(0) = _gps_sample_delayed.pos(0); @@ -159,7 +159,7 @@ bool Ekf::resetPosition() setDiag(P, 7, 8, sq(_gps_sample_delayed.hacc)); } else if (_control_status.flags.ev_pos) { - ECL_INFO_TIMESTAMPED("EKF reset position to ev position"); + ECL_INFO_TIMESTAMPED("reset position to ev position"); // this reset is only called if we have new ev data at the fusion time horizon Vector3f _ev_pos = _ev_sample_delayed.pos; @@ -173,7 +173,7 @@ bool Ekf::resetPosition() setDiag(P, 7, 8, sq(_ev_sample_delayed.posErr)); } else if (_control_status.flags.opt_flow) { - ECL_INFO_TIMESTAMPED("EKF reset position to last known position"); + ECL_INFO_TIMESTAMPED("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 @@ -192,7 +192,7 @@ bool Ekf::resetPosition() zeroRows(P,7,8); } else { - ECL_INFO_TIMESTAMPED("EKF reset position to last known position"); + ECL_INFO_TIMESTAMPED("reset position to last known position"); // Used when falling back to non-aiding mode of operation _state.pos(0) = _last_known_posNE(0); _state.pos(1) = _last_known_posNE(1); @@ -439,11 +439,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_TIMESTAMPED("EKF bad yaw corrected using GPS course"); + ECL_WARN_TIMESTAMPED("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_aligned_in_flight && (_num_bad_flight_yaw_events >= 2) && !_control_status.flags.mag_fault) { - ECL_WARN_TIMESTAMPED("EKF stopping magnetometer use"); + ECL_WARN_TIMESTAMPED("stopping magnetometer use"); _control_status.flags.mag_fault = true; } diff --git a/EKF/estimator_interface.cpp b/EKF/estimator_interface.cpp index e10130d7db31..ee9e2245be37 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_TIMESTAMPED("EKF drag buffer allocation failed"); + ECL_ERR_TIMESTAMPED("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_TIMESTAMPED("EKF mag buffer allocation failed"); + ECL_ERR_TIMESTAMPED("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_TIMESTAMPED("EKF GPS buffer allocation failed"); + ECL_ERR_TIMESTAMPED("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_TIMESTAMPED("EKF baro buffer allocation failed"); + ECL_ERR_TIMESTAMPED("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_TIMESTAMPED("EKF airspeed buffer allocation failed"); + ECL_ERR_TIMESTAMPED("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_TIMESTAMPED("EKF range finder buffer allocation failed"); + ECL_ERR_TIMESTAMPED("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_TIMESTAMPED("EKF optical flow buffer allocation failed"); + ECL_ERR_TIMESTAMPED("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_TIMESTAMPED("EKF external vision buffer allocation failed"); + ECL_ERR_TIMESTAMPED("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_TIMESTAMPED("EKF aux vel buffer allocation failed"); + ECL_ERR_TIMESTAMPED("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_TIMESTAMPED("EKF buffer allocation failed!"); + ECL_ERR_TIMESTAMPED("buffer allocation failed!"); unallocate_buffers(); return false; } diff --git a/EKF/gps_checks.cpp b/EKF/gps_checks.cpp index 85bd3e247900..aa8993373898 100644 --- a/EKF/gps_checks.cpp +++ b/EKF/gps_checks.cpp @@ -90,13 +90,14 @@ bool Ekf::collect_gps(const gps_message &gps) _gps_origin_epv = gps.epv; // if the user has selected GPS as the primary height source, switch across to using it + if (_params.vdist_sensor_type == VDIST_SENSOR_GPS) { - ECL_INFO_TIMESTAMPED("EKF GPS checks passed (WGS-84 origin set, using GPS height)"); - setControlGPSHeight(); + ECL_INFO_TIMESTAMPED("GPS checks passed (WGS-84 origin set, using GPS height)"); + setControlGPSHeight(); // zero the sensor offset _hgt_sensor_offset = 0.0f; } else { - ECL_INFO_TIMESTAMPED("EKF GPS checks passed (WGS-84 origin set)"); + ECL_INFO_TIMESTAMPED("GPS checks passed (WGS-84 origin set)"); } } diff --git a/EKF/gps_yaw_fusion.cpp b/EKF/gps_yaw_fusion.cpp index b1b5f97c9e73..305a7cee020f 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_TIMESTAMPED("EKF GPS yaw fusion numerical error - covariance reset"); + ECL_ERR_TIMESTAMPED("GPS yaw fusion numerical error - covariance reset"); return; } diff --git a/EKF/mag_fusion.cpp b/EKF/mag_fusion.cpp index ed2cc6adf491..ebc461143221 100644 --- a/EKF/mag_fusion.cpp +++ b/EKF/mag_fusion.cpp @@ -105,7 +105,7 @@ void Ekf::fuseMag() // we need to re-initialise covariances and abort this fusion step resetMagRelatedCovariances(); - ECL_ERR_TIMESTAMPED("EKF magX fusion numerical error - covariance reset"); + ECL_ERR_TIMESTAMPED("magX fusion numerical error - covariance reset"); return; } @@ -124,7 +124,7 @@ void Ekf::fuseMag() // we need to re-initialise covariances and abort this fusion step resetMagRelatedCovariances(); - ECL_ERR_TIMESTAMPED("EKF magY fusion numerical error - covariance reset"); + ECL_ERR_TIMESTAMPED("magY fusion numerical error - covariance reset"); return; } @@ -143,7 +143,7 @@ void Ekf::fuseMag() // we need to re-initialise covariances and abort this fusion step resetMagRelatedCovariances(); - ECL_ERR_TIMESTAMPED("EKF magZ fusion numerical error - covariance reset"); + ECL_ERR_TIMESTAMPED("magZ fusion numerical error - covariance reset"); return; } @@ -696,7 +696,7 @@ void Ekf::fuseHeading() // we reinitialise the covariance matrix and abort this fusion step initialiseCovariance(); - ECL_ERR_TIMESTAMPED("EKF mag yaw fusion numerical error - covariance reset"); + ECL_ERR_TIMESTAMPED("mag yaw fusion numerical error - covariance reset"); return; } diff --git a/EKF/sideslip_fusion.cpp b/EKF/sideslip_fusion.cpp index f77cb99dde77..d173bbe4cd04 100644 --- a/EKF/sideslip_fusion.cpp +++ b/EKF/sideslip_fusion.cpp @@ -132,12 +132,12 @@ void Ekf::fuseSideslip() if (update_wind_only) { resetWindStates(); resetWindCovariance(); - ECL_ERR_TIMESTAMPED("EKF synthetic sideslip fusion badly conditioned - wind covariance reset"); + ECL_ERR_TIMESTAMPED("synthetic sideslip fusion badly conditioned - wind covariance reset"); } else { initialiseCovariance(); _state.wind_vel.setZero(); - ECL_ERR_TIMESTAMPED("EKF synthetic sideslip fusion badly conditioned - full covariance reset"); + ECL_ERR_TIMESTAMPED("synthetic sideslip fusion badly conditioned - full covariance reset"); } return;