Reduce stored strings, to save flash space (#815)

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

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:

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

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

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

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

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

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