Save flash space

This commit saves roughly 400-500 Bytes in flash space

EKF/estimator_interface.cpp

@@ -171,7 +171,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("mag buffer allocation failed");
+			printBufferAllocationFailed("mag");
 			return;
 		}
 	}
@@ -203,7 +203,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("GPS buffer allocation failed");
+			printBufferAllocationFailed("GPS");
 			return;
 		}
 	}
@@ -264,7 +264,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("baro buffer allocation failed");
+			printBufferAllocationFailed("baro");
 			return;
 		}
 	}
@@ -297,7 +297,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("airspeed buffer allocation failed");
+			printBufferAllocationFailed("airspeed");
 			return;
 		}
 	}
@@ -327,7 +327,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("range finder buffer allocation failed");
+			printBufferAllocationFailed("range");
 			return;
 		}
 	}
@@ -357,7 +357,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("optical flow buffer allocation failed");
+			printBufferAllocationFailed("flow");
 			return;
 		}
 	}
@@ -423,7 +423,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("external vision buffer allocation failed");
+			printBufferAllocationFailed("vision");
 			return;
 		}
 	}
@@ -459,7 +459,7 @@ void EstimatorInterface::setAuxVelData(uint64_t time_usec, const Vector3f &veloc
 		_auxvel_buffer_fail = !_auxvel_buffer.allocate(_obs_buffer_length);
 
 		if (_auxvel_buffer_fail) {
-			ECL_ERR_TIMESTAMPED("aux vel buffer allocation failed");
+			printBufferAllocationFailed("aux vel");
 			return;
 		}
 	}
@@ -508,30 +508,15 @@ bool EstimatorInterface::initialise_interface(uint64_t timestamp)
 	      _output_buffer.allocate(_imu_buffer_length) &&
 	      _output_vert_buffer.allocate(_imu_buffer_length))) {
 
-		ECL_ERR_TIMESTAMPED("buffer allocation failed!");
+		printBufferAllocationFailed("");
 		unallocate_buffers();
 		return false;
 	}
 
-	_dt_imu_avg = 0.0f;
-
-	_imu_sample_delayed.delta_ang.setZero();
-	_imu_sample_delayed.delta_vel.setZero();
-	_imu_sample_delayed.delta_ang_dt = 0.0f;
-	_imu_sample_delayed.delta_vel_dt = 0.0f;
 	_imu_sample_delayed.time_us = timestamp;
 
-	_initialised = false;
-
-	_time_last_imu = 0;
-	_time_last_gps = 0;
-	_time_last_mag = 0;
-	_time_last_baro = 0;
-	_time_last_range = 0;
-	_time_last_airspeed = 0;
-	_time_last_optflow = 0;
 	_fault_status.value = 0;
-	_time_last_ext_vision = 0;
+
 	return true;
 }
 
@@ -558,6 +543,14 @@ bool EstimatorInterface::local_position_is_valid()
 	return !_deadreckon_time_exceeded;
 }
 
+void EstimatorInterface::printBufferAllocationFailed(const char * buffer_name)
+{
+	if(buffer_name)
+	{
+		ECL_ERR_TIMESTAMPED("%s buffer allocation failed", buffer_name);
+	}
+}
+
 void EstimatorInterface::print_status()
 {
 	ECL_INFO("local position valid: %s", local_position_is_valid() ? "yes" : "no");
@@ -570,7 +563,7 @@ void EstimatorInterface::print_status()
 	ECL_INFO("range buffer: %d (%d Bytes)", _range_buffer.get_length(), _range_buffer.get_total_size());
 	ECL_INFO("airspeed buffer: %d (%d Bytes)", _airspeed_buffer.get_length(), _airspeed_buffer.get_total_size());
 	ECL_INFO("flow buffer: %d (%d Bytes)", _flow_buffer.get_length(), _flow_buffer.get_total_size());
-	ECL_INFO("ext vision buffer: %d (%d Bytes)", _ext_vision_buffer.get_length(), _ext_vision_buffer.get_total_size());
+	ECL_INFO("vision buffer: %d (%d Bytes)", _ext_vision_buffer.get_length(), _ext_vision_buffer.get_total_size());
 	ECL_INFO("output buffer: %d (%d Bytes)", _output_buffer.get_length(), _output_buffer.get_total_size());
 	ECL_INFO("output vert buffer: %d (%d Bytes)", _output_vert_buffer.get_length(), _output_vert_buffer.get_total_size());
 	ECL_INFO("drag buffer: %d (%d Bytes)", _drag_buffer.get_length(), _drag_buffer.get_total_size());

EKF/estimator_interface.h

@@ -174,35 +174,27 @@ public:
 	// accumulate and downsample IMU data to the EKF prediction rate
 	virtual bool collect_imu(const imuSample &imu) = 0;
 
-	// set delta angle imu data
 	void setIMUData(const imuSample &imu_sample);
 
 	// legacy interface for compatibility (2018-09-14)
 	void setIMUData(uint64_t time_usec, uint64_t delta_ang_dt, uint64_t delta_vel_dt, float (&delta_ang)[3], float (&delta_vel)[3]);
 
-	// set magnetometer data
 	void setMagData(uint64_t time_usec, float (&data)[3]);
 
-	// set gps data
 	void setGpsData(uint64_t time_usec, const gps_message &gps);
 
-	// set baro data
 	void setBaroData(uint64_t time_usec, float data);
 
-	// set airspeed data
 	void setAirspeedData(uint64_t time_usec, float true_airspeed, float eas2tas);
 
-	// set range data
 	void setRangeData(uint64_t time_usec, float data, int8_t quality);
 
-	// set optical flow data
 	// if optical flow sensor gyro delta angles are not available, set gyroXYZ vector fields to NaN and the EKF will use its internal delta angle data instead
 	void setOpticalFlowData(uint64_t time_usec, flow_message *flow);
 
 	// set external vision position and attitude data
 	void setExtVisionData(uint64_t time_usec, ext_vision_message *evdata);
 
-	// set auxiliary velocity data
 	void setAuxVelData(uint64_t time_usec, const Vector3f &vel, const Vector3f &variance);
 
 	// return a address to the parameters struct
@@ -574,4 +566,5 @@ protected:
 	// calculate the inverse rotation matrix from a quaternion rotation
 	Matrix3f quat_to_invrotmat(const Quatf &quat);
 
+	void printBufferAllocationFailed(const char * buffer_name);
 };