diff --git a/libraries/AP_HAL_AVR_SITL/SITL_State.h b/libraries/AP_HAL_AVR_SITL/SITL_State.h index 1885fdf6a7..d83b290c6c 100644 --- a/libraries/AP_HAL_AVR_SITL/SITL_State.h +++ b/libraries/AP_HAL_AVR_SITL/SITL_State.h @@ -138,6 +138,46 @@ private: float _current; bool _synthetic_clock_mode; + + // delay buffer variables + static const uint8_t mag_buffer_length = 250; + static const uint8_t wind_buffer_length = 50; + static const uint8_t baro_buffer_length = 50; + + // magnetometer delay buffer variables + struct readings_mag { + uint32_t time; + Vector3f data; + }; + uint8_t store_index_mag; + uint32_t last_store_time_mag; + VectorN buffer_mag; + uint32_t time_delta_mag; + uint32_t delayed_time_mag; + Vector3f mag_data; + Vector3f new_mag_data; + + // airspeed sensor delay buffer variables + struct readings_wind { + uint32_t time; + float data; + }; + uint8_t store_index_wind; + uint32_t last_store_time_wind; + VectorN buffer_wind; + uint32_t time_delta_wind; + uint32_t delayed_time_wind; + + // barometer delay buffer variables + struct readings_baro { + uint32_t time; + float data; + }; + uint8_t store_index_baro; + uint32_t last_store_time_baro; + VectorN buffer_baro; + uint32_t time_delta_baro; + uint32_t delayed_time_baro; }; #endif // CONFIG_HAL_BOARD == HAL_BOARD_AVR_SITL diff --git a/libraries/AP_HAL_AVR_SITL/sitl_barometer.cpp b/libraries/AP_HAL_AVR_SITL/sitl_barometer.cpp index ff5e91719a..d2db65ad86 100644 --- a/libraries/AP_HAL_AVR_SITL/sitl_barometer.cpp +++ b/libraries/AP_HAL_AVR_SITL/sitl_barometer.cpp @@ -29,34 +29,65 @@ extern const AP_HAL::HAL& hal; */ void SITL_State::_update_barometer(float altitude) { - static uint32_t last_update; + static uint32_t last_update; - float sim_alt = altitude; + float sim_alt = altitude; - if (_barometer == NULL) { - // this sketch doesn't use a barometer - return; - } + if (_barometer == NULL) { + // this sketch doesn't use a barometer + return; + } - if (_sitl->baro_disable) { - // barometer is disabled - return; + if (_sitl->baro_disable) { + // barometer is disabled + return; + } + + // 80Hz, to match the real APM2 barometer + uint32_t now = hal.scheduler->millis(); + if ((now - last_update) < 12) { + return; + } + last_update = now; + + sim_alt += _sitl->baro_drift * now / 1000; + sim_alt += _sitl->baro_noise * _rand_float(); + + // add baro glitch + sim_alt += _sitl->baro_glitch; + + // add delay + uint32_t best_time_delta_baro = 200; // initialise large time representing buffer entry closest to current time - delay. + uint8_t best_index_baro = 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_baro >= 10) { // store data every 10 ms. + last_store_time_baro = now; + if (store_index_baro > baro_buffer_length-1) { // reset buffer index if index greater than size of buffer + store_index_baro = 0; + } + buffer_baro[store_index_baro].data = sim_alt; // add data to current index + buffer_baro[store_index_baro].time = last_store_time_baro; // add time_stamp_baro to current index + store_index_baro = store_index_baro + 1; // increment index + } + + // return delayed measurement + delayed_time_baro = now - _sitl->baro_delay; // get time corresponding to delay + + // find data corresponding to delayed time in buffer + for (uint8_t i=0; i<=baro_buffer_length-1; i++) { + time_delta_baro = abs(delayed_time_baro - buffer_baro[i].time); // find difference between delayed time and time stamp in buffer + // if this difference is smaller than last delta, store this time + if (time_delta_baro < best_time_delta_baro) { + best_index_baro = i; + best_time_delta_baro = time_delta_baro; } + } + if (best_time_delta_baro < 200) { // only output stored state if < 200 msec retrieval error + sim_alt = buffer_baro[best_index_baro].data; + } - // 80Hz, to match the real APM2 barometer - uint32_t now = hal.scheduler->millis(); - if ((now - last_update) < 12) { - return; - } - last_update = now; - - sim_alt += _sitl->baro_drift * now / 1000; - sim_alt += _sitl->baro_noise * _rand_float(); - - // add baro glitch - sim_alt += _sitl->baro_glitch; - - _barometer->setHIL(sim_alt); + _barometer->setHIL(sim_alt); } #endif diff --git a/libraries/AP_HAL_AVR_SITL/sitl_compass.cpp b/libraries/AP_HAL_AVR_SITL/sitl_compass.cpp index 64d00004d6..5ea684e76d 100644 --- a/libraries/AP_HAL_AVR_SITL/sitl_compass.cpp +++ b/libraries/AP_HAL_AVR_SITL/sitl_compass.cpp @@ -19,6 +19,8 @@ #include "../AP_Declination/AP_Declination.h" #include "../SITL/SITL.h" +extern const AP_HAL::HAL& hal; + using namespace AVR_SITL; /* @@ -27,21 +29,55 @@ using namespace AVR_SITL; */ void SITL_State::_update_compass(float rollDeg, float pitchDeg, float yawDeg) { - if (_compass == NULL) { - // no compass in this sketch - return; - } - yawDeg += _sitl->mag_error; - if (yawDeg > 180.0f) { - yawDeg -= 360.0f; - } - if (yawDeg < -180.0f) { - yawDeg += 360.0f; - } - _compass->setHIL(radians(rollDeg), radians(pitchDeg), radians(yawDeg)); - Vector3f noise = _rand_vec3f() * _sitl->mag_noise; + if (_compass == NULL) { + // no compass in this sketch + return; + } + yawDeg += _sitl->mag_error; + if (yawDeg > 180.0f) { + yawDeg -= 360.0f; + } + if (yawDeg < -180.0f) { + yawDeg += 360.0f; + } + _compass->setHIL(radians(rollDeg), radians(pitchDeg), radians(yawDeg)); + Vector3f noise = _rand_vec3f() * _sitl->mag_noise; Vector3f motor = _sitl->mag_mot.get() * _current; - _compass->setHIL(_compass->getHIL() + noise+motor); + + new_mag_data = _compass->getHIL() + noise + motor; + + uint32_t now = hal.scheduler->millis(); + // 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++) { + time_delta_mag = abs(delayed_time_mag - buffer_mag[i].time); // find difference between delayed time and time stamp in buffer + // 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 + mag_data = buffer_mag[best_index_mag].data; + } + + _compass->setHIL(mag_data); } #endif diff --git a/libraries/AP_HAL_AVR_SITL/sitl_ins.cpp b/libraries/AP_HAL_AVR_SITL/sitl_ins.cpp index e8472199a9..3686342195 100644 --- a/libraries/AP_HAL_AVR_SITL/sitl_ins.cpp +++ b/libraries/AP_HAL_AVR_SITL/sitl_ins.cpp @@ -25,6 +25,7 @@ #include #include +extern const AP_HAL::HAL& hal; using namespace AVR_SITL; @@ -42,6 +43,37 @@ uint16_t SITL_State::_airspeed_sensor(float airspeed) if (airspeed_raw/4 > 0xFFFF) { return 0xFFFF; } + // add delay + uint32_t now = hal.scheduler->millis(); + uint32_t best_time_delta_wind = 200; // initialise large time representing buffer entry closest to current time - delay. + uint8_t best_index_wind = 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_wind >= 10) { // store data every 10 ms. + last_store_time_wind = now; + if (store_index_wind > wind_buffer_length-1) { // reset buffer index if index greater than size of buffer + store_index_wind = 0; + } + buffer_wind[store_index_wind].data = airspeed_raw; // add data to current index + buffer_wind[store_index_wind].time = last_store_time_wind; // add time to current index + store_index_wind = store_index_wind + 1; // increment index + } + + // return delayed measurement + delayed_time_wind = now - _sitl->wind_delay; // get time corresponding to delay + // find data corresponding to delayed time in buffer + for (uint8_t i=0; i<=wind_buffer_length-1; i++) { + time_delta_wind = abs(delayed_time_wind - buffer_wind[i].time); // find difference between delayed time and time stamp in buffer + // if this difference is smaller than last delta, store this time + if (time_delta_wind < best_time_delta_wind) { + best_index_wind = i; + best_time_delta_wind = time_delta_wind; + } + } + if (best_time_delta_wind < 200) { // only output stored state if < 200 msec retrieval error + airspeed_raw = buffer_wind[best_index_wind].data; + } + return airspeed_raw/4; }