Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-07 08:23:56 -04:00
Code Issues Packages Projects Releases Wiki Activity

SITL: add vibe monitor and SIM_ACC2_RND to sitl

Browse Source
This commit is contained in:
Randy Mackay 2015-07-12 15:26:43 +09:00
parent 8b4b0b9576
commit 2b4aaf2368
3 changed files with 15 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

18
libraries/AP_HAL_SITL/sitl_ins.cpp
View File

@ -159,11 +159,13 @@ void SITL_State::_update_ins(float roll, float pitch, float yaw, // Relative
// 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 (_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);
}
// get accel bias (add only to first accelerometer)
@ -172,9 +174,9 @@ void SITL_State::_update_ins(float roll, float pitch, float yaw, // Relative
float yAccel1 = yAccel + accel_noise * _rand_float() + accel_bias.y;
float zAccel1 = zAccel + accel_noise * _rand_float() + accel_bias.z;
float xAccel2 = xAccel + accel_noise * _rand_float();
float yAccel2 = yAccel + accel_noise * _rand_float();
float zAccel2 = zAccel + accel_noise * _rand_float();
float xAccel2 = xAccel + accel2_noise * _rand_float();
float yAccel2 = yAccel + accel2_noise * _rand_float();
float zAccel2 = zAccel + accel2_noise * _rand_float();
if (fabsf(_sitl->accel_fail) > 1.0e-6f) {
xAccel1 = _sitl->accel_fail;
@ -182,8 +184,14 @@ void SITL_State::_update_ins(float roll, float pitch, float yaw, // Relative
zAccel1 = _sitl->accel_fail;
}
_ins->set_accel(0, Vector3f(xAccel1, yAccel1, zAccel1) + _ins->get_accel_offsets(0));
_ins->set_accel(1, Vector3f(xAccel2, yAccel2, zAccel2) + _ins->get_accel_offsets(1));
Vector3f accel0 = Vector3f(xAccel1, yAccel1, zAccel1) + _ins->get_accel_offsets(0);
Vector3f accel1 = Vector3f(xAccel2, yAccel2, zAccel2) + _ins->get_accel_offsets(1);
_ins->set_accel(0, accel0);
_ins->set_accel(1, accel1);
// check noise
_ins->calc_vibration_and_clipping(0, accel0, 0.0025f);
_ins->calc_vibration_and_clipping(1, accel1, 0.0025f);
float p = radians(rollRate) + _gyro_drift();
float q = radians(pitchRate) + _gyro_drift();

1
libraries/SITL/SITL.cpp
View File

@ -69,6 +69,7 @@ const AP_Param::GroupInfo SITL::var_info[] PROGMEM = {
AP_GROUPINFO("MAG_DELAY", 39, SITL, mag_delay, 0),
AP_GROUPINFO("WIND_DELAY", 40, SITL, wind_delay, 0),
AP_GROUPINFO("MAG_OFS", 41, SITL, mag_ofs, 0),
AP_GROUPINFO("ACC2_RND", 42, SITL, accel2_noise, 0),
AP_GROUPEND
};

1
libraries/SITL/SITL.h
View File

@ -59,6 +59,7 @@ public:
AP_Float baro_glitch; // glitch in meters
AP_Float gyro_noise; // in degrees/second
AP_Float accel_noise; // in m/s/s
AP_Float accel2_noise; // in m/s/s
AP_Vector3f accel_bias; // in m/s/s
AP_Float aspd_noise; // in m/s
AP_Float mag_noise; // in mag units (earth field is 818)