diff --git a/libraries/SITL/SITL.cpp b/libraries/SITL/SITL.cpp index c2be929994..28cd6a46be 100644 --- a/libraries/SITL/SITL.cpp +++ b/libraries/SITL/SITL.cpp @@ -69,6 +69,7 @@ const AP_Param::GroupInfo SITL::var_info[] PROGMEM = { AP_GROUPINFO("BARO_DELAY", 38, SITL, baro_delay, 0), 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_GROUPEND }; diff --git a/libraries/SITL/SITL.h b/libraries/SITL/SITL.h index 11f1de26bb..fa592d9802 100644 --- a/libraries/SITL/SITL.h +++ b/libraries/SITL/SITL.h @@ -30,6 +30,8 @@ class SITL public: SITL() { + // set a default compass offset + mag_ofs.set(Vector3f(5, 13, -18)); AP_Param::setup_object_defaults(this, var_info); } @@ -59,6 +61,7 @@ public: AP_Float mag_noise; // in mag units (earth field is 818) AP_Float mag_error; // in degrees AP_Vector3f mag_mot; // in mag units per amp + AP_Vector3f mag_ofs; // in mag units AP_Float servo_rate; // servo speed in degrees/second AP_Float sonar_glitch;// probablility between 0-1 that any given sonar sample will read as max distance