Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-12 10:53:59 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_InertialSensor: only use dynamic FIFO when using fast rate loop

Browse Source
This commit is contained in:
Andy Piper 2024-12-04 20:22:15 +00:00 committed by Andrew Tridgell
parent 6eda43f9f4
commit 981826dca3
8 changed files with 23 additions and 13 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
libraries/AP_InertialSensor/AP_InertialSensor.h
View File

@ -831,6 +831,8 @@ public:
void update_backend_filters();
// are rate loop samples enabled for this instance?
bool is_rate_loop_gyro_enabled(uint8_t instance) const;
// is dynamic fifo enabled for this instance
bool is_dynamic_fifo_enabled(uint8_t instance) const;
// endif AP_INERTIALSENSOR_FAST_SAMPLE_WINDOW_ENABLED
};

1
libraries/AP_InertialSensor/AP_InertialSensor_Invensense.cpp
View File

@ -26,6 +26,7 @@
#include <AP_InternalError/AP_InternalError.h>
#include <AP_Logger/AP_Logger.h>
#include "AP_InertialSensor_rate_config.h"
#include "AP_InertialSensor_Invensense.h"
#include <GCS_MAVLink/GCS.h>

1
libraries/AP_InertialSensor/AP_InertialSensor_Invensensev2.cpp
View File

@ -23,6 +23,7 @@
#include <AP_HAL/AP_HAL.h>
#include "AP_InertialSensor_rate_config.h"
#include "AP_InertialSensor_Invensensev2.h"
extern const AP_HAL::HAL& hal;

7
libraries/AP_InertialSensor/AP_InertialSensor_Invensensev3.cpp
View File

@ -31,6 +31,7 @@
*/
#include <AP_HAL/AP_HAL.h>
#include "AP_InertialSensor_rate_config.h"
#include "AP_InertialSensor_Invensensev3.h"
#include <utility>
#include <stdio.h>
@ -400,18 +401,18 @@ bool AP_InertialSensor_Invensensev3::update()
return true;
}
#if AP_INERTIALSENSOR_DYNAMIC_FIFO
void AP_InertialSensor_Invensensev3::set_primary_gyro(bool is_primary)
{
if (((1U<<gyro_instance) & AP_INERTIALSENSOR_DYNAMIC_FIFO_MASK) && enable_fast_sampling(gyro_instance)) {
#if AP_INERTIALSENSOR_FAST_SAMPLE_WINDOW_ENABLED
if (_imu.is_dynamic_fifo_enabled(gyro_instance)) {
if (is_primary) {
dev->adjust_periodic_callback(periodic_handle, backend_period_us);
} else {
dev->adjust_periodic_callback(periodic_handle, backend_period_us * get_fast_sampling_rate());
}
}
}
#endif
}
/*
accumulate new samples

2
libraries/AP_InertialSensor/AP_InertialSensor_Invensensev3.h
View File

@ -46,9 +46,7 @@ public:
const uint16_t multiplier_accel = INT16_MAX/(32*GRAVITY_MSS);
protected:
#if AP_INERTIALSENSOR_DYNAMIC_FIFO
void set_primary_gyro(bool is_primary) override;
#endif
private:
AP_InertialSensor_Invensensev3(AP_InertialSensor &imu,

8
libraries/AP_InertialSensor/AP_InertialSensor_config.h
View File

@ -73,11 +73,3 @@
#ifndef AP_INERTIALSENSOR_KILL_IMU_ENABLED
#define AP_INERTIALSENSOR_KILL_IMU_ENABLED 1
#endif
#ifndef AP_INERTIALSENSOR_DYNAMIC_FIFO
#define AP_INERTIALSENSOR_DYNAMIC_FIFO 1
#endif
#ifndef AP_INERTIALSENSOR_DYNAMIC_FIFO_MASK
#define AP_INERTIALSENSOR_DYNAMIC_FIFO_MASK 127
#endif

4
libraries/AP_InertialSensor/AP_InertialSensor_rate_config.h
View File

@ -7,3 +7,7 @@
#ifndef AP_INERTIALSENSOR_FAST_SAMPLE_WINDOW_ENABLED
#define AP_INERTIALSENSOR_FAST_SAMPLE_WINDOW_ENABLED (AP_INERTIALSENSOR_ENABLED && HAL_INS_RATE_LOOP && AP_INERTIALSENSOR_HARMONICNOTCH_ENABLED && APM_BUILD_TYPE(APM_BUILD_ArduCopter))
#endif
#ifndef AP_INERTIALSENSOR_DYNAMIC_FIFO_MASK
#define AP_INERTIALSENSOR_DYNAMIC_FIFO_MASK 127
#endif

11
libraries/AP_InertialSensor/FastRateBuffer.cpp
View File

@ -79,6 +79,17 @@ bool AP_InertialSensor::is_rate_loop_gyro_enabled(uint8_t instance) const
return fast_rate_buffer->use_rate_loop_gyro_samples() && instance == AP::ahrs().get_primary_gyro_index();
}
// whether or not to use the dynamic fifo
bool AP_InertialSensor::is_dynamic_fifo_enabled(uint8_t instance) const
{
if (!fast_rate_buffer_enabled || fast_rate_buffer == nullptr) {
return false;
}
return ((1U<<instance) & AP_INERTIALSENSOR_DYNAMIC_FIFO_MASK)
&& (_fast_sampling_mask & (1U<<instance)) != 0
&& fast_rate_buffer->use_rate_loop_gyro_samples();
}
// get the next available gyro sample from the fast rate buffer
bool AP_InertialSensor::get_next_gyro_sample(Vector3f& gyro)
{