From e883b889b6ad2eb798de5e795a0b7fff7b139fb7 Mon Sep 17 00:00:00 2001 From: Jonathan Challinger <mr.challinger@gmail.com> Date: Mon, 12 May 2014 02:44:15 -0700 Subject: [PATCH] SITL: Add compassmot interference --- libraries/AP_Compass/AP_Compass_HIL.cpp | 6 +++++- libraries/AP_Compass/AP_Compass_HIL.h | 3 ++- libraries/AP_HAL_AVR_SITL/SITL_State.cpp | 5 +++-- libraries/AP_HAL_AVR_SITL/SITL_State.h | 1 + libraries/AP_HAL_AVR_SITL/sitl_compass.cpp | 3 ++- libraries/SITL/SITL.cpp | 1 + libraries/SITL/SITL.h | 1 + 7 files changed, 15 insertions(+), 5 deletions(-) diff --git a/libraries/AP_Compass/AP_Compass_HIL.cpp b/libraries/AP_Compass/AP_Compass_HIL.cpp index 805c3af96..bee60c120 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 3bb638372..7b65a1333 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 bf843e9b8..f5e7b57f5 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 aa2d14669..8d7b74471 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 bfa7c7d9f..64d00004d 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 eebdd8144..1cd6b2ba5 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 966859d2f..95c8a5194 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 -- GitLab