Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-23 00:04:02 -04:00
Code Issues Packages Projects Releases Wiki Activity

SITL: Add noise and glitching behaviour for the sonar sensor

Browse Source
This commit is contained in:
Ben Nizette 2013-11-26 16:58:30 +11:00 committed by Andrew Tridgell
parent 70d37d2cf6
commit 339da1c21b
3 changed files with 10 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
libraries/AP_HAL_AVR_SITL/sitl_ins.cpp
View File

@ -67,6 +67,12 @@ uint16_t SITL_State::_ground_sonar(float altitude)
home_alt = altitude;
altitude = altitude - home_alt;
altitude += _sitl->sonar_noise * _rand_float();
if (_sitl->sonar_glitch >= (_rand_float() + 1.0f)/2.0f)
altitude = AP_RANGEFINDER_MAXSONARXL_MAX_DISTANCE / 100.0f;
altitude = constrain_float(altitude,
AP_RANGEFINDER_MAXSONARXL_MIN_DISTANCE / 100.0f,
AP_RANGEFINDER_MAXSONARXL_MAX_DISTANCE / 100.0f);

2
libraries/SITL/SITL.cpp
View File

@ -49,6 +49,8 @@ const AP_Param::GroupInfo SITL::var_info[] PROGMEM = {
AP_GROUPINFO("ACCEL_FAIL", 21, SITL, accel_fail, 0),
AP_GROUPINFO("BARO_DRIFT", 22, SITL, baro_drift, 0),
AP_GROUPINFO("SONAR_CON", 23, SITL, sonar_connected, 0),
AP_GROUPINFO("SONAR_GLITCH", 24, SITL, sonar_glitch, 0),
AP_GROUPINFO("SONAR_RND", 25, SITL, sonar_noise, 0),
AP_GROUPEND
};

2
libraries/SITL/SITL.h
View File

@ -56,6 +56,8 @@ public:
AP_Float servo_rate; // servo speed in degrees/second
AP_Float sonar_connected; // Analogue sonar connected to AnalogPin 0 (else airspeed)
AP_Float sonar_glitch;// probablility between 0-1 that any given sonar sample will read as max distance
AP_Float sonar_noise; // in metres
AP_Float drift_speed; // degrees/second/minute
AP_Float drift_time; // period in minutes