From 13cdc8bda895d8f6b1b6340ffe2272d14e0c55df 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 4c4a839d8d..db108eb199 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor.h +++ b/libraries/AP_InertialSensor/AP_InertialSensor.h @@ -329,6 +329,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 48f7c41050..cbf5f8db68 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_Logging.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor_Logging.cpp @@ -111,7 +111,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 a125f1a286..e86a3f559c 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();