From 88a1ebaf0ea350cd26acff1c91e971ec6e05fac4 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 4 May 2016 17:11:02 +1000 Subject: [PATCH] AP_Compass: allow setting of exact timestamp in HIL compass --- libraries/AP_Compass/AP_Compass.cpp | 4 ++-- libraries/AP_Compass/AP_Compass.h | 2 +- libraries/AP_Compass/AP_Compass_Backend.cpp | 6 ++++++ libraries/AP_Compass/AP_Compass_Backend.h | 1 + libraries/AP_Compass/AP_Compass_HIL.cpp | 2 ++ 5 files changed, 12 insertions(+), 3 deletions(-) diff --git a/libraries/AP_Compass/AP_Compass.cpp b/libraries/AP_Compass/AP_Compass.cpp index 2696071e0a..6ae6f20dd8 100644 --- a/libraries/AP_Compass/AP_Compass.cpp +++ b/libraries/AP_Compass/AP_Compass.cpp @@ -761,11 +761,11 @@ void Compass::setHIL(uint8_t instance, float roll, float pitch, float yaw) // Update raw magnetometer values from HIL mag vector // -void Compass::setHIL(uint8_t instance, const Vector3f &mag) +void Compass::setHIL(uint8_t instance, const Vector3f &mag, uint32_t update_usec) { _hil.field[instance] = mag; _hil.healthy[instance] = true; - _state[instance].last_update_usec = AP_HAL::micros(); + _state[instance].last_update_usec = update_usec; } const Vector3f& Compass::getHIL(uint8_t instance) const diff --git a/libraries/AP_Compass/AP_Compass.h b/libraries/AP_Compass/AP_Compass.h index 2ca525cafe..c1d5845be6 100644 --- a/libraries/AP_Compass/AP_Compass.h +++ b/libraries/AP_Compass/AP_Compass.h @@ -273,7 +273,7 @@ public: // HIL methods void setHIL(uint8_t instance, float roll, float pitch, float yaw); - void setHIL(uint8_t instance, const Vector3f &mag); + void setHIL(uint8_t instance, const Vector3f &mag, uint32_t last_update_usec); const Vector3f& getHIL(uint8_t instance) const; void _setup_earth_field(); diff --git a/libraries/AP_Compass/AP_Compass_Backend.cpp b/libraries/AP_Compass/AP_Compass_Backend.cpp index d2a892ebdd..6545057cc9 100644 --- a/libraries/AP_Compass/AP_Compass_Backend.cpp +++ b/libraries/AP_Compass/AP_Compass_Backend.cpp @@ -84,6 +84,12 @@ void AP_Compass_Backend::publish_filtered_field(const Vector3f &mag, uint8_t ins state.last_update_usec = AP_HAL::micros(); } +void AP_Compass_Backend::set_last_update_usec(uint32_t last_update, uint8_t instance) +{ + Compass::mag_state &state = _compass._state[instance]; + state.last_update_usec = last_update; +} + /* register a new backend with frontend, returning instance which should be used in publish_field() diff --git a/libraries/AP_Compass/AP_Compass_Backend.h b/libraries/AP_Compass/AP_Compass_Backend.h index dac8222bb6..d2ed94832d 100644 --- a/libraries/AP_Compass/AP_Compass_Backend.h +++ b/libraries/AP_Compass/AP_Compass_Backend.h @@ -61,6 +61,7 @@ protected: void publish_raw_field(const Vector3f &mag, uint32_t time_us, uint8_t instance); void correct_field(Vector3f &mag, uint8_t i); void publish_filtered_field(const Vector3f &mag, uint8_t instance); + void set_last_update_usec(uint32_t last_update, uint8_t instance); // register a new compass instance with the frontend uint8_t register_compass(void) const; diff --git a/libraries/AP_Compass/AP_Compass_HIL.cpp b/libraries/AP_Compass/AP_Compass_HIL.cpp index 3b76ba2ea7..4bddce5585 100644 --- a/libraries/AP_Compass/AP_Compass_HIL.cpp +++ b/libraries/AP_Compass/AP_Compass_HIL.cpp @@ -66,7 +66,9 @@ void AP_Compass_HIL::read() rotate_field(field, compass_instance); publish_raw_field(field, AP_HAL::micros(), compass_instance); correct_field(field, compass_instance); + uint32_t saved_last_update = _compass.last_update_usec(compass_instance); publish_filtered_field(field, compass_instance); + set_last_update_usec(saved_last_update, compass_instance); } } }