From 95da4accfb1be5b67db8563e23106468221e2e0e Mon Sep 17 00:00:00 2001
From: Tom Pittenger <magicrub@gmail.com>
Date: Tue, 14 Jun 2016 20:47:19 -0700
Subject: [PATCH] SITL: add SIM_ADSB_COUNT, RADIUS, ALT params

---
 libraries/SITL/SIM_ADSB.cpp | 42 ++++++++++++++++++++++++++++++-------
 libraries/SITL/SIM_ADSB.h   |  5 +++--
 libraries/SITL/SITL.cpp     |  3 +++
 libraries/SITL/SITL.h       |  5 +++++
 4 files changed, 46 insertions(+), 9 deletions(-)

diff --git a/libraries/SITL/SIM_ADSB.cpp b/libraries/SITL/SIM_ADSB.cpp
index ce5d61a155..b01022bb8c 100644
--- a/libraries/SITL/SIM_ADSB.cpp
+++ b/libraries/SITL/SIM_ADSB.cpp
@@ -18,6 +18,7 @@
 */
 
 #include "SIM_ADSB.h"
+#include "SITL.h"
 
 #include <stdio.h>
 
@@ -25,6 +26,8 @@
 
 namespace SITL {
 
+SITL *_sitl;
+
 ADSB::ADSB(const struct sitl_fdm &_fdm, const char *_home_str) :
     fdm(_fdm)
 {
@@ -42,11 +45,20 @@ void ADSB_Vehicle::update(float delta_t)
         initialised = true;
         ICAO_address = (uint32_t)(rand() % 10000);
         snprintf(callsign, sizeof(callsign), "SIM%u", ICAO_address);
-        position.x = Aircraft::rand_normal(0, 1000);
-        position.y = Aircraft::rand_normal(0, 1000);
-        position.z = -fabsf(Aircraft::rand_normal(3000, 1000));
-        velocity_ef.x = Aircraft::rand_normal(5, 20);
-        velocity_ef.y = Aircraft::rand_normal(5, 20);
+        position.x = Aircraft::rand_normal(0, _sitl->adsb_radius_m);
+        position.y = Aircraft::rand_normal(0, _sitl->adsb_radius_m);
+        position.z = -fabsf(_sitl->adsb_altitude_m);
+
+        double vel_min = 5, vel_max = 20;
+        if (position.length() > 500) {
+            vel_min *= 3;
+            vel_max *= 3;
+        } else if (position.length() > 10000) {
+            vel_min *= 10;
+            vel_max *= 10;
+        }
+        velocity_ef.x = Aircraft::rand_normal(vel_min, vel_max);
+        velocity_ef.y = Aircraft::rand_normal(vel_min, vel_max);
         velocity_ef.z = Aircraft::rand_normal(0, 3);
     }
 
@@ -62,6 +74,22 @@ void ADSB_Vehicle::update(float delta_t)
 */
 void ADSB::update(void)
 {
+    if (_sitl == nullptr) {
+        _sitl = (SITL *)AP_Param::find_object("SIM_");
+        return;
+    } else if (_sitl->adsb_plane_count <= 0) {
+        return;
+    } else if (_sitl->adsb_plane_count >= num_vehicles_MAX) {
+        _sitl->adsb_plane_count.set_and_save(0);
+        num_vehicles = 0;
+        return;
+    } else if (num_vehicles != _sitl->adsb_plane_count) {
+        num_vehicles = _sitl->adsb_plane_count;
+        for (uint8_t i=0; i<num_vehicles_MAX; i++) {
+            vehicles[i].initialised = false;
+        }
+    }
+
     // calculate delta time in seconds
     uint32_t now_us = AP_HAL::micros();
 
@@ -166,8 +194,8 @@ void ADSB::send_report(void)
 
             location_offset(loc, vehicle.position.x, vehicle.position.y);
 
-            // re-init when over 50km from home
-            if (get_distance(home, loc) > 1000) {
+            // re-init when exceeding radius range
+            if (get_distance(home, loc) > _sitl->adsb_radius_m) {
                 vehicle.initialised = false;
             }
             
diff --git a/libraries/SITL/SIM_ADSB.h b/libraries/SITL/SIM_ADSB.h
index 071f09d8d9..a535567800 100644
--- a/libraries/SITL/SIM_ADSB.h
+++ b/libraries/SITL/SIM_ADSB.h
@@ -52,8 +52,9 @@ private:
     const uint16_t target_port = 5762;
 
     Location home;
-    static const uint8_t num_vehicles = 6;
-    ADSB_Vehicle vehicles[num_vehicles];
+    uint8_t num_vehicles = 0;
+    static const uint8_t num_vehicles_MAX = 200;
+    ADSB_Vehicle vehicles[num_vehicles_MAX];
     
     // reporting period in ms
     const float reporting_period_ms = 500;
diff --git a/libraries/SITL/SITL.cpp b/libraries/SITL/SITL.cpp
index b3b93df4df..b26184e314 100644
--- a/libraries/SITL/SITL.cpp
+++ b/libraries/SITL/SITL.cpp
@@ -76,6 +76,9 @@ const AP_Param::GroupInfo SITL::var_info[] = {
     AP_GROUPINFO("ACC2_RND",      42, SITL,  accel2_noise, 0),
     AP_GROUPINFO("ARSPD_FAIL",    43, SITL,  arspd_fail, 0),
     AP_GROUPINFO("GYR_SCALE",     44, SITL,  gyro_scale, 0),
+    AP_GROUPINFO("ADSB_COUNT",    45, SITL,  adsb_plane_count, 0),
+    AP_GROUPINFO("ADSB_RADIUS",   46, SITL,  adsb_radius_m, 1000),
+    AP_GROUPINFO("ADSB_ALT",      47, SITL,  adsb_altitude_m, 1000),
     AP_GROUPEND
 };
 
diff --git a/libraries/SITL/SITL.h b/libraries/SITL/SITL.h
index 85e59dec31..74ab2a927d 100644
--- a/libraries/SITL/SITL.h
+++ b/libraries/SITL/SITL.h
@@ -113,6 +113,11 @@ public:
     AP_Int16  mag_delay; // magnetometer data delay in ms
     AP_Int16  wind_delay; // windspeed data delay in ms
 
+    // ADSB related run-time options
+    AP_Int16 adsb_plane_count;
+    AP_Float adsb_radius_m;
+    AP_Float adsb_altitude_m;
+
     void simstate_send(mavlink_channel_t chan);
 
     void Log_Write_SIMSTATE(DataFlash_Class *dataflash);