SITL: added SIM_MAG_ERROR parameter

this is a compass yaw error in degrees, used for testing navigation
with a bad compass
This commit is contained in:
Andrew Tridgell 2013-05-27 13:37:18 +10:00
parent f76fab299b
commit 15a84972d1
3 changed files with 9 additions and 0 deletions

View File

@ -31,6 +31,13 @@ void SITL_State::_update_compass(float rollDeg, float pitchDeg, float yawDeg)
// no compass in this sketch
return;
}
yawDeg += _sitl->mag_error;
if (yawDeg > 180.0f) {
yawDeg -= 360.0f;
}
if (yawDeg < -180.0f) {
yawDeg += 360.0f;
}
_compass->setHIL(radians(rollDeg), radians(pitchDeg), radians(yawDeg));
Vector3f noise = _rand_vec3f() * _sitl->mag_noise;
_compass->mag_x += noise.x;

View File

@ -29,6 +29,7 @@ const AP_Param::GroupInfo SITL::var_info[] PROGMEM = {
AP_GROUPINFO("GPS_TYPE", 12, SITL, gps_type, SITL::GPS_TYPE_UBLOX),
AP_GROUPINFO("GPS_BYTELOSS", 13, SITL, gps_byteloss, 0),
AP_GROUPINFO("GPS_NUMSATS", 14, SITL, gps_numsats, 10),
AP_GROUPINFO("MAG_ERROR", 15, SITL, mag_error, 0),
AP_GROUPEND
};

View File

@ -50,6 +50,7 @@ public:
AP_Float accel_noise; // in m/s/s
AP_Float mag_noise; // in mag units (earth field is 818)
AP_Float aspd_noise; // in m/s
AP_Float mag_error; // in degrees
AP_Float drift_speed; // degrees/second/minute
AP_Float drift_time; // period in minutes