Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_InertialSensor: add rpm-based motor noise to gyros and accels 

Remove sample time error in backend.
This commit is contained in:
Andy Piper 2019-09-27 20:56:45 +01:00 committed by Andrew Tridgell
parent 39e948a40a
commit 143a071788
3 changed files with 77 additions and 23 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp
View File

@ -177,6 +177,7 @@ void AP_InertialSensor_Backend::_notify_new_gyro_raw_sample(uint8_t instance,
dt = 1.0f / _imu._gyro_raw_sample_rates[instance];
_imu._gyro_last_sample_us[instance] = AP_HAL::micros64();
sample_us = _imu._gyro_last_sample_us[instance];
}
#if AP_MODULE_SUPPORTED
@ -353,6 +354,7 @@ void AP_InertialSensor_Backend::_notify_new_accel_raw_sample(uint8_t instance,
dt = 1.0f / _imu._accel_raw_sample_rates[instance];
_imu._accel_last_sample_us[instance] = AP_HAL::micros64();
sample_us = _imu._accel_last_sample_us[instance];
}
#if AP_MODULE_SUPPORTED

56
libraries/AP_InertialSensor/AP_InertialSensor_SITL.cpp
View File

@ -75,16 +75,36 @@ void AP_InertialSensor_SITL::generate_accel(uint8_t instance)
float yAccel = sitl->state.yAccel + accel_bias.y;
float zAccel = sitl->state.zAccel + accel_bias.z;
const Vector3f &vibe_freq = sitl->vibe_freq;
if (vibe_freq.is_zero()) {
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) {
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];
phase += phase_incr;
if (phase_incr > M_PI) {
phase -= 2 * M_PI;
}
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;
}
}
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
@ -142,16 +162,37 @@ void AP_InertialSensor_SITL::generate_gyro(uint8_t instance)
float r = radians(sitl->state.yawRate) + gyro_drift();
const Vector3f &vibe_freq = sitl->vibe_freq;
if (vibe_freq.is_zero()) {
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) {
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];
phase += phase_incr;
if (phase_incr > M_PI) {
phase -= 2 * M_PI;
}
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;
}
}
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);
@ -183,14 +224,23 @@ void AP_InertialSensor_SITL::timer_update(void)
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_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];
}
@ -198,6 +248,7 @@ void AP_InertialSensor_SITL::timer_update(void)
}
}
}
}
float AP_InertialSensor_SITL::gyro_drift(void)
{
@ -211,7 +262,6 @@ float AP_InertialSensor_SITL::gyro_drift(void)
return minutes * ToRad(sitl->drift_speed);
}
return (period - minutes) * ToRad(sitl->drift_speed);
}

2
libraries/AP_InertialSensor/AP_InertialSensor_SITL.h
View File

@ -36,5 +36,7 @@ private:
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];
};
#endif // CONFIG_HAL_BOARD