zrzk
/
ardupilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

AP_InertialSensor: more accurately compute INS noise taking throttle into account and adding frequency noise noisily 

make SITL fast-sampling correct
zr-v5.1
Andy Piper 5 years ago committed by Andrew Tridgell
parent
8b0fc1207d
commit
52f59fb573
1 changed files with 125 additions and 99 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 224
      libraries/AP_InertialSensor/AP_InertialSensor_SITL.cpp

224
libraries/AP_InertialSensor/AP_InertialSensor_SITL.cpp
Unescape View File

@ -55,37 +55,60 @@ bool AP_InertialSensor_SITL::init_sensor(void) @@ -55,37 +55,60 @@ bool AP_InertialSensor_SITL::init_sensor(void)
return true;
}
// calculate a noisy noise component
static float calculate_noise(float noise, float noise_variation) {
return noise * (1.0f + noise_variation * rand_float());
}
/*
generate an accelerometer sample
*/
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;
Vector3f accel_accum;
uint8_t nsamples = enable_fast_sampling(accel_instance[instance]) ? 4 : 1;
for (uint8_t j = 0; j < nsamples; j++) {
if (sitl->motors_on) {
// add extra noise when the motors are on
accel_noise += instance == 0 ? sitl->accel_noise : sitl->accel2_noise;
}
// add accel bias and noise
Vector3f accel_bias = 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;
// 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;
// this smears the individual motor peaks somewhat emulating physical motors
float freq_variation = 0.12;
// add accel bias and noise
Vector3f accel_bias = 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;
const Vector3f &vibe_freq = sitl->vibe_freq;
bool vibe_motor = !is_zero(sitl->vibe_motor);
if (vibe_freq.is_zero() && !vibe_motor) {
xAccel += accel_noise * rand_float();
yAccel += accel_noise * rand_float();
zAccel += accel_noise * rand_float();
} else {
if (vibe_motor) {
bool motors_on = sitl->throttle > sitl->ins_noise_throttle_min;
// on a real 180mm copter gyro noise varies between 0.8-4 m/s/s for throttle 0.2-0.8
// 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;
}
// 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);
}
// 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 motor_freq = sitl->state.rpm[i] / 60.0f;
float phase_incr = motor_freq * 2 * M_PI / accel_sample_hz[instance];
float &phase = accel_motor_phase[instance][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);
phase += phase_incr;
if (phase_incr > M_PI) {
phase -= 2 * M_PI;
@ -93,54 +116,49 @@ void AP_InertialSensor_SITL::generate_accel(uint8_t instance) @@ -93,54 +116,49 @@ void AP_InertialSensor_SITL::generate_accel(uint8_t instance)
else if (phase_incr < -M_PI) {
phase += 2 * M_PI;
}
xAccel += sinf(phase) * accel_noise;
yAccel += sinf(phase) * accel_noise;
zAccel += sinf(phase) * accel_noise;
xAccel += sinf(phase) * calculate_noise(accel_noise, noise_variation);
yAccel += sinf(phase) * calculate_noise(accel_noise, noise_variation);
zAccel += sinf(phase) * calculate_noise(accel_noise, noise_variation);
}
}
if (!vibe_freq.is_zero()) {
float t = AP_HAL::micros() * 1.0e-6f;
xAccel += sinf(t * 2 * M_PI * vibe_freq.x) * accel_noise;
yAccel += sinf(t * 2 * M_PI * vibe_freq.y) * accel_noise;
zAccel += sinf(t * 2 * M_PI * vibe_freq.z) * accel_noise;
// correct for the acceleration due to the IMU position offset and angular acceleration
// correct for the centripetal acceleration
// only apply corrections to first accelerometer
Vector3f pos_offset = sitl->imu_pos_offset;
if (!pos_offset.is_zero()) {
// calculate sensed acceleration due to lever arm effect
// Note: the % operator has been overloaded to provide a cross product
Vector3f angular_accel = Vector3f(radians(sitl->state.angAccel.x), radians(sitl->state.angAccel.y), radians(sitl->state.angAccel.z));
Vector3f lever_arm_accel = angular_accel % pos_offset;
// calculate sensed acceleration due to centripetal acceleration
Vector3f angular_rate = Vector3f(radians(sitl->state.rollRate), radians(sitl->state.pitchRate), radians(sitl->state.yawRate));
Vector3f centripetal_accel = angular_rate % (angular_rate % pos_offset);
// apply corrections
xAccel += lever_arm_accel.x + centripetal_accel.x;
yAccel += lever_arm_accel.y + centripetal_accel.y;
zAccel += lever_arm_accel.z + centripetal_accel.z;
}
}
// correct for the acceleration due to the IMU position offset and angular acceleration
// correct for the centripetal acceleration
// only apply corrections to first accelerometer
Vector3f pos_offset = sitl->imu_pos_offset;
if (!pos_offset.is_zero()) {
// calculate sensed acceleration due to lever arm effect
// Note: the % operator has been overloaded to provide a cross product
Vector3f angular_accel = Vector3f(radians(sitl->state.angAccel.x) , radians(sitl->state.angAccel.y) , radians(sitl->state.angAccel.z));
Vector3f lever_arm_accel = angular_accel % pos_offset;
// calculate sensed acceleration due to centripetal acceleration
Vector3f angular_rate = Vector3f(radians(sitl->state.rollRate), radians(sitl->state.pitchRate), radians(sitl->state.yawRate));
Vector3f centripetal_accel = angular_rate % (angular_rate % pos_offset);
// apply corrections
xAccel += lever_arm_accel.x + centripetal_accel.x;
yAccel += lever_arm_accel.y + centripetal_accel.y;
zAccel += lever_arm_accel.z + centripetal_accel.z;
}
if (fabsf(sitl->accel_fail) > 1.0e-6f) {
xAccel = sitl->accel_fail;
yAccel = sitl->accel_fail;
zAccel = sitl->accel_fail;
}
if (fabsf(sitl->accel_fail) > 1.0e-6f) {
xAccel = sitl->accel_fail;
yAccel = sitl->accel_fail;
zAccel = sitl->accel_fail;
}
Vector3f accel = Vector3f(xAccel, yAccel, zAccel);
Vector3f accel = Vector3f(xAccel, yAccel, zAccel);
_rotate_and_correct_accel(accel_instance[instance], accel);
_rotate_and_correct_accel(accel_instance[instance], accel);
_notify_new_accel_sensor_rate_sample(instance, accel);
uint8_t nsamples = enable_fast_sampling(accel_instance[instance]) ? 4 : 1;
for (uint8_t i=0; i<nsamples; i++) {
_notify_new_accel_raw_sample(accel_instance[instance], accel);
accel_accum += accel;
}
accel_accum /= nsamples;
_notify_new_accel_raw_sample(accel_instance[instance], accel_accum);
_publish_temperature(instance, 23);
}
@ -149,30 +167,46 @@ void AP_InertialSensor_SITL::generate_accel(uint8_t instance) @@ -149,30 +167,46 @@ void AP_InertialSensor_SITL::generate_accel(uint8_t instance)
*/
void AP_InertialSensor_SITL::generate_gyro(uint8_t instance)
{
// minimum gyro noise is less than 1 bit
float gyro_noise = ToRad(0.04f);
if (sitl->motors_on) {
// add extra noise when the motors are on
gyro_noise += ToRad(sitl->gyro_noise);
}
Vector3f gyro_accum;
uint8_t nsamples = enable_fast_sampling(gyro_instance[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();
float r = radians(sitl->state.yawRate) + gyro_drift();
float p = radians(sitl->state.rollRate) + gyro_drift();
float q = radians(sitl->state.pitchRate) + gyro_drift();
float r = radians(sitl->state.yawRate) + gyro_drift();
// minimum gyro noise is less than 1 bit
float gyro_noise = ToRad(0.04f);
float noise_variation = 0.05f;
// this smears the individual motor peaks somewhat emulating physical motors
float freq_variation = 0.12f;
const Vector3f &vibe_freq = sitl->vibe_freq;
bool vibe_motor = !is_zero(sitl->vibe_motor);
if (vibe_freq.is_zero() && !vibe_motor) {
p += gyro_noise * rand_float();
q += gyro_noise * rand_float();
r += gyro_noise * rand_float();
} else {
if (vibe_motor) {
bool motors_on = sitl->throttle > sitl->ins_noise_throttle_min;
// on a real 180mm copter gyro noise varies between 0.2-0.4 rad/s for throttle 0.2-0.8
// giving a gyro noise variation of 0.33 rad/s or 20deg/s over the full throttle range
if (motors_on) {
// add extra noise when the motors are on
gyro_noise = ToRad(sitl->gyro_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;
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);
}
// 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 = sitl->state.rpm[i] / 60.0f;
float phase_incr = motor_freq * 2 * M_PI / gyro_sample_hz[instance];
float& phase = gyro_motor_phase[instance][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];
phase += phase_incr;
if (phase_incr > M_PI) {
phase -= 2 * M_PI;
@ -180,34 +214,26 @@ void AP_InertialSensor_SITL::generate_gyro(uint8_t instance) @@ -180,34 +214,26 @@ void AP_InertialSensor_SITL::generate_gyro(uint8_t instance)
else if (phase_incr < -M_PI) {
phase += 2 * M_PI;
}
p += sinf(phase) * gyro_noise;
q += sinf(phase) * gyro_noise;
r += sinf(phase) * gyro_noise;
p += sinf(phase) * calculate_noise(gyro_noise, noise_variation);
q += sinf(phase) * calculate_noise(gyro_noise, noise_variation);
r += sinf(phase) * calculate_noise(gyro_noise, noise_variation);
}
}
if (!vibe_freq.is_zero()) {
float t = AP_HAL::micros() * 1.0e-6f;
p += sinf(t * 2 * M_PI * vibe_freq.x) * gyro_noise;
q += sinf(t * 2 * M_PI * vibe_freq.y) * gyro_noise;
r += sinf(t * 2 * M_PI * vibe_freq.z) * gyro_noise;
}
}
Vector3f gyro = Vector3f(p, q, r);
Vector3f gyro = Vector3f(p, q, r);
// add in gyro scaling
Vector3f scale = sitl->gyro_scale;
gyro.x *= (1 + scale.x * 0.01f);
gyro.y *= (1 + scale.y * 0.01f);
gyro.z *= (1 + scale.z * 0.01f);
// add in gyro scaling
Vector3f scale = sitl->gyro_scale;
gyro.x *= (1 + scale.x * 0.01f);
gyro.y *= (1 + scale.y * 0.01f);
gyro.z *= (1 + scale.z * 0.01f);
_rotate_and_correct_gyro(gyro_instance[instance], gyro);
uint8_t nsamples = enable_fast_sampling(gyro_instance[instance]) ? 8 : 1;
for (uint8_t i = 0; i < nsamples; i++) {
_notify_new_gyro_raw_sample(gyro_instance[instance], gyro);
_rotate_and_correct_gyro(gyro_instance[instance], gyro);
gyro_accum += gyro;
_notify_new_gyro_sensor_rate_sample(instance, gyro);
}
gyro_accum /= nsamples;
_notify_new_gyro_raw_sample(gyro_instance[instance], gyro_accum);
}
void AP_InertialSensor_SITL::timer_update(void)

Write Preview
Loading…
Cancel
Save