From ede927f68ac20ad1dbd25b02ddce2202f2b6adae Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <tridge@samba.org>
Date: Mon, 14 Oct 2013 15:40:45 +1100
Subject: [PATCH] HAL_SITL: added airspeed noise control

---
 libraries/AP_HAL_AVR_SITL/sitl_ins.cpp | 2 +-
 libraries/SITL/SITL.cpp                | 1 +
 libraries/SITL/SITL.h                  | 2 +-
 3 files changed, 3 insertions(+), 2 deletions(-)

diff --git a/libraries/AP_HAL_AVR_SITL/sitl_ins.cpp b/libraries/AP_HAL_AVR_SITL/sitl_ins.cpp
index 9443cafbf..f6c376a6e 100644
--- a/libraries/AP_HAL_AVR_SITL/sitl_ins.cpp
+++ b/libraries/AP_HAL_AVR_SITL/sitl_ins.cpp
@@ -122,7 +122,7 @@ void SITL_State::_update_ins(float roll, 	float pitch, 	float yaw,		// Relative
 	_ins->set_gyro(Vector3f(p, q, r) + _ins->get_gyro_offsets());
 	_ins->set_accel(Vector3f(xAccel, yAccel, zAccel) + _ins->get_accel_offsets());
 
-	airspeed_pin_value = _airspeed_sensor(airspeed);
+	airspeed_pin_value = _airspeed_sensor(airspeed + (_sitl->aspd_noise * _rand_float()));
 }
 
 #endif
diff --git a/libraries/SITL/SITL.cpp b/libraries/SITL/SITL.cpp
index d0ae7236c..03cb7bbf7 100644
--- a/libraries/SITL/SITL.cpp
+++ b/libraries/SITL/SITL.cpp
@@ -45,6 +45,7 @@ const AP_Param::GroupInfo SITL::var_info[] PROGMEM = {
     AP_GROUPINFO("GPS_GLITCH",    17, SITL,  gps_glitch,  0),
     AP_GROUPINFO("GPS_HZ",        18, SITL,  gps_hertz,  5),
     AP_GROUPINFO("BATT_VOLTAGE",  19, SITL,  batt_voltage,  12.6),
+    AP_GROUPINFO("ASPD_RND",      20, SITL,  aspd_noise,  0.5),
     AP_GROUPEND
 };
 
diff --git a/libraries/SITL/SITL.h b/libraries/SITL/SITL.h
index 04baa6335..8054a0491 100644
--- a/libraries/SITL/SITL.h
+++ b/libraries/SITL/SITL.h
@@ -49,8 +49,8 @@ public:
 	AP_Float baro_noise;  // in Pascals
 	AP_Float gyro_noise;  // in degrees/second
 	AP_Float accel_noise; // in m/s/s
+	AP_Float aspd_noise;  // in m/s
 	AP_Float mag_noise;   // in mag units (earth field is 818)
-	AP_Float aspd_noise;  // in m/s 
 	AP_Float mag_error;   // in degrees
     AP_Float servo_rate;  // servo speed in degrees/second
 
-- 
GitLab