fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-30 06:53:33 +08:00
Code Issues Packages Projects Releases Wiki Activity

EKF: allocate and unallocate buffer more robustly

Browse Source
This commit is contained in:
bugobliterator
2016-01-31 00:12:16 -08:00
parent c6249a2825
commit 8200ef4d17
3 changed files with 36 additions and 10 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EKF/RingBuffer.h
+9 -2
View File
@@ -52,7 +52,7 @@ public:
_head = _tail = _size = 0; _head = _tail = _size = 0;
_first_write = true; _first_write = true;
} }
~RingBuffer() {delete _buffer;} ~RingBuffer() { delete[] _buffer; }
bool allocate(int size) bool allocate(int size)
{ {
@@ -61,7 +61,7 @@ public:
} }
if (_buffer != NULL) { if (_buffer != NULL) {
delete _buffer; delete[] _buffer;
} }
_buffer = new data_type[size]; _buffer = new data_type[size];
@@ -75,6 +75,13 @@ public:
return true; return true;
} }
void unallocate()
{
if (_buffer != NULL) {
delete[] _buffer;
}
}
inline void push(data_type sample, bool debug = false) inline void push(data_type sample, bool debug = false)
{ {
if (debug) { if (debug) {
EKF/estimator_interface.cpp
+26 -8
View File
@@ -203,14 +203,19 @@ void EstimatorInterface::setOpticalFlowData(uint64_t time_usec, float *data)
bool EstimatorInterface::initialise_interface(uint64_t timestamp) 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; _dt_imu_avg = 0.0f;
@@ -237,6 +242,19 @@ bool EstimatorInterface::initialise_interface(uint64_t timestamp)
return true; 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() bool EstimatorInterface::position_is_valid()
{ {
// return true if the position estimate is valid // return true if the position estimate is valid
EKF/estimator_interface.h
+1
View File
@@ -224,4 +224,5 @@ protected:
fault_status_t _fault_status; fault_status_t _fault_status;
bool initialise_interface(uint64_t timestamp); bool initialise_interface(uint64_t timestamp);
void unallocate_buffers();
}; };