AP_HAL_AVR_SITL: added sensor delays to sitl.

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<readings_mag,mag_buffer_length> 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<readings_wind,wind_buffer_length> 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<readings_baro,baro_buffer_length> buffer_baro;
+    uint32_t time_delta_baro;
+    uint32_t delayed_time_baro;
 };
 
 #endif // CONFIG_HAL_BOARD == HAL_BOARD_AVR_SITL

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

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

libraries/AP_HAL_AVR_SITL/sitl_ins.cpp

@@ -25,6 +25,7 @@
 #include <SITL_State.h>
 #include <fenv.h>
 
+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;
 }