From 3c4c09593fd4a3585695748e3b4cbe08bbe4271c Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Wed, 28 Dec 2016 15:58:03 +1100 Subject: [PATCH] EKF: Rationalise console messages Combine the observation action buffer and alignment messages Ensure all data timeout messages are warnings. Use consistent terminology. --- EKF/control.cpp | 40 ++++++++++++++++++++++++------------- EKF/ekf.cpp | 5 ----- EKF/estimator_interface.cpp | 3 --- EKF/gps_checks.cpp | 5 +++-- 4 files changed, 29 insertions(+), 24 deletions(-) diff --git a/EKF/control.cpp b/EKF/control.cpp index e72e9ef4d2..0b38b5a34f 100644 --- a/EKF/control.cpp +++ b/EKF/control.cpp @@ -61,7 +61,19 @@ void Ekf::controlFusionModes() if ((angle_err_var_vec(0) + angle_err_var_vec(1)) < sq(0.05235f)) { _control_status.flags.tilt_align = true; _control_status.flags.yaw_align = resetMagHeading(_mag_sample_delayed.mag); - ECL_INFO("EKF alignment complete"); + + // send alignment status message to the console + if (_control_status.flags.baro_hgt) { + ECL_INFO("EKF aligned, (pressure height, IMU buf: %i, OBS buf: %i)",(int)_imu_buffer_length,(int)_obs_buffer_length); + } else if (_control_status.flags.ev_hgt) { + ECL_INFO("EKF aligned, (EV height, IMU buf: %i, OBS buf: %i)",(int)_imu_buffer_length,(int)_obs_buffer_length); + } else if (_control_status.flags.gps_hgt) { + ECL_INFO("EKF aligned, (GPS height, IMU buf: %i, OBS buf: %i)",(int)_imu_buffer_length,(int)_obs_buffer_length); + } 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); + } } @@ -108,7 +120,7 @@ void Ekf::controlExternalVisionFusion() if (_time_last_imu - _time_last_ext_vision < 2 * EV_MAX_INTERVAL) { // turn on use of external vision measurements for position and height _control_status.flags.ev_pos = true; - ECL_INFO("EKF switching to external vision position fusion"); + ECL_INFO("EKF commencing external vision position fusion"); // turn off other forms of height aiding _control_status.flags.baro_hgt = false; _control_status.flags.gps_hgt = false; @@ -170,7 +182,7 @@ void Ekf::controlExternalVisionFusion() _control_status.flags.mag_3D = false; _control_status.flags.mag_dec = false; - ECL_INFO("EKF switching to external vision yaw fusion"); + ECL_INFO("EKF commencing external vision yaw fusion"); } } @@ -342,7 +354,7 @@ void Ekf::controlGpsFusion() } if (_control_status.flags.gps) { - ECL_INFO("EKF commencing GPS aiding"); + ECL_INFO("EKF commencing GPS fusion"); _time_last_gps = _time_last_imu; } @@ -366,7 +378,7 @@ void Ekf::controlGpsFusion() // Reset states to the last GPS measurement resetPosition(); resetVelocity(); - ECL_WARN("EKF GPS fusion timout - resetting to GPS"); + ECL_WARN("EKF GPS fusion timeout - reset to GPS"); // Reset the timeout counters _time_last_pos_fuse = _time_last_imu; @@ -415,7 +427,7 @@ void Ekf::controlGpsFusion() _last_known_posNE(0) = _state.pos(0); _last_known_posNE(1) = _state.pos(1); _state.vel.setZero(); - ECL_WARN("EKF GPS fusion timout - stopping GPS aiding"); + ECL_WARN("EKF measurement timeout - stopping navigation"); } } @@ -484,7 +496,7 @@ void Ekf::controlHeightSensorTimeouts() // request a reset reset_height = true; - ECL_INFO("EKF baro hgt timeout - reset to GPS"); + ECL_WARN("EKF baro hgt timeout - reset to GPS"); } else if (reset_to_baro){ // set height sensor health @@ -498,7 +510,7 @@ void Ekf::controlHeightSensorTimeouts() // request a reset reset_height = true; - ECL_INFO("EKF baro hgt timeout - reset to baro"); + ECL_WARN("EKF baro hgt timeout - reset to baro"); } else { // we have nothing we can reset to @@ -543,7 +555,7 @@ void Ekf::controlHeightSensorTimeouts() // request a reset reset_height = true; - ECL_INFO("EKF gps hgt timeout - reset to baro"); + ECL_WARN("EKF gps hgt timeout - reset to baro"); } else if (reset_to_gps) { // set height sensor health @@ -557,7 +569,7 @@ void Ekf::controlHeightSensorTimeouts() // request a reset reset_height = true; - ECL_INFO("EKF gps hgt timeout - reset to GPS"); + ECL_WARN("EKF gps hgt timeout - reset to GPS"); } else { // we have nothing to reset to @@ -595,7 +607,7 @@ void Ekf::controlHeightSensorTimeouts() // request a reset reset_height = true; - ECL_INFO("EKF rng hgt timeout - reset to baro"); + ECL_WARN("EKF rng hgt timeout - reset to baro"); } else if (reset_to_rng) { // set height sensor health @@ -609,7 +621,7 @@ void Ekf::controlHeightSensorTimeouts() // request a reset reset_height = true; - ECL_INFO("EKF rng hgt timeout - reset to rng hgt"); + ECL_WARN("EKF rng hgt timeout - reset to rng hgt"); } else { // we have nothing to reset to @@ -647,7 +659,7 @@ void Ekf::controlHeightSensorTimeouts() // request a reset reset_height = true; - ECL_INFO("EKF ev hgt timeout - reset to baro"); + ECL_WARN("EKF ev hgt timeout - reset to baro"); } else if (reset_to_ev) { // reset the height mode @@ -658,7 +670,7 @@ void Ekf::controlHeightSensorTimeouts() // request a reset reset_height = true; - ECL_INFO("EKF ev hgt timeout - reset to ev hgt"); + ECL_WARN("EKF ev hgt timeout - reset to ev hgt"); } else { // we have nothing to reset to diff --git a/EKF/ekf.cpp b/EKF/ekf.cpp index 9d12acb9e0..c39bff3d09 100644 --- a/EKF/ekf.cpp +++ b/EKF/ekf.cpp @@ -400,16 +400,11 @@ bool Ekf::initialiseFilter(void) baroSample baro_newest = _baro_buffer.get_newest(); _baro_hgt_offset = baro_newest.hgt; _state.pos(2) = -math::max(_rng_filt_state * _R_to_earth(2, 2),_params.rng_gnd_clearance); - ECL_INFO("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 // because the initialisation position is not the zero datum resetHeight(); - ECL_INFO("EKF using vision height - commencing alignment"); - - } else if (_control_status.flags.baro_hgt){ - ECL_INFO("EKF using pressure height - commencing alignment"); } diff --git a/EKF/estimator_interface.cpp b/EKF/estimator_interface.cpp index d22b33ee76..51a5b78e08 100644 --- a/EKF/estimator_interface.cpp +++ b/EKF/estimator_interface.cpp @@ -362,9 +362,6 @@ bool EstimatorInterface::initialise_interface(uint64_t timestamp) // limit to be no longer than the IMU buffer (we can't process data faster than the EKF prediction rate) _obs_buffer_length = math::min(_obs_buffer_length,_imu_buffer_length); - ECL_INFO("EKF IMU buffer length = %i",(int)_imu_buffer_length); - ECL_INFO("EKF observation buffer length = %i",(int)_obs_buffer_length); - if (!(_imu_buffer.allocate(_imu_buffer_length) && _gps_buffer.allocate(_obs_buffer_length) && _mag_buffer.allocate(_obs_buffer_length) && diff --git a/EKF/gps_checks.cpp b/EKF/gps_checks.cpp index ffdd04c5e8..952987879c 100644 --- a/EKF/gps_checks.cpp +++ b/EKF/gps_checks.cpp @@ -61,7 +61,6 @@ bool Ekf::collect_gps(uint64_t time_usec, struct gps_message *gps) if (!_NED_origin_initialised) { // we have good GPS data so can now set the origin's WGS-84 position if (gps_is_good(gps) && !_NED_origin_initialised) { - ECL_INFO("EKF gps is good - setting origin"); // Set the origin's WGS-84 position to the last gps fix double lat = gps->lat / 1.0e7; double lon = gps->lon / 1.0e7; @@ -86,12 +85,14 @@ bool Ekf::collect_gps(uint64_t time_usec, struct 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 switching to GPS height"); + ECL_INFO("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)"); } } }