EKF: allocate and unallocate buffer more robustly

EKF/RingBuffer.h

@@ -52,7 +52,7 @@ public:
 		_head = _tail = _size = 0;
 		_first_write = true;
 	}
-	~RingBuffer() {delete _buffer;}
+	~RingBuffer() { delete[] _buffer; }
 
 	bool allocate(int size)
 	{
@@ -61,7 +61,7 @@ public:
 		}
 
 		if (_buffer != NULL) {
-			delete _buffer;
+			delete[] _buffer;
 		}
 
 		_buffer = new data_type[size];
@@ -75,6 +75,13 @@ public:
 		return true;
 	}
 
+	void unallocate()
+	{
+		if (_buffer != NULL) {
+			delete[] _buffer;
+		}
+	}
+
 	inline void push(data_type sample, bool debug = false)
 	{
 		if (debug) {

EKF/estimator_interface.cpp

@@ -203,14 +203,19 @@ void EstimatorInterface::setOpticalFlowData(uint64_t time_usec, float *data)
 
 bool EstimatorInterface::initialise_interface(uint64_t timestamp)
 {
-	_imu_buffer.allocate(IMU_BUFFER_LENGTH);
+
-	_gps_buffer.allocate(OBS_BUFFER_LENGTH);
+	if(!(_imu_buffer.allocate(IMU_BUFFER_LENGTH) &&
-	_mag_buffer.allocate(OBS_BUFFER_LENGTH);
+		_gps_buffer.allocate(OBS_BUFFER_LENGTH) &&
-	_baro_buffer.allocate(OBS_BUFFER_LENGTH);
+		_mag_buffer.allocate(OBS_BUFFER_LENGTH) &&
-	_range_buffer.allocate(OBS_BUFFER_LENGTH);
+		_baro_buffer.allocate(OBS_BUFFER_LENGTH) &&
-	_airspeed_buffer.allocate(OBS_BUFFER_LENGTH);
+		_range_buffer.allocate(OBS_BUFFER_LENGTH) &&
-	_flow_buffer.allocate(OBS_BUFFER_LENGTH);
+		_airspeed_buffer.allocate(OBS_BUFFER_LENGTH) &&
-	_output_buffer.allocate(IMU_BUFFER_LENGTH);
+		_flow_buffer.allocate(OBS_BUFFER_LENGTH) &&
+		_output_buffer.allocate(IMU_BUFFER_LENGTH))) {
+		PX4_WARN("Estimator Buffer Allocation failed!");
+		unallocate_buffers();
+		return false;
+	}
 
 
 	_dt_imu_avg = 0.0f;
@@ -237,6 +242,19 @@ bool EstimatorInterface::initialise_interface(uint64_t timestamp)
 	return true;
 }
 
+void EstimatorInterface::unallocate_buffers()
+{
+	_imu_buffer.unallocate();
+	_gps_buffer.unallocate();
+	_mag_buffer.unallocate();
+	_baro_buffer.unallocate();
+	_range_buffer.unallocate();
+	_airspeed_buffer.unallocate();
+	_flow_buffer.unallocate();
+	_output_buffer.unallocate();
+
+}
+
 bool EstimatorInterface::position_is_valid()
 {
 	// return true if the position estimate is valid

EKF/estimator_interface.h

@@ -224,4 +224,5 @@ protected:
 
 	fault_status_t _fault_status;
 	bool initialise_interface(uint64_t timestamp);
+	void unallocate_buffers();
 };