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

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
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
libraries/AP_HAL_AVR_SITL/sitl_compass.cpp
View File

@ -31,6 +31,13 @@ void SITL_State::_update_compass(float rollDeg, float pitchDeg, float yawDeg)
// no compass in this sketch // no compass in this sketch
return; 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)); _compass->setHIL(radians(rollDeg), radians(pitchDeg), radians(yawDeg));
Vector3f noise = _rand_vec3f() * _sitl->mag_noise; Vector3f noise = _rand_vec3f() * _sitl->mag_noise;
_compass->mag_x += noise.x; _compass->mag_x += noise.x;

1
libraries/SITL/SITL.cpp
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_TYPE", 12, SITL, gps_type, SITL::GPS_TYPE_UBLOX),
AP_GROUPINFO("GPS_BYTELOSS", 13, SITL, gps_byteloss, 0), AP_GROUPINFO("GPS_BYTELOSS", 13, SITL, gps_byteloss, 0),
AP_GROUPINFO("GPS_NUMSATS", 14, SITL, gps_numsats, 10), AP_GROUPINFO("GPS_NUMSATS", 14, SITL, gps_numsats, 10),
AP_GROUPINFO("MAG_ERROR", 15, SITL, mag_error, 0),
AP_GROUPEND AP_GROUPEND
}; };

1
libraries/SITL/SITL.h
View File

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