Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-03-13 10:03:57 -03:00
Code Issues Packages Projects Releases Wiki Activity

AP_InertialSensor: belt and braces checks

Browse Source
This commit is contained in:
Andy Piper 2024-11-16 16:30:46 +00:00 committed by Andrew Tridgell
parent 277386e6e2
commit 88f6125a94
1 changed files with 9 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

15
libraries/AP_InertialSensor/FastRateBuffer.cpp
View File

@ -40,7 +40,7 @@ void AP_InertialSensor::enable_fast_rate_buffer()
if (fast_rate_buffer == nullptr) { if (fast_rate_buffer == nullptr) {
fast_rate_buffer = NEW_NOTHROW FastRateBuffer(); fast_rate_buffer = NEW_NOTHROW FastRateBuffer();
} }
fast_rate_buffer_enabled = true; fast_rate_buffer_enabled = fast_rate_buffer != nullptr;
} }
// disable the fast rate buffer and stop pushing samples to it // disable the fast rate buffer and stop pushing samples to it
@ -55,7 +55,7 @@ void AP_InertialSensor::disable_fast_rate_buffer()
// get the number of available gyro samples in the fast rate buffer // get the number of available gyro samples in the fast rate buffer
uint32_t AP_InertialSensor::get_num_gyro_samples() uint32_t AP_InertialSensor::get_num_gyro_samples()
{ {
if (fast_rate_buffer == nullptr) { if (!fast_rate_buffer_enabled || fast_rate_buffer == nullptr) {
return 0; return 0;
} }
return fast_rate_buffer->get_num_gyro_samples(); return fast_rate_buffer->get_num_gyro_samples();
@ -64,7 +64,7 @@ uint32_t AP_InertialSensor::get_num_gyro_samples()
// set the rate at which samples are collected, unused samples are dropped // set the rate at which samples are collected, unused samples are dropped
void AP_InertialSensor::set_rate_decimation(uint8_t rdec) void AP_InertialSensor::set_rate_decimation(uint8_t rdec)
{ {
if (fast_rate_buffer == nullptr) { if (!fast_rate_buffer_enabled || fast_rate_buffer == nullptr) {
return; return;
} }
fast_rate_buffer->set_rate_decimation(rdec); fast_rate_buffer->set_rate_decimation(rdec);
@ -73,13 +73,16 @@ void AP_InertialSensor::set_rate_decimation(uint8_t rdec)
// whether or not to push the current gyro sample // whether or not to push the current gyro sample
bool AP_InertialSensor::is_rate_loop_gyro_enabled(uint8_t instance) const bool AP_InertialSensor::is_rate_loop_gyro_enabled(uint8_t instance) const
{ {
return fast_rate_buffer_enabled && fast_rate_buffer->use_rate_loop_gyro_samples() && instance == AP::ahrs().get_primary_gyro_index(); if (!fast_rate_buffer_enabled || fast_rate_buffer == nullptr) {
return false;
}
return fast_rate_buffer->use_rate_loop_gyro_samples() && instance == AP::ahrs().get_primary_gyro_index();
} }
// get the next available gyro sample from the fast rate buffer // get the next available gyro sample from the fast rate buffer
bool AP_InertialSensor::get_next_gyro_sample(Vector3f& gyro) bool AP_InertialSensor::get_next_gyro_sample(Vector3f& gyro)
{ {
if (!fast_rate_buffer_enabled) { if (!fast_rate_buffer_enabled || fast_rate_buffer == nullptr) {
return false; return false;
} }
@ -109,7 +112,7 @@ void FastRateBuffer::reset()
bool AP_InertialSensor::push_next_gyro_sample(const Vector3f& gyro) bool AP_InertialSensor::push_next_gyro_sample(const Vector3f& gyro)
{ {
if (fast_rate_buffer == nullptr) { if (!fast_rate_buffer_enabled || fast_rate_buffer == nullptr) {
return false; return false;
} }