aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_InertialSensor: SITL the raw sample rate is not the same as the sensor rate

Browse Source 
use regulated time for frequency noise to avoid spurious harmonics
SITL sensors must be true separate instances
don't compile in FFT structures if DSP disabled
This commit is contained in:
Andy Piper 2020-01-03 19:52:33 +00:00 committed by Andrew Tridgell
parent 83b1c3a0bd
commit 0e9b2a26c5
5 changed files with 97 additions and 94 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
libraries/AP_InertialSensor/AP_InertialSensor.cpp
View File

@ -656,6 +656,7 @@ AP_InertialSensor_Backend *AP_InertialSensor::_find_backend(int16_t backend_id,
}
bool AP_InertialSensor::set_gyro_window_size(uint16_t size) {
#if HAL_WITH_DSP
_gyro_window_size = size;
// allocate FFT gyro window
@ -676,7 +677,7 @@ bool AP_InertialSensor::set_gyro_window_size(uint16_t size) {
}
}
}
#endif
return true;
}
@ -797,7 +798,8 @@ AP_InertialSensor::detect_backends(void)
// IMUs defined by IMU lines in hwdef.dat
HAL_INS_PROBE_LIST;
#elif CONFIG_HAL_BOARD == HAL_BOARD_SITL
ADD_BACKEND(AP_InertialSensor_SITL::detect(*this));
ADD_BACKEND(AP_InertialSensor_SITL::detect(*this, INS_SITL_SENSOR_A));
ADD_BACKEND(AP_InertialSensor_SITL::detect(*this, INS_SITL_SENSOR_B));
#elif HAL_INS_DEFAULT == HAL_INS_HIL
ADD_BACKEND(AP_InertialSensor_HIL::detect(*this));
#elif AP_FEATURE_BOARD_DETECT

6
libraries/AP_InertialSensor/AP_InertialSensor.h
View File

@ -153,6 +153,7 @@ public:
uint16_t get_accel_rate_hz(uint8_t instance) const { return uint16_t(_accel_raw_sample_rates[instance] * _accel_over_sampling[instance]); }
// FFT support access
#if HAL_WITH_DSP
const Vector3f &get_raw_gyro(void) const { return _gyro_raw[_primary_gyro]; }
GyroWindow get_raw_gyro_window(uint8_t instance, uint8_t axis) const { return _gyro_window[instance][axis]; }
GyroWindow get_raw_gyro_window(uint8_t axis) const { return get_raw_gyro_window(_primary_gyro, axis); }
@ -161,8 +162,8 @@ public:
uint16_t get_raw_gyro_window_index(uint8_t instance) const { return _circular_buffer_idx[instance]; }
uint16_t get_raw_gyro_rate_hz() const { return get_raw_gyro_rate_hz(_primary_gyro); }
uint16_t get_raw_gyro_rate_hz(uint8_t instance) const { return _gyro_raw_sample_rates[_primary_gyro]; }
#endif
bool set_gyro_window_size(uint16_t size);
// get accel offsets in m/s/s
const Vector3f &get_accel_offsets(uint8_t i) const { return _accel_offset[i]; }
const Vector3f &get_accel_offsets(void) const { return get_accel_offsets(_primary_accel); }
@ -441,13 +442,14 @@ private:
LowPassFilter2pVector3f _gyro_filter[INS_MAX_INSTANCES];
Vector3f _accel_filtered[INS_MAX_INSTANCES];
Vector3f _gyro_filtered[INS_MAX_INSTANCES];
#if HAL_WITH_DSP
// Thread-safe public version of _last_raw_gyro
Vector3f _gyro_raw[INS_MAX_INSTANCES];
// circular buffer of gyro data for frequency analysis
uint16_t _circular_buffer_idx[INS_MAX_INSTANCES];
GyroWindow _gyro_window[INS_MAX_INSTANCES][XYZ_AXIS_COUNT];
uint16_t _gyro_window_size;
#endif
bool _new_accel_data[INS_MAX_INSTANCES];
bool _new_gyro_data[INS_MAX_INSTANCES];

6
libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp
View File

@ -225,11 +225,11 @@ void AP_InertialSensor_Backend::_notify_new_gyro_raw_sample(uint8_t instance,
// save previous delta angle for coning correction
_imu._last_delta_angle[instance] = delta_angle;
_imu._last_raw_gyro[instance] = gyro;
#if HAL_WITH_DSP
// capture gyro window for FFT analysis
_last_gyro_window[_num_gyro_samples++] = gyro * _imu._gyro_raw_sampling_multiplier[instance];
_num_gyro_samples = _num_gyro_samples % INS_MAX_GYRO_WINDOW_SAMPLES; // protect against overrun
#endif
// apply the low pass filter
Vector3f gyro_filtered = _imu._gyro_filter[instance].apply(gyro);
@ -514,6 +514,7 @@ void AP_InertialSensor_Backend::update_gyro(uint8_t instance)
if (_imu._new_gyro_data[instance]) {
_publish_gyro(instance, _imu._gyro_filtered[instance]);
// copy the gyro samples from the backend to the frontend window
#if HAL_WITH_DSP
if (_imu._gyro_window_size > 0) {
uint8_t idx = _imu._circular_buffer_idx[instance];
for (uint8_t i = 0; i < _num_gyro_samples; i++) {
@ -526,6 +527,7 @@ void AP_InertialSensor_Backend::update_gyro(uint8_t instance)
_imu._circular_buffer_idx[instance] = idx;
}
_imu._gyro_raw[instance] = _imu._last_raw_gyro[instance] * _imu._gyro_raw_sampling_multiplier[instance];
#endif
_imu._new_gyro_data[instance] = false;
}

139
libraries/AP_InertialSensor/AP_InertialSensor_SITL.cpp
View File

@ -7,17 +7,19 @@
const extern AP_HAL::HAL& hal;
AP_InertialSensor_SITL::AP_InertialSensor_SITL(AP_InertialSensor &imu) :
AP_InertialSensor_Backend(imu)
AP_InertialSensor_SITL::AP_InertialSensor_SITL(AP_InertialSensor &imu, const uint16_t sample_rates[]) :
AP_InertialSensor_Backend(imu),
gyro_sample_hz(sample_rates[0]),
accel_sample_hz(sample_rates[1])
{
}
/*
detect the sensor
*/
AP_InertialSensor_Backend *AP_InertialSensor_SITL::detect(AP_InertialSensor &_imu)
AP_InertialSensor_Backend *AP_InertialSensor_SITL::detect(AP_InertialSensor &_imu, const uint16_t sample_rates[])
{
AP_InertialSensor_SITL *sensor = new AP_InertialSensor_SITL(_imu);
AP_InertialSensor_SITL *sensor = new AP_InertialSensor_SITL(_imu, sample_rates);
if (sensor == nullptr) {
return nullptr;
}
@ -34,24 +36,6 @@ bool AP_InertialSensor_SITL::init_sensor(void)
if (sitl == nullptr) {
return false;
}
// grab the used instances
for (uint8_t i=0; i<INS_SITL_INSTANCES; i++) {
gyro_instance[i] = _imu.register_gyro(gyro_sample_hz[i],
AP_HAL::Device::make_bus_id(AP_HAL::Device::BUS_TYPE_SITL, i, 1, DEVTYPE_SITL));
accel_instance[i] = _imu.register_accel(accel_sample_hz[i],
AP_HAL::Device::make_bus_id(AP_HAL::Device::BUS_TYPE_SITL, i, 2, DEVTYPE_SITL));
if (enable_fast_sampling(accel_instance[i])) {
_set_accel_raw_sample_rate(accel_instance[i], accel_sample_hz[i]*4);
}
if (enable_fast_sampling(gyro_instance[i])) {
_set_gyro_raw_sample_rate(gyro_instance[i], gyro_sample_hz[i]*8);
}
}
hal.scheduler->register_timer_process(FUNCTOR_BIND_MEMBER(&AP_InertialSensor_SITL::timer_update, void));
return true;
}
@ -63,14 +47,14 @@ static float calculate_noise(float noise, float noise_variation) {
/*
generate an accelerometer sample
*/
void AP_InertialSensor_SITL::generate_accel(uint8_t instance)
void AP_InertialSensor_SITL::generate_accel()
{
Vector3f accel_accum;
uint8_t nsamples = enable_fast_sampling(accel_instance[instance]) ? 4 : 1;
uint8_t nsamples = enable_fast_sampling(accel_instance) ? 4 : 1;
for (uint8_t j = 0; j < nsamples; j++) {
// add accel bias and noise
Vector3f accel_bias = instance == 0 ? sitl->accel_bias.get() : sitl->accel2_bias.get();
Vector3f accel_bias = accel_instance == 0 ? sitl->accel_bias.get() : sitl->accel2_bias.get();
float xAccel = sitl->state.xAccel + accel_bias.x;
float yAccel = sitl->state.yAccel + accel_bias.y;
float zAccel = sitl->state.zAccel + accel_bias.z;
@ -78,9 +62,9 @@ void AP_InertialSensor_SITL::generate_accel(uint8_t instance)
// minimum noise levels are 2 bits, but averaged over many
// samples, giving around 0.01 m/s/s
float accel_noise = 0.01f;
float noise_variation = 0.05;
float noise_variation = 0.05f;
// this smears the individual motor peaks somewhat emulating physical motors
float freq_variation = 0.12;
float freq_variation = 0.12f;
xAccel += accel_noise * rand_float();
yAccel += accel_noise * rand_float();
@ -91,24 +75,24 @@ void AP_InertialSensor_SITL::generate_accel(uint8_t instance)
// giving a accel noise variation of 5.33 m/s/s over the full throttle range
if (motors_on) {
// add extra noise when the motors are on
accel_noise = (instance == 0 ? sitl->accel_noise : sitl->accel2_noise) * sitl->throttle;
accel_noise = (accel_instance == 0 ? sitl->accel_noise : sitl->accel2_noise) * sitl->throttle;
}
// VIB_FREQ is a static vibration applied to each axis
const Vector3f &vibe_freq = sitl->vibe_freq;
if (!vibe_freq.is_zero() && motors_on) {
float t = AP_HAL::micros() * 1.0e-6f;
xAccel += sinf(t * 2 * M_PI * vibe_freq.x) * calculate_noise(accel_noise, noise_variation);
yAccel += sinf(t * 2 * M_PI * vibe_freq.y) * calculate_noise(accel_noise, noise_variation);
zAccel += sinf(t * 2 * M_PI * vibe_freq.z) * calculate_noise(accel_noise, noise_variation);
xAccel += sinf(accel_time * 2 * M_PI * vibe_freq.x) * calculate_noise(accel_noise, noise_variation);
yAccel += sinf(accel_time * 2 * M_PI * vibe_freq.y) * calculate_noise(accel_noise, noise_variation);
zAccel += sinf(accel_time * 2 * M_PI * vibe_freq.z) * calculate_noise(accel_noise, noise_variation);
accel_time += 1.0f / (accel_sample_hz * nsamples);
}
// VIB_MOT_MAX is a rpm-scaled vibration applied to each axis
if (!is_zero(sitl->vibe_motor) && motors_on) {
for (uint8_t i = 0; i < sitl->state.num_motors; i++) {
float &phase = accel_motor_phase[instance][i];
float &phase = accel_motor_phase[i];
float motor_freq = calculate_noise(sitl->state.rpm[i] / 60.0f, freq_variation);
float phase_incr = motor_freq * 2 * M_PI / (accel_sample_hz[instance] * nsamples);
float phase_incr = motor_freq * 2 * M_PI / (accel_sample_hz * nsamples);
phase += phase_incr;
if (phase_incr > M_PI) {
phase -= 2 * M_PI;
@ -150,25 +134,26 @@ void AP_InertialSensor_SITL::generate_accel(uint8_t instance)
Vector3f accel = Vector3f(xAccel, yAccel, zAccel);
_rotate_and_correct_accel(accel_instance[instance], accel);
_notify_new_accel_sensor_rate_sample(instance, accel);
_notify_new_accel_sensor_rate_sample(accel_instance, accel);
accel_accum += accel;
}
accel_accum /= nsamples;
_notify_new_accel_raw_sample(accel_instance[instance], accel_accum);
_rotate_and_correct_accel(accel_instance, accel_accum);
_notify_new_accel_raw_sample(accel_instance, accel_accum);
_publish_temperature(instance, 23);
_publish_temperature(accel_instance, 23);
}
/*
generate a gyro sample
*/
void AP_InertialSensor_SITL::generate_gyro(uint8_t instance)
void AP_InertialSensor_SITL::generate_gyro()
{
Vector3f gyro_accum;
uint8_t nsamples = enable_fast_sampling(gyro_instance[instance]) ? 8 : 1;
uint8_t nsamples = enable_fast_sampling(gyro_instance) ? 8 : 1;
for (uint8_t j = 0; j < nsamples; j++) {
float p = radians(sitl->state.rollRate) + gyro_drift();
float q = radians(sitl->state.pitchRate) + gyro_drift();
@ -195,18 +180,18 @@ void AP_InertialSensor_SITL::generate_gyro(uint8_t instance)
// VIB_FREQ is a static vibration applied to each axis
const Vector3f &vibe_freq = sitl->vibe_freq;
if (!vibe_freq.is_zero() && motors_on) {
float t = AP_HAL::micros() * 1.0e-6f;
p += sinf(t * 2 * M_PI * vibe_freq.x) * calculate_noise(gyro_noise, noise_variation);
q += sinf(t * 2 * M_PI * vibe_freq.y) * calculate_noise(gyro_noise, noise_variation);
r += sinf(t * 2 * M_PI * vibe_freq.z) * calculate_noise(gyro_noise, noise_variation);
p += sinf(gyro_time * 2 * M_PI * vibe_freq.x) * calculate_noise(gyro_noise, noise_variation);
q += sinf(gyro_time * 2 * M_PI * vibe_freq.y) * calculate_noise(gyro_noise, noise_variation);
r += sinf(gyro_time * 2 * M_PI * vibe_freq.z) * calculate_noise(gyro_noise, noise_variation);
gyro_time += 1.0f / (gyro_sample_hz * nsamples);
}
// VIB_MOT_MAX is a rpm-scaled vibration applied to each axis
if (!is_zero(sitl->vibe_motor) && motors_on) {
for (uint8_t i = 0; i < sitl->state.num_motors; i++) {
float motor_freq = calculate_noise(sitl->state.rpm[i] / 60.0f, freq_variation);
float phase_incr = motor_freq * 2 * M_PI / (gyro_sample_hz[instance] * nsamples);
float &phase = gyro_motor_phase[instance][i];
float phase_incr = motor_freq * 2 * M_PI / (gyro_sample_hz * nsamples);
float &phase = gyro_motor_phase[i];
phase += phase_incr;
if (phase_incr > M_PI) {
phase -= 2 * M_PI;
@ -228,12 +213,12 @@ void AP_InertialSensor_SITL::generate_gyro(uint8_t instance)
gyro.y *= (1 + scale.y * 0.01f);
gyro.z *= (1 + scale.z * 0.01f);
_rotate_and_correct_gyro(gyro_instance[instance], gyro);
gyro_accum += gyro;
_notify_new_gyro_sensor_rate_sample(instance, gyro);
_notify_new_gyro_sensor_rate_sample(gyro_instance, gyro);
}
gyro_accum /= nsamples;
_notify_new_gyro_raw_sample(gyro_instance[instance], gyro_accum);
_rotate_and_correct_gyro(gyro_instance, gyro_accum);
_notify_new_gyro_raw_sample(gyro_instance, gyro_accum);
}
void AP_InertialSensor_SITL::timer_update(void)
@ -246,30 +231,26 @@ void AP_InertialSensor_SITL::timer_update(void)
return;
}
#endif
for (uint8_t i=0; i<INS_SITL_INSTANCES; i++) {
if (now >= next_accel_sample[i]) {
if (((1U<<i) & sitl->accel_fail_mask) == 0) {
generate_accel(i);
if (next_accel_sample[i] == 0) {
next_accel_sample[i] = now + 1000000UL / accel_sample_hz[i];
}
else {
while (now >= next_accel_sample[i]) {
next_accel_sample[i] += 1000000UL / accel_sample_hz[i];
}
if (now >= next_accel_sample) {
if (((1U << accel_instance) & sitl->accel_fail_mask) == 0) {
generate_accel();
if (next_accel_sample == 0) {
next_accel_sample = now + 1000000UL / accel_sample_hz;
} else {
while (now >= next_accel_sample) {
next_accel_sample += 1000000UL / accel_sample_hz;
}
}
}
if (now >= next_gyro_sample[i]) {
if (((1U<<i) & sitl->gyro_fail_mask) == 0) {
generate_gyro(i);
if (next_gyro_sample[i] == 0) {
next_gyro_sample[i] = now + 1000000UL / gyro_sample_hz[i];
}
else {
while (now >= next_gyro_sample[i]) {
next_gyro_sample[i] += 1000000UL / gyro_sample_hz[i];
}
}
if (now >= next_gyro_sample) {
if (((1U << gyro_instance) & sitl->gyro_fail_mask) == 0) {
generate_gyro();
if (next_gyro_sample == 0) {
next_gyro_sample = now + 1000000UL / gyro_sample_hz;
} else {
while (now >= next_gyro_sample) {
next_gyro_sample += 1000000UL / gyro_sample_hz;
}
}
}
@ -293,11 +274,21 @@ float AP_InertialSensor_SITL::gyro_drift(void)
bool AP_InertialSensor_SITL::update(void)
{
for (uint8_t i=0; i<INS_SITL_INSTANCES; i++) {
update_accel(accel_instance[i]);
update_gyro(gyro_instance[i]);
}
update_accel(accel_instance);
update_gyro(gyro_instance);
return true;
}
uint8_t AP_InertialSensor_SITL::bus_id = 0;
void AP_InertialSensor_SITL::start()
{
gyro_instance = _imu.register_gyro(gyro_sample_hz,
AP_HAL::Device::make_bus_id(AP_HAL::Device::BUS_TYPE_SITL, bus_id, 1, DEVTYPE_SITL));
accel_instance = _imu.register_accel(accel_sample_hz,
AP_HAL::Device::make_bus_id(AP_HAL::Device::BUS_TYPE_SITL, bus_id, 2, DEVTYPE_SITL));
bus_id++;
hal.scheduler->register_timer_process(FUNCTOR_BIND_MEMBER(&AP_InertialSensor_SITL::timer_update, void));
}
#endif // HAL_BOARD_SITL

34
libraries/AP_InertialSensor/AP_InertialSensor_SITL.h
View File

@ -6,37 +6,43 @@
#include "AP_InertialSensor.h"
#include "AP_InertialSensor_Backend.h"
#define INS_SITL_INSTANCES 2
// simulated sensor rates in Hz. This matches a pixhawk1
const uint16_t INS_SITL_SENSOR_A[] = { 1000, 1000 };
const uint16_t INS_SITL_SENSOR_B[] = { 760, 800 };
class AP_InertialSensor_SITL : public AP_InertialSensor_Backend
{
public:
AP_InertialSensor_SITL(AP_InertialSensor &imu);
AP_InertialSensor_SITL(AP_InertialSensor &imu, const uint16_t sample_rates[]);
/* update accel and gyro state */
bool update() override;
void start() override;
// detect the sensor
static AP_InertialSensor_Backend *detect(AP_InertialSensor &imu);
static AP_InertialSensor_Backend *detect(AP_InertialSensor &imu, const uint16_t sample_rates[]);
private:
bool init_sensor(void);
void timer_update();
float gyro_drift(void);
void generate_accel(uint8_t instance);
void generate_gyro(uint8_t instance);
void generate_accel();
void generate_gyro();
SITL::SITL *sitl;
// simulated sensor rates in Hz. This matches a pixhawk1
const uint16_t gyro_sample_hz[INS_SITL_INSTANCES] { 1000, 760 };
const uint16_t accel_sample_hz[INS_SITL_INSTANCES] { 1000, 800 };
const uint16_t gyro_sample_hz;
const uint16_t accel_sample_hz;
uint8_t gyro_instance[INS_SITL_INSTANCES];
uint8_t accel_instance[INS_SITL_INSTANCES];
uint64_t next_gyro_sample[INS_SITL_INSTANCES];
uint64_t next_accel_sample[INS_SITL_INSTANCES];
float gyro_motor_phase[INS_SITL_INSTANCES][12];
float accel_motor_phase[INS_SITL_INSTANCES][12];
uint8_t gyro_instance;
uint8_t accel_instance;
uint64_t next_gyro_sample;
uint64_t next_accel_sample;
float gyro_time;
float accel_time;
float gyro_motor_phase[12];
float accel_motor_phase[12];
static uint8_t bus_id;
};
#endif // CONFIG_HAL_BOARD