diff --git a/libraries/AP_Compass/AP_Compass_HIL.cpp b/libraries/AP_Compass/AP_Compass_HIL.cpp index 805c3af9664e102678f37bd258bf83700866502c..bee60c1206209d6d3bfbb02b40918c8d344e2292 100644 --- a/libraries/AP_Compass/AP_Compass_HIL.cpp +++ b/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 diff --git a/libraries/AP_Compass/AP_Compass_HIL.h b/libraries/AP_Compass/AP_Compass_HIL.h index 3bb63837247051f8e229265095c7a04f974f01f9..7b65a1333c9d3754b1ab9702f5817039d7aba260 100644 --- a/libraries/AP_Compass/AP_Compass_HIL.h +++ b/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; diff --git a/libraries/AP_HAL_AVR_SITL/SITL_State.cpp b/libraries/AP_HAL_AVR_SITL/SITL_State.cpp index bf843e9b89cf793cb3beb2b7e96c575ccefd82c4..f5e7b57f57bbfae077af95df8a2849929eef540a 100644 --- a/libraries/AP_HAL_AVR_SITL/SITL_State.cpp +++ b/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; diff --git a/libraries/AP_HAL_AVR_SITL/SITL_State.h b/libraries/AP_HAL_AVR_SITL/SITL_State.h index aa2d146693dfa2eed63a8364bfa8b9c2e68b6aa7..8d7b74471e3a6090f0d857a55ec3d1309bb4622a 100644 --- a/libraries/AP_HAL_AVR_SITL/SITL_State.h +++ b/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 diff --git a/libraries/AP_HAL_AVR_SITL/sitl_compass.cpp b/libraries/AP_HAL_AVR_SITL/sitl_compass.cpp index bfa7c7d9fa0c2ac837f3478c4fac36270c7f2afb..64d00004d69bd7c00a52207bda30ead25c8de0a6 100644 --- a/libraries/AP_HAL_AVR_SITL/sitl_compass.cpp +++ b/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 diff --git a/libraries/SITL/SITL.cpp b/libraries/SITL/SITL.cpp index eebdd814454a87c9a2bf4fb34c311728ea3b49a5..1cd6b2ba57a1cd96ef3216a4d0d961f1c81e9678 100644 --- a/libraries/SITL/SITL.cpp +++ b/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 }; diff --git a/libraries/SITL/SITL.h b/libraries/SITL/SITL.h index 966859d2fb15bcd513299a5f49a917a65db3807c..95c8a5194fecd522aeaca3bde508201173c177cc 100644 --- a/libraries/SITL/SITL.h +++ b/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