SITL: Add compassmot interference

libraries/AP_Compass/AP_Compass_HIL.cpp

@@ -108,7 +108,7 @@ void AP_Compass_HIL::setHIL(float roll, float pitch, float yaw)
 
 // Update raw magnetometer values from HIL mag vector
 //
-void AP_Compass_HIL::setHIL(const Vector3i &mag)
+void AP_Compass_HIL::setHIL(const Vector3f &mag)
 {
     _hil_mag.x = mag.x;
     _hil_mag.y = mag.y;
@@ -116,6 +116,10 @@ void AP_Compass_HIL::setHIL(const Vector3i &mag)
     _healthy[0] = true;
 }
 
+const Vector3f& AP_Compass_HIL::getHIL() const {
+    return _hil_mag;
+}
+
 void AP_Compass_HIL::accumulate(void)
 {
     // nothing to do

libraries/AP_Compass/AP_Compass_HIL.h

@@ -11,7 +11,8 @@ public:
     bool        read(void);
     void        accumulate(void);
     void        setHIL(float roll, float pitch, float yaw);
-    void        setHIL(const Vector3i &mag);
+    void        setHIL(const Vector3f &mag);
+    const Vector3f&   getHIL() const;
 
 private:
     Vector3f    _hil_mag;

libraries/AP_HAL_AVR_SITL/SITL_State.cpp

@@ -62,6 +62,7 @@ uint16_t SITL_State::sonar_pin_value;
 uint16_t SITL_State::airspeed_pin_value;
 uint16_t SITL_State::voltage_pin_value;
 uint16_t SITL_State::current_pin_value;
+float SITL_State::_current;
 
 AP_Baro_HIL *SITL_State::_barometer;
 AP_InertialSensor_HIL *SITL_State::_ins;
@@ -528,10 +529,10 @@ void SITL_State::_simulator_output(void)
     // lose 0.7V at full throttle
     float voltage = _sitl->batt_voltage - 0.7f*throttle;
     // assume 50A at full throttle
-    float current = 50.0 * throttle;
+    _current = 50.0 * throttle;
     // assume 3DR power brick
     voltage_pin_value = ((voltage / 10.1) / 5.0) * 1024;
-    current_pin_value = ((current / 17.0) / 5.0) * 1024;
+    current_pin_value = ((_current / 17.0) / 5.0) * 1024;
 
 	// setup wind control
     float wind_speed = _sitl->wind_speed * 100;

libraries/AP_HAL_AVR_SITL/SITL_State.h

@@ -129,6 +129,7 @@ private:
     static SITL *_sitl;
     static uint16_t _rcout_port;
     static uint16_t _simin_port;
+    static float _current;
 };
 
 #endif // CONFIG_HAL_BOARD == HAL_BOARD_AVR_SITL

libraries/AP_HAL_AVR_SITL/sitl_compass.cpp

@@ -40,7 +40,8 @@ void SITL_State::_update_compass(float rollDeg, float pitchDeg, float yawDeg)
 	}
 	_compass->setHIL(radians(rollDeg), radians(pitchDeg), radians(yawDeg));
 	Vector3f noise = _rand_vec3f() * _sitl->mag_noise;
-        _compass->set_field(_compass->get_field() + noise);
+    Vector3f motor = _sitl->mag_mot.get() * _current;
+    _compass->setHIL(_compass->getHIL() + noise+motor);
 }
 
 #endif

libraries/SITL/SITL.cpp

@@ -57,6 +57,7 @@ const AP_Param::GroupInfo SITL::var_info[] PROGMEM = {
     AP_GROUPINFO("GPS2_ENABLE",   26, SITL,  gps2_enable, 0),
     AP_GROUPINFO("BARO_DISABLE",  27, SITL,  baro_disable, 0),
     AP_GROUPINFO("FLOAT_EXCEPT",  28, SITL,  float_exception, 1),
+    AP_GROUPINFO("MAG_MOT",       29, SITL,  mag_mot, 0),
     AP_GROUPEND
 };
 

libraries/SITL/SITL.h

@@ -56,6 +56,7 @@ public:
 	AP_Float aspd_noise;  // in m/s
 	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_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