From 99ff76399574f63828cc5559c865f139ae355aaf Mon Sep 17 00:00:00 2001 From: bugobliterator Date: Tue, 4 Apr 2023 18:09:18 +1000 Subject: [PATCH] AP_InertialSensor: fix hardfault in BatchSampler --- libraries/AP_InertialSensor/AP_InertialSensor.h | 1 + .../AP_InertialSensor_Logging.cpp | 2 +- libraries/AP_InertialSensor/BatchSampler.cpp | 14 ++++++++------ 3 files changed, 10 insertions(+), 7 deletions(-) diff --git a/libraries/AP_InertialSensor/AP_InertialSensor.h b/libraries/AP_InertialSensor/AP_InertialSensor.h index ee0b0046af..ba6afdb86c 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor.h +++ b/libraries/AP_InertialSensor/AP_InertialSensor.h @@ -349,6 +349,7 @@ public: // Parameters AP_Int16 _required_count; + uint16_t _real_required_count; AP_Int8 _sensor_mask; AP_Int8 _batch_options_mask; diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Logging.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_Logging.cpp index bd54f3135d..742932691f 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_Logging.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor_Logging.cpp @@ -110,7 +110,7 @@ bool AP_InertialSensor::BatchSampler::Write_ISBH(const float sample_rate_hz) con sensor_type : (uint8_t)type, instance : instance_to_write, multiplier : multiplier, - sample_count : (uint16_t)_required_count, + sample_count : (uint16_t)_real_required_count, sample_us : measurement_started_us, sample_rate_hz : sample_rate_hz, }; diff --git a/libraries/AP_InertialSensor/BatchSampler.cpp b/libraries/AP_InertialSensor/BatchSampler.cpp index a7914d5a00..376331cd1e 100644 --- a/libraries/AP_InertialSensor/BatchSampler.cpp +++ b/libraries/AP_InertialSensor/BatchSampler.cpp @@ -58,12 +58,14 @@ void AP_InertialSensor::BatchSampler::init() _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()); - data_x = (int16_t*)calloc(_required_count, sizeof(int16_t)); - data_y = (int16_t*)calloc(_required_count, sizeof(int16_t)); - data_z = (int16_t*)calloc(_required_count, sizeof(int16_t)); + data_x = (int16_t*)calloc(_real_required_count, sizeof(int16_t)); + data_y = (int16_t*)calloc(_real_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) { free(data_x); free(data_y); @@ -226,7 +228,7 @@ void AP_InertialSensor::BatchSampler::push_data_to_log() data_read_offset += samples_per_msg; 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: data_read_offset = 0; isb_seqnum++; @@ -251,7 +253,7 @@ bool AP_InertialSensor::BatchSampler::should_log(uint8_t _instance, IMU_SENSOR_T if (_type != type) { return false; } - if (data_write_offset >= _required_count) { + if (data_write_offset >= _real_required_count) { return false; } AP_Logger *logger = AP_Logger::get_singleton();