From 24f2e60b7eff664d0035d5f75f410073be28d894 Mon Sep 17 00:00:00 2001 From: kritz Date: Wed, 13 May 2020 08:09:26 +0200 Subject: [PATCH] Reduce stored strings, to save flash space (#815) --- EKF/airspeed_fusion.cpp | 6 ++++-- EKF/control.cpp | 42 +++++++++++++++++++++++++++-------------- EKF/covariance.cpp | 2 +- EKF/ekf.h | 10 +++++----- EKF/gps_checks.cpp | 2 +- EKF/gps_yaw_fusion.cpp | 2 +- EKF/mag_fusion.cpp | 7 ++++--- EKF/sideslip_fusion.cpp | 6 ++++-- 8 files changed, 48 insertions(+), 29 deletions(-) diff --git a/EKF/airspeed_fusion.cpp b/EKF/airspeed_fusion.cpp index 588944c7bc..4b63549262 100644 --- a/EKF/airspeed_fusion.cpp +++ b/EKF/airspeed_fusion.cpp @@ -94,16 +94,18 @@ void Ekf::fuseAirspeed() _fault_status.flags.bad_airspeed = true; // if we are getting aiding from other sources, warn and reset the wind states and covariances only + const char* action_string = nullptr; if (update_wind_only) { resetWindStates(); resetWindCovariance(); - ECL_ERR_TIMESTAMPED("airspeed fusion badly conditioned - wind covariance reset"); + action_string = "wind"; } else { initialiseCovariance(); _state.wind_vel.setZero(); - ECL_ERR_TIMESTAMPED("airspeed fusion badly conditioned - full covariance reset"); + action_string = "full"; } + ECL_ERR("airspeed badly conditioned - %s covariance reset", action_string); return; } diff --git a/EKF/control.cpp b/EKF/control.cpp index 641b906275..8ec3954d8c 100644 --- a/EKF/control.cpp +++ b/EKF/control.cpp @@ -71,14 +71,14 @@ void Ekf::controlFusionModes() height_source = "gps"; } else if (_control_status.flags.rng_hgt) { - height_source = "range"; + height_source = "rng"; } else { height_source = "unknown"; } if(height_source){ - ECL_INFO("%llu: EKF aligned, (%s height, IMU buf: %i, OBS buf: %i)", + ECL_INFO("%llu: EKF aligned, (%s hgt, IMU buf: %i, OBS buf: %i)", (unsigned long long)_imu_sample_delayed.time_us, height_source, (int)_imu_buffer_length, (int)_obs_buffer_length); } } @@ -537,7 +537,7 @@ void Ekf::controlGpsFusion() stopMagHdgFusion(); stopMag3DFusion(); - ECL_INFO_TIMESTAMPED("commencing GPS yaw fusion"); + ECL_INFO_TIMESTAMPED("starting GPS yaw fusion"); } } @@ -588,7 +588,7 @@ void Ekf::controlGpsFusion() } if (_control_status.flags.gps) { - ECL_INFO_TIMESTAMPED("commencing GPS fusion"); + ECL_INFO_TIMESTAMPED("starting GPS fusion"); _time_last_gps = _time_last_imu; } } @@ -606,7 +606,7 @@ void Ekf::controlGpsFusion() if (_control_status.flags.ev_pos && !(_params.fusion_mode & MASK_ROTATE_EV)) { resetPosition(); } - ECL_WARN_TIMESTAMPED("GPS data quality poor - stopping use"); + ECL_WARN_TIMESTAMPED("GPS quality poor - stopping use"); } // handle case where we are not currently using GPS, but need to align yaw angle using EKF-GSF before @@ -760,6 +760,8 @@ void Ekf::controlHeightSensorTimeouts() if (hgt_fusion_timeout || continuous_bad_accel_hgt) { bool request_height_reset = false; + const char* failing_height_source = nullptr; + const char* new_height_source = nullptr; if (_control_status.flags.baro_hgt) { // check if GPS height is available @@ -780,11 +782,13 @@ void Ekf::controlHeightSensorTimeouts() startGpsHgtFusion(); request_height_reset = true; - ECL_WARN_TIMESTAMPED("baro hgt timeout - reset to GPS"); + failing_height_source = "baro"; + new_height_source = "gps"; } else if (!_baro_hgt_faulty) { request_height_reset = true; - ECL_WARN_TIMESTAMPED("baro hgt timeout - reset to baro"); + failing_height_source = "baro"; + new_height_source = "baro"; } } else if (_control_status.flags.gps_hgt) { @@ -806,24 +810,28 @@ void Ekf::controlHeightSensorTimeouts() startBaroHgtFusion(); request_height_reset = true; - ECL_WARN_TIMESTAMPED("gps hgt timeout - reset to baro"); + failing_height_source = "gps"; + new_height_source = "baro"; } else if (!_gps_hgt_intermittent) { request_height_reset = true; - ECL_WARN_TIMESTAMPED("gps hgt timeout - reset to GPS"); + failing_height_source = "gps"; + new_height_source = "gps"; } } else if (_control_status.flags.rng_hgt) { if (_range_sensor.isHealthy()) { request_height_reset = true; - ECL_WARN_TIMESTAMPED("rng hgt timeout - reset to rng hgt"); + failing_height_source = "rng"; + new_height_source = "rng"; } else if (!_baro_hgt_faulty) { startBaroHgtFusion(); request_height_reset = true; - ECL_WARN_TIMESTAMPED("rng hgt timeout - reset to baro"); + failing_height_source = "rng"; + new_height_source = "baro"; } } else if (_control_status.flags.ev_hgt) { @@ -833,16 +841,22 @@ void Ekf::controlHeightSensorTimeouts() if (ev_data_available) { request_height_reset = true; - ECL_WARN_TIMESTAMPED("ev hgt timeout - reset to ev hgt"); + failing_height_source = "ev"; + new_height_source = "ev"; } else if (!_baro_hgt_faulty) { startBaroHgtFusion(); request_height_reset = true; - ECL_WARN_TIMESTAMPED("ev hgt timeout - reset to baro"); + failing_height_source = "ev"; + new_height_source = "baro"; } } + if (failing_height_source && new_height_source) { + ECL_WARN("%s hgt timeout - reset to %s", failing_height_source, new_height_source); + } + // Reset vertical position and velocity states to the last measurement if (request_height_reset) { resetHeight(); @@ -901,7 +915,7 @@ void Ekf::controlHeightFusion() switch (_params.vdist_sensor_type) { default: - ECL_ERR("Invalid height mode: %d", _params.vdist_sensor_type); + ECL_ERR("Invalid hgt mode: %d", _params.vdist_sensor_type); // FALLTHROUGH case VDIST_SENSOR_BARO: diff --git a/EKF/covariance.cpp b/EKF/covariance.cpp index cb1d8eee91..ad11df4a7b 100644 --- a/EKF/covariance.cpp +++ b/EKF/covariance.cpp @@ -822,7 +822,7 @@ void Ekf::fixCovarianceErrors(bool force_symmetry) _time_acc_bias_check = _time_last_imu; _fault_status.flags.bad_acc_bias = false; - ECL_WARN_TIMESTAMPED("invalid accel bias - resetting covariance"); + ECL_WARN_TIMESTAMPED("invalid accel bias - covariance reset"); } else if (force_symmetry) { // ensure the covariance values are symmetrical diff --git a/EKF/ekf.h b/EKF/ekf.h index f98853ec88..610b4c321c 100644 --- a/EKF/ekf.h +++ b/EKF/ekf.h @@ -574,15 +574,15 @@ private: // reset velocity states of the ekf bool resetVelocity(); - void resetVelocityToGps(); + inline void resetVelocityToGps(); - void resetHorizontalVelocityToOpticalFlow(); + inline void resetHorizontalVelocityToOpticalFlow(); - void resetVelocityToVision(); + inline void resetVelocityToVision(); - void resetHorizontalVelocityToZero(); + inline void resetHorizontalVelocityToZero(); - void resetVelocityTo(const Vector3f &vel); + inline void resetVelocityTo(const Vector3f &vel); inline void resetHorizontalVelocityTo(const Vector2f &new_horz_vel); diff --git a/EKF/gps_checks.cpp b/EKF/gps_checks.cpp index 3739c3c104..737fcf4b4e 100644 --- a/EKF/gps_checks.cpp +++ b/EKF/gps_checks.cpp @@ -101,7 +101,7 @@ bool Ekf::collect_gps(const gps_message &gps) if (_params.vdist_sensor_type == VDIST_SENSOR_GPS) { startGpsHgtFusion(); } - ECL_INFO_TIMESTAMPED("GPS checks passed (WGS-84 origin set)"); + ECL_INFO_TIMESTAMPED("GPS checks passed"); } // start collecting GPS if there is a 3D fix and the NED origin has been set diff --git a/EKF/gps_yaw_fusion.cpp b/EKF/gps_yaw_fusion.cpp index 506527e407..e6d3134e0f 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("GPS yaw fusion numerical error - covariance reset"); + ECL_ERR_TIMESTAMPED("GPS yaw numerical error - covariance reset"); return; } diff --git a/EKF/mag_fusion.cpp b/EKF/mag_fusion.cpp index 861fe04039..9eb5951567 100644 --- a/EKF/mag_fusion.cpp +++ b/EKF/mag_fusion.cpp @@ -93,6 +93,7 @@ void Ekf::fuseMag() // X axis innovation variance _mag_innov_var[0] = (P(19,19) + R_MAG + P(1,19)*SH_MAG[0] - P(2,19)*SH_MAG[1] + P(3,19)*SH_MAG[2] - P(16,19)*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + (2.0f*q0*q3 + 2.0f*q1*q2)*(P(19,17) + P(1,17)*SH_MAG[0] - P(2,17)*SH_MAG[1] + P(3,17)*SH_MAG[2] - P(16,17)*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P(17,17)*(2.0f*q0*q3 + 2.0f*q1*q2) - P(18,17)*(2.0f*q0*q2 - 2.0f*q1*q3) + P(0,17)*(SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)) - (2.0f*q0*q2 - 2.0f*q1*q3)*(P(19,18) + P(1,18)*SH_MAG[0] - P(2,18)*SH_MAG[1] + P(3,18)*SH_MAG[2] - P(16,18)*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P(17,18)*(2.0f*q0*q3 + 2.0f*q1*q2) - P(18,18)*(2.0f*q0*q2 - 2.0f*q1*q3) + P(0,18)*(SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)) + (SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)*(P(19,0) + P(1,0)*SH_MAG[0] - P(2,0)*SH_MAG[1] + P(3,0)*SH_MAG[2] - P(16,0)*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P(17,0)*(2.0f*q0*q3 + 2.0f*q1*q2) - P(18,0)*(2.0f*q0*q2 - 2.0f*q1*q3) + P(0,0)*(SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)) + P(17,19)*(2.0f*q0*q3 + 2.0f*q1*q2) - P(18,19)*(2.0f*q0*q2 - 2.0f*q1*q3) + SH_MAG[0]*(P(19,1) + P(1,1)*SH_MAG[0] - P(2,1)*SH_MAG[1] + P(3,1)*SH_MAG[2] - P(16,1)*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P(17,1)*(2.0f*q0*q3 + 2.0f*q1*q2) - P(18,1)*(2.0f*q0*q2 - 2.0f*q1*q3) + P(0,1)*(SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)) - SH_MAG[1]*(P(19,2) + P(1,2)*SH_MAG[0] - P(2,2)*SH_MAG[1] + P(3,2)*SH_MAG[2] - P(16,2)*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P(17,2)*(2.0f*q0*q3 + 2.0f*q1*q2) - P(18,2)*(2.0f*q0*q2 - 2.0f*q1*q3) + P(0,2)*(SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)) + SH_MAG[2]*(P(19,3) + P(1,3)*SH_MAG[0] - P(2,3)*SH_MAG[1] + P(3,3)*SH_MAG[2] - P(16,3)*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P(17,3)*(2.0f*q0*q3 + 2.0f*q1*q2) - P(18,3)*(2.0f*q0*q2 - 2.0f*q1*q3) + P(0,3)*(SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)) - (SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6])*(P(19,16) + P(1,16)*SH_MAG[0] - P(2,16)*SH_MAG[1] + P(3,16)*SH_MAG[2] - P(16,16)*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P(17,16)*(2.0f*q0*q3 + 2.0f*q1*q2) - P(18,16)*(2.0f*q0*q2 - 2.0f*q1*q3) + P(0,16)*(SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)) + P(0,19)*(SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)); // check for a badly conditioned covariance matrix + const char* numerical_error_covariance_reset_string = "numerical error - covariance reset"; if (_mag_innov_var[0] >= R_MAG) { // the innovation variance contribution from the state covariances is non-negative - no fault @@ -104,7 +105,7 @@ void Ekf::fuseMag() // we need to re-initialise covariances and abort this fusion step resetMagRelatedCovariances(); - ECL_ERR_TIMESTAMPED("magX fusion numerical error - covariance reset"); + ECL_ERR("magX %s", numerical_error_covariance_reset_string); return; } @@ -123,7 +124,7 @@ void Ekf::fuseMag() // we need to re-initialise covariances and abort this fusion step resetMagRelatedCovariances(); - ECL_ERR_TIMESTAMPED("magY fusion numerical error - covariance reset"); + ECL_ERR("magY %s", numerical_error_covariance_reset_string); return; } @@ -142,7 +143,7 @@ void Ekf::fuseMag() // we need to re-initialise covariances and abort this fusion step resetMagRelatedCovariances(); - ECL_ERR_TIMESTAMPED("magZ fusion numerical error - covariance reset"); + ECL_ERR("magZ %s", numerical_error_covariance_reset_string); return; } diff --git a/EKF/sideslip_fusion.cpp b/EKF/sideslip_fusion.cpp index 8672f0529b..bb40d14274 100644 --- a/EKF/sideslip_fusion.cpp +++ b/EKF/sideslip_fusion.cpp @@ -129,16 +129,18 @@ void Ekf::fuseSideslip() _fault_status.flags.bad_sideslip = true; // if we are getting aiding from other sources, warn and reset the wind states and covariances only + const char* action_string = nullptr; if (update_wind_only) { resetWindStates(); resetWindCovariance(); - ECL_ERR_TIMESTAMPED("synthetic sideslip fusion badly conditioned - wind covariance reset"); + action_string = "wind"; } else { initialiseCovariance(); _state.wind_vel.setZero(); - ECL_ERR_TIMESTAMPED("synthetic sideslip fusion badly conditioned - full covariance reset"); + action_string = "full"; } + ECL_ERR("sideslip badly conditioned - %s covariance reset", action_string); return; }