Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

SITL: add SIM_ADSB_COUNT, RADIUS, ALT params

This commit is contained in:
Tom Pittenger 2016-06-14 20:47:19 -07:00
parent ad81e56a64
commit 95da4accfb
4 changed files with 46 additions and 9 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

42
libraries/SITL/SIM_ADSB.cpp
View File

@ -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;
}

5
libraries/SITL/SIM_ADSB.h
View File

@ -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;

3
libraries/SITL/SITL.cpp
View File

@ -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
};

5
libraries/SITL/SITL.h
View File

@ -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);