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

AP_InertialSensor: fix hardfault in BatchSampler

This commit is contained in:
bugobliterator 2023-04-04 18:09:18 +10:00 committed by Randy Mackay
parent 959b520b27
commit 99ff763995
3 changed files with 10 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
libraries/AP_InertialSensor/AP_InertialSensor.h
View File

@ -349,6 +349,7 @@ public:
// Parameters // Parameters
AP_Int16 _required_count; AP_Int16 _required_count;
uint16_t _real_required_count;
AP_Int8 _sensor_mask; AP_Int8 _sensor_mask;
AP_Int8 _batch_options_mask; AP_Int8 _batch_options_mask;

2
libraries/AP_InertialSensor/AP_InertialSensor_Logging.cpp
View File

@ -110,7 +110,7 @@ bool AP_InertialSensor::BatchSampler::Write_ISBH(const float sample_rate_hz) con
sensor_type : (uint8_t)type, sensor_type : (uint8_t)type,
instance : instance_to_write, instance : instance_to_write,
multiplier : multiplier, multiplier : multiplier,
sample_count : (uint16_t)_required_count, sample_count : (uint16_t)_real_required_count,
sample_us : measurement_started_us, sample_us : measurement_started_us,
sample_rate_hz : sample_rate_hz, sample_rate_hz : sample_rate_hz,
}; };

14
libraries/AP_InertialSensor/BatchSampler.cpp
View File

@ -58,12 +58,14 @@ void AP_InertialSensor::BatchSampler::init()
_required_count.set(_required_count - (_required_count % 32)); // round down to nearest multiple of 32 _required_count.set(_required_count - (_required_count % 32)); // round down to nearest multiple of 32
const uint32_t total_allocation = 3*_required_count*sizeof(uint16_t); _real_required_count = _required_count;
const uint32_t total_allocation = 3*_real_required_count*sizeof(uint16_t);
GCS_SEND_TEXT(MAV_SEVERITY_DEBUG, "INS: alloc %u bytes for ISB (free=%u)", (unsigned int)total_allocation, (unsigned int)hal.util->available_memory()); GCS_SEND_TEXT(MAV_SEVERITY_DEBUG, "INS: alloc %u bytes for ISB (free=%u)", (unsigned int)total_allocation, (unsigned int)hal.util->available_memory());
data_x = (int16_t*)calloc(_required_count, sizeof(int16_t)); data_x = (int16_t*)calloc(_real_required_count, sizeof(int16_t));
data_y = (int16_t*)calloc(_required_count, sizeof(int16_t)); data_y = (int16_t*)calloc(_real_required_count, sizeof(int16_t));
data_z = (int16_t*)calloc(_required_count, sizeof(int16_t)); data_z = (int16_t*)calloc(_real_required_count, sizeof(int16_t));
if (data_x == nullptr || data_y == nullptr || data_z == nullptr) { if (data_x == nullptr || data_y == nullptr || data_z == nullptr) {
free(data_x); free(data_x);
free(data_y); free(data_y);
@ -226,7 +228,7 @@ void AP_InertialSensor::BatchSampler::push_data_to_log()
data_read_offset += samples_per_msg; data_read_offset += samples_per_msg;
last_sent_ms = AP_HAL::millis(); last_sent_ms = AP_HAL::millis();
if (data_read_offset >= _required_count) { if (data_read_offset >= _real_required_count) {
// that was the last one. Clean up: // that was the last one. Clean up:
data_read_offset = 0; data_read_offset = 0;
isb_seqnum++; isb_seqnum++;
@ -251,7 +253,7 @@ bool AP_InertialSensor::BatchSampler::should_log(uint8_t _instance, IMU_SENSOR_T
if (_type != type) { if (_type != type) {
return false; return false;
} }
if (data_write_offset >= _required_count) { if (data_write_offset >= _real_required_count) {
return false; return false;
} }
AP_Logger *logger = AP_Logger::get_singleton(); AP_Logger *logger = AP_Logger::get_singleton();