diff --git a/libraries/AP_Compass/AP_Compass.cpp b/libraries/AP_Compass/AP_Compass.cpp index 911b564334..4e07e673d0 100644 --- a/libraries/AP_Compass/AP_Compass.cpp +++ b/libraries/AP_Compass/AP_Compass.cpp @@ -5,6 +5,7 @@ #include #include +#include "AP_Compass_SITL.h" #include "AP_Compass_AK8963.h" #include "AP_Compass_Backend.h" #include "AP_Compass_BMM150.h" @@ -513,7 +514,12 @@ void Compass::_detect_backends(void) return; \ } \ } while (0) - + +#if CONFIG_HAL_BOARD == HAL_BOARD_SITL + ADD_BACKEND(new AP_Compass_SITL(*this), nullptr, false); + return; +#endif + #if HAL_COMPASS_DEFAULT == HAL_COMPASS_HIL ADD_BACKEND(AP_Compass_HIL::detect(*this), nullptr, false); #elif HAL_COMPASS_DEFAULT == HAL_COMPASS_PX4 || HAL_COMPASS_DEFAULT == HAL_COMPASS_VRBRAIN diff --git a/libraries/AP_Compass/AP_Compass.h b/libraries/AP_Compass/AP_Compass.h index bcd5491900..ccb94a9350 100644 --- a/libraries/AP_Compass/AP_Compass.h +++ b/libraries/AP_Compass/AP_Compass.h @@ -261,6 +261,9 @@ public: uint32_t last_update_usec(void) const { return _state[get_primary()].last_update_usec; } uint32_t last_update_usec(uint8_t i) const { return _state[i].last_update_usec; } + uint32_t last_update_ms(void) const { return _state[get_primary()].last_update_ms; } + uint32_t last_update_ms(uint8_t i) const { return _state[i].last_update_ms; } + static const struct AP_Param::GroupInfo var_info[]; // HIL variables @@ -286,7 +289,7 @@ public: uint16_t get_offsets_max(void) const { return (uint16_t)_offset_max.get(); } - + private: /// Register a new compas driver, allocating an instance number /// @@ -388,7 +391,7 @@ private: } _state[COMPASS_MAX_INSTANCES]; AP_Int16 _offset_max; - + CompassCalibrator _calibrator[COMPASS_MAX_INSTANCES]; // if we want HIL only diff --git a/libraries/AP_Compass/AP_Compass_SITL.cpp b/libraries/AP_Compass/AP_Compass_SITL.cpp new file mode 100644 index 0000000000..6dd2f7cca2 --- /dev/null +++ b/libraries/AP_Compass/AP_Compass_SITL.cpp @@ -0,0 +1,113 @@ +#include "AP_Compass_SITL.h" + +#include + +#if CONFIG_HAL_BOARD == HAL_BOARD_SITL +extern const AP_HAL::HAL& hal; + +AP_Compass_SITL::AP_Compass_SITL(Compass &compass): + _has_sample(false), + AP_Compass_Backend(compass) +{ + _sitl = (SITL::SITL *)AP_Param::find_object("SIM_"); + if (_sitl != nullptr) { + _compass._setup_earth_field(); + for (uint8_t i=0; iregister_timer_process(FUNCTOR_BIND(this, &AP_Compass_SITL::_timer, void)); + } +} + +void AP_Compass_SITL::_timer() +{ + // TODO: Refactor delay buffer with AP_Baro_SITL. + + // Sampled at 100Hz + uint32_t now = AP_HAL::millis(); + if ((now - _last_sample_time) < 10) { + return; + } + _last_sample_time = now; + + // calculate sensor noise and add to 'truth' field in body frame + // units are milli-Gauss + Vector3f noise = rand_vec3f() * _sitl->mag_noise; + Vector3f new_mag_data = _sitl->state.bodyMagField + noise; + + // add delay + uint32_t best_time_delta = 1000; // initialise large time representing buffer entry closest to current time - delay. + uint8_t best_index = 0; // initialise number representing the index of the entry in buffer closest to delay. + + // storing data from sensor to buffer + if (now - last_store_time >= 10) { // store data every 10 ms. + last_store_time = now; + if (store_index > buffer_length-1) { // reset buffer index if index greater than size of buffer + store_index = 0; + } + buffer[store_index].data = new_mag_data; // add data to current index + buffer[store_index].time = last_store_time; // add time to current index + store_index = store_index + 1; // increment index + } + + // return delayed measurement + uint32_t delayed_time = now - _sitl->mag_delay; // get time corresponding to delay + // find data corresponding to delayed time in buffer + for (uint8_t i=0; i<=buffer_length-1; i++) { + // find difference between delayed time and time stamp in buffer + uint32_t time_delta = abs((int32_t)(delayed_time - buffer[i].time)); + // if this difference is smaller than last delta, store this time + if (time_delta < best_time_delta) { + best_index= i; + best_time_delta = time_delta; + } + } + if (best_time_delta < 1000) { // only output stored state if < 1 sec retrieval error + new_mag_data = buffer[best_index].data; + } + + new_mag_data -= _sitl->mag_ofs.get(); + + for (uint8_t i=0; itake(HAL_SEMAPHORE_BLOCK_FOREVER)) { + return; + } + + _mag_accum += new_mag_data; + _accum_count++; + if (_accum_count == 10) { + _mag_accum /= 2; + _accum_count = 5; + _has_sample = true; + } + _sem->give(); +} + +void AP_Compass_SITL::read() +{ + if (_sem->take_nonblocking()) { + if (!_has_sample) { + _sem->give(); + return; + } + + Vector3f field(_mag_accum); + field /= _accum_count; + _mag_accum.zero(); + _accum_count = 0; + + for (uint8_t i=0; igive(); + } + +} +#endif diff --git a/libraries/AP_Compass/AP_Compass_SITL.h b/libraries/AP_Compass/AP_Compass_SITL.h new file mode 100644 index 0000000000..49ad98acce --- /dev/null +++ b/libraries/AP_Compass/AP_Compass_SITL.h @@ -0,0 +1,43 @@ +#pragma once + +#include "AP_Compass.h" +#include "AP_Compass_Backend.h" + +#if CONFIG_HAL_BOARD == HAL_BOARD_SITL +#include +#include +#include +#include + +#define SITL_NUM_COMPASSES 2 + +class AP_Compass_SITL : public AP_Compass_Backend { +public: + AP_Compass_SITL(Compass &); + + void read(void); + +private: + uint8_t _compass_instance[SITL_NUM_COMPASSES]; + SITL::SITL *_sitl; + + // delay buffer variables + struct readings_compass { + uint32_t time; + Vector3f data; + }; + uint8_t store_index; + uint32_t last_store_time; + static const uint8_t buffer_length = 50; + VectorN buffer; + + void _timer(); + bool _has_sample; + uint32_t _last_sample_time; + + Vector3f _mag_accum; + uint32_t _accum_count; + + +}; +#endif // CONFIG_HAL_BOARD