void func_1()
{
int iVar0;
iVar0 = 0;
while (iVar0 < 4)
{
if (ENTITY::DOES_ENTITY_EXIST(iLocal_49[iVar0]))
{
if (ENTITY::IS_ENTITY_OCCLUDED(iLocal_49[iVar0]) || (!ENTITY::IS_ENTITY_DEAD(iLocal_49[iVar0], 0) && !is_entity_visible(iLocal_49[iVar0])))
{
delete_vehicle(&(iLocal_49[iVar0]));
PED::DELETE_PED(&(iLocal_55[iVar0]));
}
else
{
if (iVar0 == 0 || iVar0 == 2)
{
func_3(1);
}
func_2(&(iLocal_49[iVar0]), &(iLocal_55[iVar0]));
set_vehicle_as_no_longer_needed(&(iLocal_49[iVar0]));
set_ped_as_no_longer_needed(&(iLocal_55[iVar0]));
}
}
iVar0++;
}
if (ENTITY::DOES_ENTITY_EXIST(iLocal_54))
{
if (ENTITY::IS_ENTITY_OCCLUDED(iLocal_54))
{
delete_vehicle(&iLocal_54);
PED::DELETE_PED(&iLocal_60);
}
else
{
func_2(&iLocal_54, &iLocal_60);
set_vehicle_as_no_longer_needed(&iLocal_54);
set_ped_as_no_longer_needed(&iLocal_60);
}
}
if (iLocal_82)
{
remove_vehicle_recording(101, "AirportJetTakeOff");
remove_vehicle_recording(102, "AirportJetTakeOff");
remove_vehicle_recording(101, "AirportNew");
remove_vehicle_recording(102, "AirportNew");
remove_vehicle_recording(103, "AirplaneLandingRedux");
remove_vehicle_recording(104, "AirplaneLandingRedux");
remove_vehicle_recording(101, "EastWestFlight");
}
}
static void show_gtfsrt (uint8_t *buf, size_t len) {
TransitRealtime__FeedMessage *msg;
msg = transit_realtime__feed_message__unpack (NULL, len, buf);
if (msg == NULL) {
fprintf (stderr, "error unpacking incoming gtfs-rt message\n");
return;
}
printf("Received feed message with %lu entities.\n", msg->n_entity);
for (int e = 0; e < msg->n_entity; ++e) {
TransitRealtime__FeedEntity *entity = msg->entity[e];
printf(" entity %d has id %s\n", e, entity->id);
if (entity->has_is_deleted && entity->is_deleted) {
delete_vehicle(entity->id);
} else {
if (entity->vehicle && entity->vehicle->vehicle && entity->vehicle->position) {
int32_t delay = 0;
if (entity->vehicle->ovapi_vehicle_position) {
delay = entity->vehicle->ovapi_vehicle_position->delay;
}
printf(" vehicle id: %s at %.4f, %.4f delay %d\n",
entity->vehicle->vehicle->id,
entity->vehicle->position->latitude,
entity->vehicle->position->longitude,
delay);
update_vehicle(lon2x_d((GLfloat) entity->vehicle->position->longitude),
lat2y_d((GLfloat) entity->vehicle->position->latitude),
delay,
entity->id);
}
}
}
transit_realtime__feed_message__free_unpacked (msg, NULL);
}
/*
* periodic update to handle vehicle timeouts and trigger collision detection
*/
void AP_ADSB::update(void)
{
if (!_enabled) {
if (_vehicle_list != NULL) {
deinit();
}
// nothing to do
return;
} else if (_vehicle_list == NULL) {
init();
return;
}
uint16_t index = 0;
while (index < _vehicle_count) {
// check list and drop stale vehicles. When disabled, the list will get flushed
if (AP_HAL::millis() - _vehicle_list[index].last_update_ms > VEHICLE_TIMEOUT_MS) {
// don't increment index, we want to check this same index again because the contents changed
// also, if we're disabled then clear the list
delete_vehicle(index);
} else {
index++;
}
}
perform_threat_detection();
//hal.console->printf("ADSB: cnt %u, lowT %.0f, highT %.0f\r", _vehicle_count, _lowest_threat_distance, _highest_threat_distance);
}
void *accept_connection(void *data)
{
char command;
struct accept_connection_data *d = (struct accept_connection_data*)data;
if(socket_read(d->fd, &command, 1) == 1)
{
switch(command)
{
case 'r':
register_vehicle(d->fd, d->vehicles);
break;
case 'd':
delete_vehicle(d->fd, d->vehicles);
break;
case 'h':
get_history(d->fd, d->vehicles);
break;
case 't':
calculate_distance(d->fd, d->vehicles);
break;
case 's':
check_status(d->fd, d->vehicles);
break;
}
}
TEMP_FAILURE_RETRY(close(d->fd));
stop_detached_thread(&d->mutex, d);
return NULL;
}
void func_13()
{
Vector3 vVar0;
Vector3 vVar3;
Vector3 vVar6;
Vector3 fVar9;
if (ENTITY::DOES_ENTITY_EXIST(iLocal_89))
{
if (ENTITY::IS_ENTITY_OCCLUDED(iLocal_89))
{
delete_vehicle(&iLocal_89);
}
else if (!ENTITY::IS_ENTITY_DEAD(iLocal_89, 0) && !PED::IS_PED_INJURED(iLocal_90))
{
stop_playback_recorded_vehicle(iLocal_89);
set_ped_keep_task(iLocal_90, true);
vVar0 = {ENTITY::GET_ENTITY_COORDS(iLocal_89, 1)};
fVar9 = get_entity_heading(iLocal_89);
vVar3 = {0f, 500f, 50f};
vVar6 = {_get_object_offset_from_coords(vVar0, fVar9, vVar3)};
task_heli_mission(iLocal_90, iLocal_89, false, false, vVar6, 4, 50f, -1f, 0f, 100, 50, -1f, 0);
}
}
if (iLocal_95)
{
remove_vehicle_recording(102, "HelicopterTakeOff");
}
}
/*
* Update the vehicle list. If the vehicle is already in the
* list then it will update it, otherwise it will be added.
*/
void AP_ADSB::update_vehicle(const mavlink_message_t* packet)
{
if (in_state.vehicle_list == nullptr) {
// We are only null when disabled. Updating is inhibited.
return;
}
uint16_t index = in_state.list_size + 1; // initialize with invalid index
adsb_vehicle_t vehicle {};
mavlink_msg_adsb_vehicle_decode(packet, &vehicle.info);
Location_Class vehicle_loc = Location_Class(AP_ADSB::get_location(vehicle));
bool my_loc_is_zero = _my_loc.is_zero();
float my_loc_distance_to_vehicle = _my_loc.get_distance(vehicle_loc);
bool out_of_range = in_state.list_radius > 0 && !my_loc_is_zero && my_loc_distance_to_vehicle > in_state.list_radius;
bool is_tracked_in_list = find_index(vehicle, &index);
if (vehicle_loc.is_zero() || out_of_range) {
// vehicle is out of range or invalid lat/lng. If we're tracking it, delete from list. Otherwise ignore it.
if (is_tracked_in_list) {
delete_vehicle(index);
}
return;
} else if (is_tracked_in_list) {
// found, update it
set_vehicle(index, vehicle);
} else if (in_state.vehicle_count < in_state.list_size) {
// not found and there's room, add it to the end of the list
set_vehicle(in_state.vehicle_count, vehicle);
in_state.vehicle_count++;
} else if (!my_loc_is_zero &&
!is_zero(avoid_state.lowest_threat_distance) &&
my_loc_distance_to_vehicle < avoid_state.lowest_threat_distance) { // is closer than the furthest
// buffer is full, replace the vehicle with lowest threat as long as it's not further away
// overwrite the lowest_threat/furthest
index = avoid_state.lowest_threat_index;
set_vehicle(index, vehicle);
// this is now invalid because the vehicle was overwritten, need
// to run perform_threat_detection() to determine new one because
// we aren't keeping track of the second-furthest vehicle.
avoid_state.lowest_threat_distance = 0;
// is it the nearest? Then it's the highest threat. That's an easy check
// that we don't need to run perform_threat_detection() to determine
if (avoid_state.highest_threat_distance > my_loc_distance_to_vehicle) {
avoid_state.highest_threat_distance = my_loc_distance_to_vehicle;
avoid_state.highest_threat_index = index;
}
} // if buffer full
vehicle.last_update_ms = AP_HAL::millis() - (vehicle.info.tslc * 1000);
push_sample(vehicle); // note that set_vehicle modifies vehicle
}
/*
* Update the vehicle list. If the vehicle is already in the
* list then it will update it, otherwise it will be added.
*/
void AP_ADSB::handle_vehicle(const mavlink_message_t* packet)
{
if (in_state.vehicle_list == nullptr) {
// We are only null when disabled. Updating is inhibited.
return;
}
uint16_t index = in_state.list_size + 1; // initialize with invalid index
adsb_vehicle_t vehicle {};
mavlink_msg_adsb_vehicle_decode(packet, &vehicle.info);
Location_Class vehicle_loc = Location_Class(AP_ADSB::get_location(vehicle));
bool my_loc_is_zero = _my_loc.is_zero();
float my_loc_distance_to_vehicle = _my_loc.get_distance(vehicle_loc);
bool out_of_range = in_state.list_radius > 0 && !my_loc_is_zero && my_loc_distance_to_vehicle > in_state.list_radius;
bool is_tracked_in_list = find_index(vehicle, &index);
uint32_t now = AP_HAL::millis();
// note the last time the receiver got a packet from the aircraft
vehicle.last_update_ms = now - (vehicle.info.tslc * 1000);
const uint16_t required_flags_position = ADSB_FLAGS_VALID_COORDS | ADSB_FLAGS_VALID_ALTITUDE;
const bool detected_ourself = (out_state.cfg.ICAO_id != 0) && ((uint32_t)out_state.cfg.ICAO_id == vehicle.info.ICAO_address);
if (vehicle_loc.is_zero() ||
out_of_range ||
detected_ourself ||
(vehicle.info.ICAO_address > 0x00FFFFFF) || // ICAO address is 24bits, so ignore higher values.
!(vehicle.info.flags & required_flags_position) ||
now - vehicle.last_update_ms > VEHICLE_TIMEOUT_MS) {
// vehicle is out of range or invalid lat/lng. If we're tracking it, delete from list. Otherwise ignore it.
if (is_tracked_in_list) {
delete_vehicle(index);
}
return;
} else if (is_tracked_in_list) {
// found, update it
set_vehicle(index, vehicle);
} else if (in_state.vehicle_count < in_state.list_size) {
// not found and there's room, add it to the end of the list
set_vehicle(in_state.vehicle_count, vehicle);
in_state.vehicle_count++;
} else {
// buffer is full. if new vehicle is closer than furthest, replace furthest with new
if (my_loc_is_zero) {
// nothing else to do
furthest_vehicle_distance = 0;
furthest_vehicle_index = 0;
} else {
if (furthest_vehicle_distance <= 0) {
// ensure this is populated
determine_furthest_aircraft();
}
if (my_loc_distance_to_vehicle < furthest_vehicle_distance) { // is closer than the furthest
// replace with the furthest vehicle
set_vehicle(furthest_vehicle_index, vehicle);
// furthest_vehicle_index is now invalid because the vehicle was overwritten, need
// to run determine_furthest_aircraft() to determine a new one next time
furthest_vehicle_distance = 0;
furthest_vehicle_index = 0;
}
}
} // if buffer full
const uint16_t required_flags_avoidance =
ADSB_FLAGS_VALID_COORDS |
ADSB_FLAGS_VALID_ALTITUDE |
ADSB_FLAGS_VALID_HEADING |
ADSB_FLAGS_VALID_VELOCITY;
if (vehicle.info.flags & required_flags_avoidance) {
push_sample(vehicle); // note that set_vehicle modifies vehicle
}
}
/*
* periodic update to handle vehicle timeouts and trigger collision detection
*/
void AP_ADSB::update(void)
{
// update _my_loc
if (!AP::ahrs().get_position(_my_loc)) {
_my_loc.zero();
}
if (!_enabled) {
if (in_state.vehicle_list != nullptr) {
deinit();
}
// nothing to do
return;
} else if (in_state.vehicle_list == nullptr) {
init();
return;
} else if (in_state.list_size != in_state.list_size_param) {
deinit();
return;
}
uint32_t now = AP_HAL::millis();
// check current list for vehicles that time out
uint16_t index = 0;
while (index < in_state.vehicle_count) {
// check list and drop stale vehicles. When disabled, the list will get flushed
if (now - in_state.vehicle_list[index].last_update_ms > VEHICLE_TIMEOUT_MS) {
// don't increment index, we want to check this same index again because the contents changed
// also, if we're disabled then clear the list
delete_vehicle(index);
} else {
index++;
}
}
if (_my_loc.is_zero()) {
// if we don't have a GPS lock then there's nothing else to do
return;
}
if (out_state.chan < 0) {
// if there's no transceiver detected then do not set ICAO and do not service the transceiver
return;
}
if (out_state.cfg.squawk_octal_param != out_state.cfg.squawk_octal) {
// param changed, check that it's a valid octal
if (!is_valid_octal(out_state.cfg.squawk_octal_param)) {
// invalid, reset it to default
out_state.cfg.squawk_octal_param = ADSB_SQUAWK_OCTAL_DEFAULT;
}
out_state.cfg.squawk_octal = (uint16_t)out_state.cfg.squawk_octal_param;
}
// ensure it's positive 24bit but allow -1
if (out_state.cfg.ICAO_id_param <= -1 || out_state.cfg.ICAO_id_param > 0x00FFFFFF) {
// icao param of -1 means static information is not sent, transceiver is assumed pre-programmed.
// reset timer constantly so it never reaches 10s so it never sends
out_state.last_config_ms = now;
} else if (out_state.cfg.ICAO_id == 0 ||
out_state.cfg.ICAO_id_param_prev != out_state.cfg.ICAO_id_param) {
// if param changed then regenerate. This allows the param to be changed back to zero to trigger a re-generate
if (out_state.cfg.ICAO_id_param == 0) {
out_state.cfg.ICAO_id = genICAO(_my_loc);
} else {
out_state.cfg.ICAO_id = out_state.cfg.ICAO_id_param;
}
out_state.cfg.ICAO_id_param_prev = out_state.cfg.ICAO_id_param;
set_callsign("PING", true);
gcs().send_text(MAV_SEVERITY_INFO, "ADSB: Using ICAO_id %d and Callsign %s", out_state.cfg.ICAO_id, out_state.cfg.callsign);
out_state.last_config_ms = 0; // send now
}
// send static configuration data to transceiver, every 5s
if (out_state.chan_last_ms > 0 && now - out_state.chan_last_ms > ADSB_CHAN_TIMEOUT_MS) {
// haven't gotten a heartbeat health status packet in a while, assume hardware failure
// TODO: reset out_state.chan
out_state.chan = -1;
gcs().send_text(MAV_SEVERITY_ERROR, "ADSB: Transceiver heartbeat timed out");
} else if (out_state.chan < MAVLINK_COMM_NUM_BUFFERS) {
mavlink_channel_t chan = (mavlink_channel_t)(MAVLINK_COMM_0 + out_state.chan);
if (now - out_state.last_config_ms >= 5000 && HAVE_PAYLOAD_SPACE(chan, UAVIONIX_ADSB_OUT_CFG)) {
out_state.last_config_ms = now;
send_configure(chan);
} // last_config_ms
// send dynamic data to transceiver at 5Hz
if (now - out_state.last_report_ms >= 200 && HAVE_PAYLOAD_SPACE(chan, UAVIONIX_ADSB_OUT_DYNAMIC)) {
out_state.last_report_ms = now;
send_dynamic_out(chan);
} // last_report_ms
} // chan_last_ms
}
