Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AC_PrecLand: rename BUFFER to LAG and add alloc failure check 

avoids issue where LAG parameter is set too large which would lead to trying to access unallocated memory
inertial buffer is only allocated at startup to avoid memory leak
This commit is contained in:
Randy Mackay 2018-09-19 11:15:58 +09:00
parent 02da34a246
commit 40d4bcb84a
2 changed files with 21 additions and 19 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

35
libraries/AC_PrecLand/AC_PrecLand.cpp
View File

@ -90,14 +90,15 @@ const AP_Param::GroupInfo AC_PrecLand::var_info[] = {
// @User: Advanced
AP_GROUPINFO("BUS", 8, AC_PrecLand, _bus, -1),
// @Param: BUFFER
// @DisplayName: Inertial History Buffer
// @Description: Length of inertial history buffer in ms, to cope with variable landing_target latency
// @Param: LAG
// @DisplayName: Precision Landing sensor lag
// @Description: Precision Landing sensor lag in ms, to cope with variable landing_target latency
// @Range: 1 250
// @Increment: 1
// @User: Advanced
// @Units: ms
AP_GROUPINFO("BUFFER", 9, AC_PrecLand, _inertial_buffer_length, 20), // 20ms is the old default buffer size (8 frames @ 400hz/2.5ms)
// @User: Advanced
// @RebootRequired: True
AP_GROUPINFO("LAG", 9, AC_PrecLand, _lag_ms, 20), // 20ms is the old default buffer size (8 frames @ 400hz/2.5ms)
AP_GROUPEND
};
@ -125,7 +126,7 @@ void AC_PrecLand::init()
_backend_state.healthy = false;
// Create inertial history buffer
inertial_buffer();
check_inertial_buffer();
/* NOTE: If this check below returns without creating buffer and backend then SITL crashes */
if (_inertial_history == nullptr) {
return; // allocation failed
@ -161,29 +162,31 @@ void AC_PrecLand::init()
}
}
// inertial_buffer - create/update inertial buffer
void AC_PrecLand::inertial_buffer()
// create inertial buffer
void AC_PrecLand::check_inertial_buffer()
{
// If PLND_LATENCY parameter is not within bounds, return without doing anything
if ( _inertial_buffer_length <= 0 || _inertial_buffer_length > 250 ) {
// if lag parameter is not within bounds, return without doing anything
if (_lag_ms <= 0 || _lag_ms > 250) {
return;
}
// calculate inertial buffer size from delay length (loop runs at 400hz)
_inertial_buffer_size = (uint16_t)roundf(_inertial_buffer_length / (1000/400.0f));
uint16_t inertial_buffer_size = (uint16_t)roundf(_lag_ms / (1000/400.0f));
// Create buffer if it doesn't exist or if latency parameter has changed
if (_inertial_history == nullptr || _inertial_buffer_size != _inertial_history->size()) {
// create buffer if it doesn't exist or if latency parameter has changed
if (_inertial_history == nullptr || inertial_buffer_size != _inertial_history->size()) {
// instantiate ring buffer to hold inertial history
_inertial_history = new ObjectArray<inertial_data_frame_s>(_inertial_buffer_size);
_inertial_history = new ObjectArray<inertial_data_frame_s>(inertial_buffer_size);
}
}
// update - give chance to driver to get updates from sensor
void AC_PrecLand::update(float rangefinder_alt_cm, bool rangefinder_alt_valid)
{
// Recreate inertial buffer if latency parameter has changed
inertial_buffer();
// exit immediately if not enabled
if (_backend == nullptr || _inertial_history == nullptr) {
return;
}
// append current velocity and attitude correction into history buffer
struct inertial_data_frame_s inertial_data_newest;

5
libraries/AC_PrecLand/AC_PrecLand.h
View File

@ -60,7 +60,7 @@ public:
uint32_t last_update_ms() const { return _last_update_ms; }
// create or update the inertial buffer
void inertial_buffer();
void check_inertial_buffer();
// returns time of last time target was seen
uint32_t last_backend_los_meas_ms() const { return _last_backend_los_meas_ms; }
@ -122,14 +122,13 @@ private:
AP_Int8 _type; // precision landing sensor type
AP_Int8 _bus; // which sensor bus
AP_Int8 _estimator_type; // precision landing estimator type
AP_Int16 _inertial_buffer_length; // inertial buffer length in ms
AP_Int16 _lag_ms; // sensor lag in ms
AP_Float _yaw_align; // Yaw angle from body x-axis to sensor x-axis.
AP_Float _land_ofs_cm_x; // Desired landing position of the camera forward of the target in vehicle body frame
AP_Float _land_ofs_cm_y; // Desired landing position of the camera right of the target in vehicle body frame
AP_Float _accel_noise; // accelometer process noise
AP_Vector3f _cam_offset; // Position of the camera relative to the CG
uint16_t _inertial_buffer_size; // inertial buffer queue length, calculated from _inertial_buffer_length
uint32_t _last_update_ms; // system time in millisecond when update was last called
bool _target_acquired; // true if target has been seen recently
uint32_t _last_backend_los_meas_ms; // system time target was last seen