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

AP_InertialSensor: make SITL sensor rate match a Pixhawk1

Browse Source 
use 760Hz 2nd gyro and 800 Hz 2nd accel
This commit is contained in:
Andrew Tridgell 2017-05-01 10:51:15 +10:00
parent 25cd16a275
commit 74eb7a7243
2 changed files with 69 additions and 45 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

104
libraries/AP_InertialSensor/AP_InertialSensor_SITL.cpp
View File

@ -1,6 +1,7 @@
#include <AP_HAL/AP_HAL.h>
#include "AP_InertialSensor_SITL.h"
#include <SITL/SITL.h>
#include <stdio.h>
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
@ -36,8 +37,8 @@ bool AP_InertialSensor_SITL::init_sensor(void)
// grab the used instances
for (uint8_t i=0; i<INS_SITL_INSTANCES; i++) {
gyro_instance[i] = _imu.register_gyro(sitl->update_rate_hz, i);
accel_instance[i] = _imu.register_accel(sitl->update_rate_hz, i);
gyro_instance[i] = _imu.register_gyro(gyro_sample_hz[i], i);
accel_instance[i] = _imu.register_accel(accel_sample_hz[i], i);
}
hal.scheduler->register_timer_process(FUNCTOR_BIND_MEMBER(&AP_InertialSensor_SITL::timer_update, void));
@ -45,32 +46,25 @@ bool AP_InertialSensor_SITL::init_sensor(void)
return true;
}
void AP_InertialSensor_SITL::timer_update(void)
/*
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;
float accel2_noise = 0.01f;
// minimum gyro noise is also less than 1 bit
float gyro_noise = ToRad(0.04f);
if (sitl->motors_on) {
// add extra noise when the motors are on
accel_noise += sitl->accel_noise;
accel2_noise += sitl->accel2_noise;
gyro_noise += ToRad(sitl->gyro_noise);
}
// add accel bias and noise
Vector3f accel_bias = sitl->accel_bias.get();
float xAccel1 = sitl->state.xAccel + accel_noise * rand_float() + accel_bias.x;
float yAccel1 = sitl->state.yAccel + accel_noise * rand_float() + accel_bias.y;
float zAccel1 = sitl->state.zAccel + accel_noise * rand_float() + accel_bias.z;
accel_bias = sitl->accel2_bias.get();
float xAccel2 = sitl->state.xAccel + accel2_noise * rand_float() + accel_bias.x;
float yAccel2 = sitl->state.yAccel + accel2_noise * rand_float() + accel_bias.y;
float zAccel2 = sitl->state.zAccel + accel2_noise * rand_float() + accel_bias.z;
Vector3f accel_bias = instance==0?sitl->accel_bias.get():sitl->accel2_bias.get();
float xAccel = sitl->state.xAccel + accel_noise * rand_float() + accel_bias.x;
float yAccel = sitl->state.yAccel + accel_noise * rand_float() + accel_bias.y;
float zAccel = sitl->state.zAccel + accel_noise * rand_float() + accel_bias.z;
// correct for the acceleration due to the IMU position offset and angular acceleration
// correct for the centripetal acceleration
@ -87,49 +81,71 @@ void AP_InertialSensor_SITL::timer_update(void)
Vector3f centripetal_accel = angular_rate % (angular_rate % pos_offset);
// apply corrections
xAccel1 += lever_arm_accel.x + centripetal_accel.x;
yAccel1 += lever_arm_accel.y + centripetal_accel.y;
zAccel1 += lever_arm_accel.z + centripetal_accel.z;
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) {
xAccel1 = sitl->accel_fail;
yAccel1 = sitl->accel_fail;
zAccel1 = sitl->accel_fail;
xAccel = sitl->accel_fail;
yAccel = sitl->accel_fail;
zAccel = sitl->accel_fail;
}
Vector3f accel0 = Vector3f(xAccel1, yAccel1, zAccel1) + _imu.get_accel_offsets(0);
Vector3f accel1 = Vector3f(xAccel2, yAccel2, zAccel2) + _imu.get_accel_offsets(1);
_notify_new_accel_raw_sample(accel_instance[0], accel0);
_notify_new_accel_raw_sample(accel_instance[1], accel1);
Vector3f accel = Vector3f(xAccel, yAccel, zAccel) + _imu.get_accel_offsets(instance);
_notify_new_accel_raw_sample(accel_instance[instance], accel, AP_HAL::micros64());
}
/*
generate a gyro sample
*/
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);
}
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 p1 = p + gyro_noise * rand_float();
float q1 = q + gyro_noise * rand_float();
float r1 = r + gyro_noise * rand_float();
p += gyro_noise * rand_float();
q += gyro_noise * rand_float();
r += gyro_noise * rand_float();
float p2 = p + gyro_noise * rand_float();
float q2 = q + gyro_noise * rand_float();
float r2 = r + gyro_noise * rand_float();
Vector3f gyro0 = Vector3f(p1, q1, r1) + _imu.get_gyro_offsets(0);
Vector3f gyro1 = Vector3f(p2, q2, r2) + _imu.get_gyro_offsets(1);
Vector3f gyro = Vector3f(p, q, r) + _imu.get_gyro_offsets(instance);
// add in gyro scaling
Vector3f scale = sitl->gyro_scale;
gyro0.x *= (1 + scale.x*0.01);
gyro0.y *= (1 + scale.y*0.01);
gyro0.z *= (1 + scale.z*0.01);
gyro.x *= (1 + scale.x*0.01);
gyro.y *= (1 + scale.y*0.01);
gyro.z *= (1 + scale.z*0.01);
gyro1.x *= (1 + scale.x*0.01);
gyro1.y *= (1 + scale.y*0.01);
gyro1.z *= (1 + scale.z*0.01);
_notify_new_gyro_raw_sample(gyro_instance[instance], gyro, AP_HAL::micros64());
}
_notify_new_gyro_raw_sample(gyro_instance[0], gyro0);
_notify_new_gyro_raw_sample(gyro_instance[1], gyro1);
void AP_InertialSensor_SITL::timer_update(void)
{
uint64_t now = AP_HAL::micros64();
for (uint8_t i=0; i<INS_SITL_INSTANCES; i++) {
if (now >= next_accel_sample[i]) {
generate_accel(i);
while (now >= next_accel_sample[i]) {
next_accel_sample[i] += 1000000UL / accel_sample_hz[i];
}
}
if (now >= next_gyro_sample[i]) {
generate_gyro(i);
while (now >= next_gyro_sample[i]) {
next_gyro_sample[i] += 1000000UL / gyro_sample_hz[i];
}
}
}
}
// generate a random float between -1 and 1

8
libraries/AP_InertialSensor/AP_InertialSensor_SITL.h
View File

@ -23,9 +23,17 @@ private:
void timer_update();
float rand_float(void);
float gyro_drift(void);
void generate_accel(uint8_t instance);
void generate_gyro(uint8_t instance);
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 };
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];
};