From 86b9942d5124a6f54f137993461b0d0e1a0111d9 Mon Sep 17 00:00:00 2001 From: Miguel Arroyo Date: Wed, 21 Jun 2017 13:39:07 -0400 Subject: [PATCH] AP_HAL_SITL: move SITL Compass to standard sensor backend model --- libraries/AP_HAL_SITL/SITL_State.cpp | 16 ----- libraries/AP_HAL_SITL/SITL_State.h | 2 - libraries/AP_HAL_SITL/sitl_compass.cpp | 87 -------------------------- 3 files changed, 105 deletions(-) delete mode 100644 libraries/AP_HAL_SITL/sitl_compass.cpp diff --git a/libraries/AP_HAL_SITL/SITL_State.cpp b/libraries/AP_HAL_SITL/SITL_State.cpp index 5c30a68d3f..01e9236a0d 100644 --- a/libraries/AP_HAL_SITL/SITL_State.cpp +++ b/libraries/AP_HAL_SITL/SITL_State.cpp @@ -88,7 +88,6 @@ void SITL_State::_sitl_setup(const char *home_str) // setup some initial values #ifndef HIL_MODE _update_airspeed(0); - _update_compass(); _update_gps(0, 0, 0, 0, 0, 0, false); _update_rangefinder(0); #endif @@ -167,7 +166,6 @@ void SITL_State::_fdm_input_step(void) _sitl->state.speedN, _sitl->state.speedE, _sitl->state.speedD, !_sitl->gps_disable); _update_airspeed(_sitl->state.airspeed); - _update_compass(); _update_rangefinder(_sitl->state.range); if (_sitl->adsb_plane_count >= 0 && @@ -431,20 +429,6 @@ void SITL_State::_simulator_servos(SITL::Aircraft::sitl_input &input) current_pin_value = ((_current / 17.0f) / 5.0f) * 1024; } - -// generate a random Vector3f of size 1 -Vector3f SITL_State::_rand_vec3f(void) -{ - Vector3f v = Vector3f(rand_float(), - rand_float(), - rand_float()); - if (v.length() != 0.0f) { - v.normalize(); - } - return v; -} - - void SITL_State::init(int argc, char * const argv[]) { pwm_input[0] = pwm_input[1] = pwm_input[3] = 1500; diff --git a/libraries/AP_HAL_SITL/SITL_State.h b/libraries/AP_HAL_SITL/SITL_State.h index 72c9eeb69f..d9e4bf0bd7 100644 --- a/libraries/AP_HAL_SITL/SITL_State.h +++ b/libraries/AP_HAL_SITL/SITL_State.h @@ -83,7 +83,6 @@ private: void _setup_adc(void); void set_height_agl(void); - void _update_compass(void); void _update_rangefinder(float range_value); void _set_signal_handlers(void) const; @@ -131,7 +130,6 @@ private: void _simulator_output(bool synthetic_clock_mode); uint16_t _airspeed_sensor(float airspeed); uint16_t _ground_sonar(); - Vector3f _rand_vec3f(void); void _fdm_input_step(void); void wait_clock(uint64_t wait_time_usec); diff --git a/libraries/AP_HAL_SITL/sitl_compass.cpp b/libraries/AP_HAL_SITL/sitl_compass.cpp deleted file mode 100644 index b10d76643f..0000000000 --- a/libraries/AP_HAL_SITL/sitl_compass.cpp +++ /dev/null @@ -1,87 +0,0 @@ -/* - SITL handling - - This simulates a compass - - Andrew Tridgell November 2011 - */ - -#include -#if CONFIG_HAL_BOARD == HAL_BOARD_SITL - -#include "AP_HAL_SITL.h" -#include "AP_HAL_SITL_Namespace.h" -#include "HAL_SITL_Class.h" - -#include -#include -#include -#include - -extern const AP_HAL::HAL& hal; - -using namespace HALSITL; - -/* - setup the compass with new input - all inputs are in degrees - */ -void SITL_State::_update_compass(void) -{ - static uint32_t last_update; - - if (_compass == nullptr) { - // no compass in this sketch - return; - } - - // 100Hz - uint32_t now = AP_HAL::millis(); - if ((now - last_update) < 10) { - return; - } - last_update = 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_mag = 1000; // initialise large time representing buffer entry closest to current time - delay. - uint8_t best_index_mag = 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_mag >= 10) { // store data every 10 ms. - last_store_time_mag = now; - if (store_index_mag > mag_buffer_length-1) { // reset buffer index if index greater than size of buffer - store_index_mag = 0; - } - buffer_mag[store_index_mag].data = new_mag_data; // add data to current index - buffer_mag[store_index_mag].time = last_store_time_mag; // add time to current index - store_index_mag = store_index_mag + 1; // increment index - } - - // return delayed measurement - delayed_time_mag = now - _sitl->mag_delay; // get time corresponding to delay - // find data corresponding to delayed time in buffer - for (uint8_t i=0; i<=mag_buffer_length-1; i++) { - // find difference between delayed time and time stamp in buffer - time_delta_mag = abs((int32_t)(delayed_time_mag - buffer_mag[i].time)); - // if this difference is smaller than last delta, store this time - if (time_delta_mag < best_time_delta_mag) { - best_index_mag = i; - best_time_delta_mag = time_delta_mag; - } - } - if (best_time_delta_mag < 1000) { // only output stored state if < 1 sec retrieval error - new_mag_data = buffer_mag[best_index_mag].data; - } - - new_mag_data -= _sitl->mag_ofs.get(); - - _compass->setHIL(0, new_mag_data, AP_HAL::micros()); - _compass->setHIL(1, new_mag_data, AP_HAL::micros()); -} - -#endif