Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-08 17:08:28 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_InertialSensor: create explicit define for AP_INERTIALSENSOR_BATCHSAMPLER_ENABLED

Browse Source 
this allows the batch sampler to be removed for size reasons, or because you don't have logging enabled
This commit is contained in:
Peter Barker 2023-01-03 16:51:51 +11:00 committed by Andrew Tridgell
parent c620ba58df
commit e5c3dd871c
6 changed files with 45 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
libraries/AP_InertialSensor/AP_InertialSensor.cpp
View File

@ -550,9 +550,11 @@ const AP_Param::GroupInfo AP_InertialSensor::var_info[] = {
// index 37 was NOTCH_
#if AP_INERTIALSENSOR_BATCHSAMPLER_ENABLED
// @Group: LOG_
// @Path: ../AP_InertialSensor/BatchSampler.cpp
AP_SUBGROUPINFO(batchsampler, "LOG_", 39, AP_InertialSensor, AP_InertialSensor::BatchSampler),
#endif
// @Param: ENABLE_MASK
// @DisplayName: IMU enable mask
@ -889,8 +891,10 @@ AP_InertialSensor::init(uint16_t loop_rate)
_last_sample_usec = 0;
_have_sample = false;
#if AP_INERTIALSENSOR_BATCHSAMPLER_ENABLED
// initialise IMU batch logging
batchsampler.init();
#endif
#if HAL_WITH_DSP
AP_GyroFFT* fft = AP::fft();
@ -1230,7 +1234,9 @@ AP_InertialSensor::detect_backends(void)
// ins_periodic: 57500 events, 0 overruns, 208754us elapsed, 3us avg, min 1us max 218us 40.662us rms
void AP_InertialSensor::periodic()
{
#if AP_INERTIALSENSOR_BATCHSAMPLER_ENABLED
batchsampler.periodic();
#endif
}

2
libraries/AP_InertialSensor/AP_InertialSensor.h
View File

@ -330,6 +330,7 @@ public:
IMU_SENSOR_TYPE_GYRO = 1,
};
#if AP_INERTIALSENSOR_BATCHSAMPLER_ENABLED
class BatchSampler {
public:
BatchSampler(const AP_InertialSensor &imu) :
@ -415,6 +416,7 @@ public:
const AP_InertialSensor &_imu;
};
BatchSampler batchsampler{*this};
#endif
#if HAL_EXTERNAL_AHRS_ENABLED
// handle external AHRS data

28
libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp
View File

@ -339,12 +339,17 @@ void AP_InertialSensor_Backend::_notify_new_gyro_raw_sample(uint8_t instance,
}
// 5us
#if AP_INERTIALSENSOR_BATCHSAMPLER_ENABLED
if (!_imu.batchsampler.doing_post_filter_logging()) {
log_gyro_raw(instance, sample_us, gyro);
}
else {
log_gyro_raw(instance, sample_us, _imu._gyro_filtered[instance]);
}
#else
// assume pre-filter logging if batchsampler is not enabled
log_gyro_raw(instance, sample_us, gyro);
#endif
}
/*
@ -431,12 +436,17 @@ void AP_InertialSensor_Backend::_notify_new_delta_angle(uint8_t instance, const
_imu._new_gyro_data[instance] = true;
}
#if AP_INERTIALSENSOR_BATCHSAMPLER_ENABLED
if (!_imu.batchsampler.doing_post_filter_logging()) {
log_gyro_raw(instance, sample_us, gyro);
}
else {
log_gyro_raw(instance, sample_us, _imu._gyro_filtered[instance]);
}
#else
// assume we're doing pre-filter logging:
log_gyro_raw(instance, sample_us, gyro);
#endif
}
void AP_InertialSensor_Backend::log_gyro_raw(uint8_t instance, const uint64_t sample_us, const Vector3f &gyro)
@ -450,9 +460,11 @@ void AP_InertialSensor_Backend::log_gyro_raw(uint8_t instance, const uint64_t sa
if (should_log_imu_raw()) {
Write_GYR(instance, sample_us, gyro);
} else {
#if AP_INERTIALSENSOR_BATCHSAMPLER_ENABLED
if (!_imu.batchsampler.doing_sensor_rate_logging()) {
_imu.batchsampler.sample(instance, AP_InertialSensor::IMU_SENSOR_TYPE_GYRO, sample_us, gyro);
}
#endif
}
#endif
}
@ -558,11 +570,16 @@ void AP_InertialSensor_Backend::_notify_new_accel_raw_sample(uint8_t instance,
}
// 5us
#if AP_INERTIALSENSOR_BATCHSAMPLER_ENABLED
if (!_imu.batchsampler.doing_post_filter_logging()) {
log_accel_raw(instance, sample_us, accel);
} else {
log_accel_raw(instance, sample_us, _imu._accel_filtered[instance]);
}
#else
// assume we're doing pre-filter logging:
log_accel_raw(instance, sample_us, accel);
#endif
}
/*
@ -630,29 +647,38 @@ void AP_InertialSensor_Backend::_notify_new_delta_velocity(uint8_t instance, con
_imu._new_accel_data[instance] = true;
}
#if AP_INERTIALSENSOR_BATCHSAMPLER_ENABLED
if (!_imu.batchsampler.doing_post_filter_logging()) {
log_accel_raw(instance, sample_us, accel);
} else {
log_accel_raw(instance, sample_us, _imu._accel_filtered[instance]);
}
#else
// assume we're doing pre-filter logging
log_accel_raw(instance, sample_us, accel);
#endif
}
void AP_InertialSensor_Backend::_notify_new_accel_sensor_rate_sample(uint8_t instance, const Vector3f &accel)
{
#if AP_INERTIALSENSOR_BATCHSAMPLER_ENABLED
if (!_imu.batchsampler.doing_sensor_rate_logging()) {
return;
}
_imu.batchsampler.sample(instance, AP_InertialSensor::IMU_SENSOR_TYPE_ACCEL, AP_HAL::micros64(), accel);
#endif
}
void AP_InertialSensor_Backend::_notify_new_gyro_sensor_rate_sample(uint8_t instance, const Vector3f &gyro)
{
#if AP_INERTIALSENSOR_BATCHSAMPLER_ENABLED
if (!_imu.batchsampler.doing_sensor_rate_logging()) {
return;
}
_imu.batchsampler.sample(instance, AP_InertialSensor::IMU_SENSOR_TYPE_GYRO, AP_HAL::micros64(), gyro);
#endif
}
void AP_InertialSensor_Backend::log_accel_raw(uint8_t instance, const uint64_t sample_us, const Vector3f &accel)
@ -666,9 +692,11 @@ void AP_InertialSensor_Backend::log_accel_raw(uint8_t instance, const uint64_t s
if (should_log_imu_raw()) {
Write_ACC(instance, sample_us, accel);
} else {
#if AP_INERTIALSENSOR_BATCHSAMPLER_ENABLED
if (!_imu.batchsampler.doing_sensor_rate_logging()) {
_imu.batchsampler.sample(instance, AP_InertialSensor::IMU_SENSOR_TYPE_ACCEL, sample_us, accel);
}
#endif
}
#endif
}

2
libraries/AP_InertialSensor/AP_InertialSensor_Logging.cpp
View File

@ -95,6 +95,7 @@ void AP_InertialSensor::Write_Vibration() const
}
}
#if AP_INERTIALSENSOR_BATCHSAMPLER_ENABLED
// Write information about a series of IMU readings to log:
bool AP_InertialSensor::BatchSampler::Write_ISBH(const float sample_rate_hz) const
{
@ -133,6 +134,7 @@ bool AP_InertialSensor::BatchSampler::Write_ISBD() const
return AP::logger().WriteBlock_first_succeed(&pkt, sizeof(pkt));
}
#endif
// @LoggerMessage: FTN
// @Description: Filter Tuning Message - per motor

5
libraries/AP_InertialSensor/AP_InertialSensor_config.h
View File

@ -1,7 +1,12 @@
#pragma once
#include <AP_HAL/AP_HAL_Boards.h>
#include <AP_Logger/AP_Logger_config.h>
#ifndef AP_INERTIALSENSOR_ENABLED
#define AP_INERTIALSENSOR_ENABLED 1
#endif
#ifndef AP_INERTIALSENSOR_BATCHSAMPLER_ENABLED
#define AP_INERTIALSENSOR_BATCHSAMPLER_ENABLED (AP_INERTIALSENSOR_ENABLED && HAL_LOGGING_ENABLED)
#endif

4
libraries/AP_InertialSensor/BatchSampler.cpp
View File

@ -1,6 +1,6 @@
#include "AP_InertialSensor.h"
#if AP_INERTIALSENSOR_ENABLED
#if AP_INERTIALSENSOR_BATCHSAMPLER_ENABLED
#include <GCS_MAVLink/GCS.h>
#include <AP_Logger/AP_Logger.h>
@ -282,4 +282,4 @@ void AP_InertialSensor::BatchSampler::sample(uint8_t _instance, AP_InertialSenso
data_write_offset++; // may unblock the reading process
#endif
}
#endif //#if AP_INERTIALSENSOR_ENABLED
#endif //#if AP_INERTIALSENSOR_BATCHSAMPLER_ENABLED