mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-10-21 01:13:35 +08:00
[navigator] Tone Down traffic warnings
- add unit tests for adsb conflict detection - move adsb conflict detection to lib/adsb and adsb conflict class - use containers/Array.hpp for buffer array - expand fake_traffic
This commit is contained in:
Mohamad Akkawi
committed by
Beat Küng
Beat Küng
parent
89548e4f9e
commit
9de52bb5ec
@@ -0,0 +1,501 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2023 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
|
||||
|
||||
|
||||
#include "AdsbConflict.h"
|
||||
#include "geo/geo.h"
|
||||
|
||||
#include <uORB/topics/transponder_report.h>
|
||||
|
||||
#include <float.h>
|
||||
|
||||
|
||||
void AdsbConflict::detect_traffic_conflict(double lat_now, double lon_now, float alt_now, float vx_now, float vy_now,
|
||||
float vz_now)
|
||||
|
||||
{
|
||||
|
||||
float d_hor, d_vert;
|
||||
get_distance_to_point_global_wgs84(lat_now, lon_now, alt_now,
|
||||
_transponder_report.lat, _transponder_report.lon, _transponder_report.altitude, &d_hor, &d_vert);
|
||||
|
||||
const float xyz_traffic_speed = sqrtf(_transponder_report.hor_velocity * _transponder_report.hor_velocity +
|
||||
_transponder_report.ver_velocity * _transponder_report.ver_velocity);
|
||||
|
||||
const float hor_uav_speed = sqrtf(vx_now * vx_now + vy_now * vy_now);
|
||||
const float xyz_uav_speed = sqrtf(hor_uav_speed * hor_uav_speed + vz_now * vz_now);
|
||||
|
||||
//assume always pointing at each other
|
||||
const float relative_uav_traffic_speed = xyz_traffic_speed + xyz_uav_speed;
|
||||
|
||||
|
||||
// Predict until the vehicle would have passed this system at its current speed
|
||||
const float prediction_distance = d_hor + TRAFFIC_TO_UAV_DISTANCE_EXTENSION;
|
||||
|
||||
double end_lat, end_lon;
|
||||
waypoint_from_heading_and_distance(_transponder_report.lat, _transponder_report.lon,
|
||||
_transponder_report.heading, prediction_distance, &end_lat, &end_lon);
|
||||
|
||||
|
||||
const bool cs_distance_conflict_threshold = (!get_distance_to_line(_crosstrack_error, lat_now,
|
||||
lon_now, _transponder_report.lat,
|
||||
_transponder_report.lon, end_lat,
|
||||
end_lon))
|
||||
&& (!_crosstrack_error.past_end)
|
||||
&& (fabsf(_crosstrack_error.distance) < _conflict_detection_params.crosstrack_separation);
|
||||
|
||||
const bool _crosstrack_separation_check = (fabsf(alt_now - _transponder_report.altitude) <
|
||||
_conflict_detection_params.crosstrack_separation);
|
||||
|
||||
bool collision_time_check = false;
|
||||
|
||||
const float d_xyz = sqrtf(d_hor * d_hor + d_vert * d_vert);
|
||||
|
||||
if (relative_uav_traffic_speed > FLT_EPSILON) {
|
||||
const float time_to_collsion = d_xyz / relative_uav_traffic_speed;
|
||||
collision_time_check = (time_to_collsion < _conflict_detection_params.collision_time_threshold);
|
||||
}
|
||||
|
||||
_conflict_detected = (cs_distance_conflict_threshold && _crosstrack_separation_check
|
||||
&& collision_time_check);
|
||||
}
|
||||
|
||||
int AdsbConflict::find_icao_address_in_conflict_list(uint32_t icao_address)
|
||||
{
|
||||
|
||||
for (uint8_t i = 0; i < _traffic_buffer.icao_address.size(); i++) {
|
||||
if (_traffic_buffer.icao_address[i] == icao_address) {
|
||||
return i;
|
||||
}
|
||||
}
|
||||
|
||||
return -1;
|
||||
}
|
||||
|
||||
void AdsbConflict::remove_icao_address_from_conflict_list(int traffic_index)
|
||||
{
|
||||
_traffic_buffer.icao_address.remove(traffic_index);
|
||||
_traffic_buffer.timestamp.remove(traffic_index);
|
||||
}
|
||||
|
||||
void AdsbConflict::add_icao_address_from_conflict_list(uint32_t icao_address)
|
||||
{
|
||||
_traffic_buffer.timestamp.push_back(hrt_absolute_time());
|
||||
_traffic_buffer.icao_address.push_back(icao_address);
|
||||
}
|
||||
|
||||
void AdsbConflict::get_traffic_state()
|
||||
{
|
||||
|
||||
const int traffic_index = find_icao_address_in_conflict_list(_transponder_report.icao_address);
|
||||
|
||||
const bool old_conflict = (traffic_index >= 0);
|
||||
const bool new_traffic = (traffic_index < 0);
|
||||
const bool _traffic_buffer_full = (_traffic_buffer.icao_address.size() >= NAVIGATOR_MAX_TRAFFIC);
|
||||
|
||||
bool old_conflict_warning_expired = false;
|
||||
|
||||
if (old_conflict && _conflict_detected) {
|
||||
old_conflict_warning_expired = (hrt_elapsed_time(&_traffic_buffer.timestamp[traffic_index]) > CONFLICT_WARNING_TIMEOUT);
|
||||
|
||||
}
|
||||
|
||||
if (new_traffic && _conflict_detected && !_traffic_buffer_full) {
|
||||
add_icao_address_from_conflict_list(_transponder_report.icao_address);
|
||||
_traffic_state = TRAFFIC_STATE::ADD_CONFLICT;
|
||||
|
||||
} else if (new_traffic && _conflict_detected && _traffic_buffer_full) {
|
||||
_traffic_state = TRAFFIC_STATE::BUFFER_FULL;
|
||||
|
||||
} else if (old_conflict && _conflict_detected
|
||||
&& old_conflict_warning_expired) {
|
||||
_traffic_buffer.timestamp[traffic_index] = hrt_absolute_time();
|
||||
_traffic_state = TRAFFIC_STATE::REMIND_CONFLICT;
|
||||
|
||||
} else if (old_conflict && !_conflict_detected) {
|
||||
remove_icao_address_from_conflict_list(traffic_index);
|
||||
_traffic_state = TRAFFIC_STATE::REMOVE_OLD_CONFLICT;
|
||||
|
||||
} else {
|
||||
_traffic_state = TRAFFIC_STATE::NO_CONFLICT;
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
|
||||
bool AdsbConflict::handle_traffic_conflict()
|
||||
{
|
||||
|
||||
get_traffic_state();
|
||||
|
||||
char uas_id[UTM_GUID_MSG_LENGTH]; //GUID of incoming UTM messages
|
||||
|
||||
//convert UAS_id byte array to char array for User Warning
|
||||
for (int i = 0; i < 5; i++) {
|
||||
snprintf(&uas_id[i * 2], sizeof(uas_id) - i * 2, "%02x", _transponder_report.uas_id[PX4_GUID_BYTE_LENGTH - 5 + i]);
|
||||
}
|
||||
|
||||
uint64_t uas_id_int = 0;
|
||||
|
||||
for (int i = 0; i < 8; i++) {
|
||||
uas_id_int |= (uint64_t)(_transponder_report.uas_id[PX4_GUID_BYTE_LENGTH - i - 1]) << (i * 8);
|
||||
}
|
||||
|
||||
|
||||
bool take_action = false;
|
||||
|
||||
switch (_traffic_state) {
|
||||
|
||||
case TRAFFIC_STATE::ADD_CONFLICT:
|
||||
case TRAFFIC_STATE::REMIND_CONFLICT: {
|
||||
|
||||
take_action = send_traffic_warning(math::degrees(_transponder_report.heading) + 180,
|
||||
(int)fabsf(_crosstrack_error.distance), _transponder_report.flags, uas_id,
|
||||
_transponder_report.callsign,
|
||||
uas_id_int);
|
||||
|
||||
}
|
||||
break;
|
||||
|
||||
case TRAFFIC_STATE::REMOVE_OLD_CONFLICT: {
|
||||
|
||||
mavlink_log_critical(&_mavlink_log_pub, "TRAFFIC UPDATE: %s is no longer in conflict!",
|
||||
_transponder_report.flags & transponder_report_s::PX4_ADSB_FLAGS_VALID_CALLSIGN ?
|
||||
_transponder_report.callsign : uas_id);
|
||||
|
||||
events::send<uint64_t>(events::ID("navigator_traffic_resolved"), events::Log::Critical, "Traffic Conflict Resolved",
|
||||
uas_id_int);
|
||||
|
||||
}
|
||||
break;
|
||||
|
||||
case TRAFFIC_STATE::BUFFER_FULL: {
|
||||
|
||||
if (_traffic_state_previous != TRAFFIC_STATE::BUFFER_FULL) {
|
||||
PX4_WARN("Too much traffic! Showing all messages from now on");
|
||||
}
|
||||
|
||||
//stop buffering incoming conflicts
|
||||
take_action = send_traffic_warning(math::degrees(_transponder_report.heading) + 180,
|
||||
(int)fabsf(_crosstrack_error.distance), _transponder_report.flags, uas_id,
|
||||
_transponder_report.callsign,
|
||||
uas_id_int);
|
||||
|
||||
}
|
||||
break;
|
||||
|
||||
case TRAFFIC_STATE::NO_CONFLICT: {
|
||||
|
||||
}
|
||||
break;
|
||||
}
|
||||
|
||||
|
||||
_traffic_state_previous = _traffic_state;
|
||||
|
||||
return take_action;
|
||||
|
||||
}
|
||||
|
||||
void AdsbConflict::set_conflict_detection_params(float crosstrack_separation, float vertical_separation,
|
||||
int collision_time_threshold, uint8_t traffic_avoidance_mode)
|
||||
{
|
||||
|
||||
_conflict_detection_params.crosstrack_separation = crosstrack_separation;
|
||||
_conflict_detection_params.vertical_separation = vertical_separation;
|
||||
_conflict_detection_params.collision_time_threshold = collision_time_threshold;
|
||||
_conflict_detection_params.traffic_avoidance_mode = traffic_avoidance_mode;
|
||||
|
||||
}
|
||||
|
||||
|
||||
bool AdsbConflict::send_traffic_warning(int traffic_direction, int traffic_seperation, uint16_t tr_flags,
|
||||
char uas_id[UTM_GUID_MSG_LENGTH], char tr_callsign[UTM_CALLSIGN_LENGTH], uint64_t uas_id_int)
|
||||
{
|
||||
|
||||
switch (_conflict_detection_params.traffic_avoidance_mode) {
|
||||
|
||||
case 0: {
|
||||
PX4_WARN("TRAFFIC %s! dst %d, hdg %d",
|
||||
tr_flags & transponder_report_s::PX4_ADSB_FLAGS_VALID_CALLSIGN ? tr_callsign : uas_id,
|
||||
traffic_seperation,
|
||||
traffic_direction);
|
||||
break;
|
||||
}
|
||||
|
||||
case 1: {
|
||||
mavlink_log_critical(&_mavlink_log_pub, "Warning TRAFFIC %s! dst %d, hdg %d\t",
|
||||
tr_flags & transponder_report_s::PX4_ADSB_FLAGS_VALID_CALLSIGN ? tr_callsign : uas_id,
|
||||
traffic_seperation,
|
||||
traffic_direction);
|
||||
/* EVENT
|
||||
* @description
|
||||
* - ID: {1}
|
||||
* - Distance: {2m}
|
||||
* - Direction: {3} degrees
|
||||
*/
|
||||
events::send<uint64_t, int32_t, int16_t>(events::ID("navigator_traffic"), events::Log::Critical, "Traffic alert",
|
||||
uas_id_int, traffic_seperation, traffic_direction);
|
||||
break;
|
||||
}
|
||||
|
||||
case 2: {
|
||||
mavlink_log_critical(&_mavlink_log_pub, "TRAFFIC: %s Returning home! dst %d, hdg %d\t",
|
||||
tr_flags & transponder_report_s::PX4_ADSB_FLAGS_VALID_CALLSIGN ? tr_callsign : uas_id,
|
||||
traffic_seperation,
|
||||
traffic_direction);
|
||||
/* EVENT
|
||||
* @description
|
||||
* - ID: {1}
|
||||
* - Distance: {2m}
|
||||
* - Direction: {3} degrees
|
||||
*/
|
||||
events::send<uint64_t, int32_t, int16_t>(events::ID("navigator_traffic_rtl"), events::Log::Critical,
|
||||
"Traffic alert, returning home",
|
||||
uas_id_int, traffic_seperation, traffic_direction);
|
||||
|
||||
return true;
|
||||
|
||||
break;
|
||||
}
|
||||
|
||||
case 3: {
|
||||
mavlink_log_critical(&_mavlink_log_pub, "TRAFFIC: %s Landing! dst %d, hdg % d\t",
|
||||
tr_flags & transponder_report_s::PX4_ADSB_FLAGS_VALID_CALLSIGN ? tr_callsign : uas_id,
|
||||
traffic_seperation,
|
||||
traffic_direction);
|
||||
/* EVENT
|
||||
* @description
|
||||
* - ID: {1}
|
||||
* - Distance: {2m}
|
||||
* - Direction: {3} degrees
|
||||
*/
|
||||
events::send<uint64_t, int32_t, int16_t>(events::ID("navigator_traffic_land"), events::Log::Critical,
|
||||
"Traffic alert, landing",
|
||||
uas_id_int, traffic_seperation, traffic_direction);
|
||||
|
||||
return true;
|
||||
|
||||
break;
|
||||
|
||||
}
|
||||
|
||||
case 4: {
|
||||
mavlink_log_critical(&_mavlink_log_pub, "TRAFFIC: %s Holding position! dst %d, hdg %d\t",
|
||||
tr_flags & transponder_report_s::PX4_ADSB_FLAGS_VALID_CALLSIGN ? tr_callsign : uas_id,
|
||||
traffic_seperation,
|
||||
traffic_direction);
|
||||
/* EVENT
|
||||
* @description
|
||||
* - ID: {1}
|
||||
* - Distance: {2m}
|
||||
* - Direction: {3} degrees
|
||||
*/
|
||||
events::send<uint64_t, int32_t, int16_t>(events::ID("navigator_traffic_hold"), events::Log::Critical,
|
||||
"Traffic alert, holding position",
|
||||
uas_id_int, traffic_seperation, traffic_direction);
|
||||
|
||||
return true;
|
||||
|
||||
break;
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
return false;
|
||||
|
||||
}
|
||||
|
||||
void AdsbConflict::fake_traffic(const char *callsign, float distance, float direction, float traffic_heading,
|
||||
float altitude_diff, float hor_velocity, float ver_velocity, int emitter_type, uint32_t icao_address, double lat_uav,
|
||||
double lon_uav,
|
||||
float &alt_uav)
|
||||
{
|
||||
double lat{0.0};
|
||||
double lon{0.0};
|
||||
|
||||
waypoint_from_heading_and_distance(lat_uav, lon_uav, direction, distance, &lat,
|
||||
&lon);
|
||||
float alt = alt_uav + altitude_diff;
|
||||
|
||||
tr.timestamp = hrt_absolute_time();
|
||||
tr.icao_address = icao_address;
|
||||
tr.lat = lat; // Latitude, expressed as degrees
|
||||
tr.lon = lon; // Longitude, expressed as degrees
|
||||
tr.altitude_type = 0;
|
||||
tr.altitude = alt;
|
||||
tr.heading = traffic_heading; //-atan2(vel_e, vel_n); // Course over ground in radians
|
||||
tr.hor_velocity = hor_velocity; //sqrtf(vel_e * vel_e + vel_n * vel_n); // The horizontal velocity in m/s
|
||||
tr.ver_velocity = ver_velocity; //-vel_d; // The vertical velocity in m/s, positive is up
|
||||
strncpy(&tr.callsign[0], callsign, sizeof(tr.callsign) - 1);
|
||||
tr.callsign[sizeof(tr.callsign) - 1] = 0;
|
||||
tr.emitter_type = emitter_type; // Type from ADSB_EMITTER_TYPE enum
|
||||
tr.tslc = 2; // Time since last communication in seconds
|
||||
tr.flags = transponder_report_s::PX4_ADSB_FLAGS_VALID_COORDS | transponder_report_s::PX4_ADSB_FLAGS_VALID_HEADING |
|
||||
transponder_report_s::PX4_ADSB_FLAGS_VALID_VELOCITY |
|
||||
transponder_report_s::PX4_ADSB_FLAGS_VALID_ALTITUDE |
|
||||
(transponder_report_s::ADSB_EMITTER_TYPE_UAV & emitter_type ? 0 :
|
||||
transponder_report_s::PX4_ADSB_FLAGS_VALID_CALLSIGN); // Flags to indicate various statuses including valid data fields
|
||||
tr.squawk = 6667;
|
||||
|
||||
#ifndef BOARD_HAS_NO_UUID
|
||||
px4_guid_t px4_guid;
|
||||
board_get_px4_guid(px4_guid);
|
||||
memcpy(tr.uas_id, px4_guid, sizeof(px4_guid_t)); //simulate own GUID
|
||||
#else
|
||||
|
||||
for (int i = 0; i < PX4_GUID_BYTE_LENGTH ; i++) {
|
||||
tr.uas_id[i] = 0xe0 + i; //simulate GUID
|
||||
}
|
||||
|
||||
#endif /* BOARD_HAS_NO_UUID */
|
||||
|
||||
orb_publish(ORB_ID(transponder_report), fake_traffic_report_publisher, &tr);
|
||||
|
||||
}
|
||||
|
||||
|
||||
void AdsbConflict::run_fake_traffic(double &lat_uav, double &lon_uav,
|
||||
float &alt_uav)
|
||||
{
|
||||
|
||||
//first conflict
|
||||
fake_traffic("LX001", 5, 1.0f, 0.0f, 0.0f, 90000.0f, 90000.0f,
|
||||
transponder_report_s::ADSB_EMITTER_TYPE_LIGHT, 1, lat_uav, lon_uav,
|
||||
alt_uav);
|
||||
|
||||
//spam
|
||||
fake_traffic("LX002", 5, 1.0f, 0.0f, 0.0f, 90000.0f, 90000.0f,
|
||||
transponder_report_s::ADSB_EMITTER_TYPE_LIGHT, 2, lat_uav, lon_uav,
|
||||
alt_uav);
|
||||
fake_traffic("LX0002", 5, 1.0f, 0.0f, 0.0f, 90000.0f, 90000.0f,
|
||||
transponder_report_s::ADSB_EMITTER_TYPE_LIGHT, 2, lat_uav, lon_uav,
|
||||
alt_uav);
|
||||
fake_traffic("LX0002", 5, 1.0f, 0.0f, 0.0f, 90000.0f, 90000.0f,
|
||||
transponder_report_s::ADSB_EMITTER_TYPE_LIGHT, 2, lat_uav, lon_uav,
|
||||
alt_uav);
|
||||
|
||||
//stop spamming
|
||||
|
||||
fake_traffic("LX003", 5, 1.0f, 0.0f, 0.0f, 90000.0f, 90000.0f,
|
||||
transponder_report_s::ADSB_EMITTER_TYPE_LIGHT, 3, lat_uav, lon_uav,
|
||||
alt_uav);
|
||||
|
||||
fake_traffic("LX004", 5, 1.0f, 0.0f, 0.0f, 90000.0f, 90000.0f,
|
||||
transponder_report_s::ADSB_EMITTER_TYPE_LIGHT, 4, lat_uav, lon_uav,
|
||||
alt_uav);
|
||||
|
||||
fake_traffic("LX005", 5, 1.0f, 0.0f, 0.0f, 90000.0f, 90000.0f,
|
||||
transponder_report_s::ADSB_EMITTER_TYPE_LIGHT, 5, lat_uav, lon_uav,
|
||||
alt_uav);
|
||||
|
||||
fake_traffic("LX006", 5, 1.0f, 0.0f, 0.0f, 90000.0f, 90000.0f,
|
||||
transponder_report_s::ADSB_EMITTER_TYPE_LIGHT, 6, lat_uav, lon_uav,
|
||||
alt_uav);
|
||||
|
||||
fake_traffic("LX007", 5, 1.0f, 0.0f, 0.0f, 90000.0f, 90000.0f,
|
||||
transponder_report_s::ADSB_EMITTER_TYPE_LIGHT, 7, lat_uav, lon_uav,
|
||||
alt_uav);
|
||||
|
||||
|
||||
fake_traffic("LX008", 5, 1.0f, 0.0f, 0.0f, 90000.0f, 90000.0f,
|
||||
transponder_report_s::ADSB_EMITTER_TYPE_LIGHT, 8, lat_uav, lon_uav,
|
||||
alt_uav);
|
||||
|
||||
fake_traffic("LX009", 5, 1.0f, 0.0f, 0.0f, 90000.0f, 90000.0f,
|
||||
transponder_report_s::ADSB_EMITTER_TYPE_LIGHT, 9, lat_uav, lon_uav,
|
||||
alt_uav);
|
||||
|
||||
fake_traffic("LX010", 5, 1.0f, 0.0f, 0.0f, 90000.0f, 90000.0f,
|
||||
transponder_report_s::ADSB_EMITTER_TYPE_LIGHT, 10, lat_uav, lon_uav,
|
||||
alt_uav);
|
||||
|
||||
//buffer full
|
||||
fake_traffic("LX011", 5, 1.0f, 0.0f, 0.0f, 90000.0f, 90000.0f,
|
||||
transponder_report_s::ADSB_EMITTER_TYPE_LIGHT, 11, lat_uav, lon_uav,
|
||||
alt_uav);
|
||||
|
||||
fake_traffic("LX012", 5, 1.0f, 0.0f, 0.0f, 90000.0f, 90000.0f,
|
||||
transponder_report_s::ADSB_EMITTER_TYPE_LIGHT, 12, lat_uav, lon_uav,
|
||||
alt_uav);
|
||||
|
||||
fake_traffic("LX013", 5, 1.0f, 0.0f, 0.0f, 90000.0f, 90000.0f,
|
||||
transponder_report_s::ADSB_EMITTER_TYPE_LIGHT, 13, lat_uav, lon_uav,
|
||||
alt_uav);
|
||||
|
||||
|
||||
//end conflict
|
||||
fake_traffic("LX001", 5000, 1.0f, 0.0f, 0.0f, 90000.0f, 90000.0f,
|
||||
transponder_report_s::ADSB_EMITTER_TYPE_LIGHT, 1, lat_uav, lon_uav,
|
||||
alt_uav);
|
||||
fake_traffic("LX002", 5000, 1.0f, 0.0f, 0.0f, 90000.0f, 90000.0f,
|
||||
transponder_report_s::ADSB_EMITTER_TYPE_LIGHT, 2, lat_uav, lon_uav,
|
||||
alt_uav);
|
||||
|
||||
fake_traffic("LX003", 5000, 1.0f, 0.0f, 0.0f, 90000.0f, 90000.0f,
|
||||
transponder_report_s::ADSB_EMITTER_TYPE_LIGHT, 3, lat_uav, lon_uav,
|
||||
alt_uav);
|
||||
|
||||
fake_traffic("LX004", 5000, 1.0f, 0.0f, 0.0f, 90000.0f, 90000.0f,
|
||||
transponder_report_s::ADSB_EMITTER_TYPE_LIGHT, 4, lat_uav, lon_uav,
|
||||
alt_uav);
|
||||
|
||||
|
||||
//new conflicts with space in buffer
|
||||
fake_traffic("LX013", 5, 1.0f, 0.0f, 0.0f, 90000.0f, 90000.0f,
|
||||
transponder_report_s::ADSB_EMITTER_TYPE_LIGHT, 13, lat_uav, lon_uav,
|
||||
alt_uav);
|
||||
|
||||
fake_traffic("LX013", 5, 1.0f, 0.0f, 0.0f, 90000.0f, 90000.0f,
|
||||
transponder_report_s::ADSB_EMITTER_TYPE_LIGHT, 13, lat_uav, lon_uav,
|
||||
alt_uav);
|
||||
|
||||
fake_traffic("LX014", 5, 1.0f, 0.0f, 0.0f, 90000.0f, 90000.0f,
|
||||
transponder_report_s::ADSB_EMITTER_TYPE_LIGHT, 14, lat_uav, lon_uav,
|
||||
alt_uav);
|
||||
|
||||
fake_traffic("LX014", 5, 1.0f, 0.0f, 0.0f, 90000.0f, 90000.0f,
|
||||
transponder_report_s::ADSB_EMITTER_TYPE_LIGHT, 14, lat_uav, lon_uav,
|
||||
alt_uav);
|
||||
|
||||
fake_traffic("LX015", 5, 1.0f, 0.0f, 0.0f, 90000.0f, 90000.0f,
|
||||
transponder_report_s::ADSB_EMITTER_TYPE_LIGHT, 15, lat_uav, lon_uav,
|
||||
alt_uav);
|
||||
|
||||
for (size_t i = 0; i < _traffic_buffer.icao_address.size(); i++) {
|
||||
PX4_INFO("%u ", static_cast<unsigned int>(_traffic_buffer.icao_address[i]));
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,158 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2012-2023 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <stdbool.h>
|
||||
#include <stdint.h>
|
||||
|
||||
#include <lib/geo/geo.h>
|
||||
|
||||
#include <drivers/drv_hrt.h>
|
||||
#include <uORB/Publication.hpp>
|
||||
#include <uORB/Subscription.hpp>
|
||||
#include <uORB/topics/transponder_report.h>
|
||||
#include <uORB/topics/vehicle_command.h>
|
||||
|
||||
#include <systemlib/mavlink_log.h>
|
||||
#include <px4_platform_common/events.h>
|
||||
|
||||
#include <px4_platform_common/board_common.h>
|
||||
|
||||
#include <containers/Array.hpp>
|
||||
|
||||
using namespace time_literals;
|
||||
|
||||
static constexpr uint8_t NAVIGATOR_MAX_TRAFFIC{10};
|
||||
|
||||
static constexpr uint8_t UTM_GUID_MSG_LENGTH{11};
|
||||
|
||||
static constexpr uint8_t UTM_CALLSIGN_LENGTH{9};
|
||||
|
||||
static constexpr uint64_t CONFLICT_WARNING_TIMEOUT{60_s};
|
||||
|
||||
static constexpr float TRAFFIC_TO_UAV_DISTANCE_EXTENSION{1000.0f};
|
||||
|
||||
|
||||
struct traffic_data_s {
|
||||
double lat_traffic;
|
||||
double lon_traffic;
|
||||
float alt_traffic;
|
||||
float heading_traffic;
|
||||
float vxy_traffic;
|
||||
float vz_traffic;
|
||||
bool in_conflict;
|
||||
};
|
||||
|
||||
struct traffic_buffer_s {
|
||||
px4::Array<uint32_t, NAVIGATOR_MAX_TRAFFIC> icao_address {};
|
||||
px4::Array<hrt_abstime, NAVIGATOR_MAX_TRAFFIC> timestamp {};
|
||||
};
|
||||
|
||||
struct conflict_detection_params_s {
|
||||
float crosstrack_separation;
|
||||
float vertical_separation;
|
||||
int collision_time_threshold;
|
||||
uint8_t traffic_avoidance_mode;
|
||||
};
|
||||
|
||||
enum class TRAFFIC_STATE {
|
||||
NO_CONFLICT = 0,
|
||||
ADD_CONFLICT = 1,
|
||||
REMIND_CONFLICT = 2,
|
||||
REMOVE_OLD_CONFLICT = 3,
|
||||
BUFFER_FULL = 4
|
||||
};
|
||||
|
||||
|
||||
class AdsbConflict
|
||||
{
|
||||
public:
|
||||
AdsbConflict() = default;
|
||||
~AdsbConflict() = default;
|
||||
|
||||
void detect_traffic_conflict(double lat_now, double lon_now, float alt_now, float vx_now, float vy_now, float vz_now);
|
||||
|
||||
int find_icao_address_in_conflict_list(uint32_t icao_address);
|
||||
|
||||
void remove_icao_address_from_conflict_list(int traffic_index);
|
||||
|
||||
void add_icao_address_from_conflict_list(uint32_t icao_address);
|
||||
|
||||
void get_traffic_state();
|
||||
|
||||
void set_conflict_detection_params(float crosstrack_separation, float vertical_separation,
|
||||
int collision_time_threshold, uint8_t traffic_avoidance_mode);
|
||||
|
||||
|
||||
bool send_traffic_warning(int traffic_direction, int traffic_seperation, uint16_t tr_flags,
|
||||
char uas_id[UTM_GUID_MSG_LENGTH], char tr_callsign[UTM_CALLSIGN_LENGTH], uint64_t uas_id_int);
|
||||
|
||||
transponder_report_s _transponder_report{};
|
||||
|
||||
bool handle_traffic_conflict();
|
||||
|
||||
void fake_traffic(const char *const callsign, float distance, float direction, float traffic_heading,
|
||||
float altitude_diff,
|
||||
float hor_velocity, float ver_velocity, int emitter_type, uint32_t icao_address, double lat_uav, double lon_uav,
|
||||
float &alt_uav);
|
||||
|
||||
void run_fake_traffic(double &lat_uav, double &lon_uav,
|
||||
float &alt_uav);
|
||||
|
||||
bool _conflict_detected{false};
|
||||
|
||||
TRAFFIC_STATE _traffic_state{TRAFFIC_STATE::NO_CONFLICT};
|
||||
|
||||
conflict_detection_params_s _conflict_detection_params{};
|
||||
|
||||
|
||||
protected:
|
||||
traffic_buffer_s _traffic_buffer;
|
||||
|
||||
private:
|
||||
|
||||
orb_advert_t _mavlink_log_pub{nullptr};
|
||||
|
||||
crosstrack_error_s _crosstrack_error{};
|
||||
|
||||
|
||||
transponder_report_s tr{};
|
||||
|
||||
orb_advert_t fake_traffic_report_publisher = orb_advertise_queue(ORB_ID(transponder_report), &tr, (unsigned int)20);
|
||||
|
||||
TRAFFIC_STATE _traffic_state_previous{TRAFFIC_STATE::NO_CONFLICT};
|
||||
|
||||
};
|
||||
@@ -0,0 +1,288 @@
|
||||
#include <gtest/gtest.h>
|
||||
|
||||
#include "AdsbConflict.h"
|
||||
|
||||
#include "AdsbConflictTest.h"
|
||||
|
||||
|
||||
class TestAdsbConflict : public AdsbConflict
|
||||
{
|
||||
public:
|
||||
TestAdsbConflict() : AdsbConflict() {}
|
||||
~TestAdsbConflict() = default;
|
||||
|
||||
void set_traffic_buffer(const traffic_buffer_s &traffic_buffer)
|
||||
{
|
||||
_traffic_buffer = traffic_buffer;
|
||||
}
|
||||
|
||||
void set_conflict(bool &conflict_detected)
|
||||
{
|
||||
_conflict_detected = conflict_detected;
|
||||
}
|
||||
};
|
||||
|
||||
|
||||
TEST_F(AdsbConflictTest, detectTrafficConflict)
|
||||
{
|
||||
|
||||
|
||||
int collision_time_threshold = 60;
|
||||
|
||||
float crosstrack_separation = 500.0f;
|
||||
float vertical_separation = 500.0f;
|
||||
|
||||
double lat_now = 32.617013;
|
||||
double lon_now = -96.490564;
|
||||
float alt_now = 1000.0f;
|
||||
|
||||
float vx_now = 0.0f;
|
||||
float vy_now = 0.0f;
|
||||
float vz_now = 0.0f;
|
||||
|
||||
uint32_t traffic_dataset_size = sizeof(traffic_dataset) / sizeof(traffic_dataset[0]);
|
||||
|
||||
|
||||
TestAdsbConflict adsb_conflict;
|
||||
|
||||
adsb_conflict.set_conflict_detection_params(crosstrack_separation, vertical_separation, collision_time_threshold, 1);
|
||||
|
||||
for (uint32_t i = 0; i < traffic_dataset_size; i++) {
|
||||
|
||||
|
||||
printf("---------------%d--------------\n", i);
|
||||
|
||||
struct traffic_data_s traffic = traffic_dataset[i];
|
||||
|
||||
|
||||
adsb_conflict._transponder_report.lat = traffic.lat_traffic;
|
||||
adsb_conflict._transponder_report.lon = traffic.lon_traffic;
|
||||
adsb_conflict._transponder_report.altitude = traffic.alt_traffic;
|
||||
adsb_conflict._transponder_report.heading = traffic.heading_traffic;
|
||||
adsb_conflict._transponder_report.hor_velocity = traffic.vxy_traffic;
|
||||
adsb_conflict._transponder_report.ver_velocity = traffic.vz_traffic;
|
||||
|
||||
adsb_conflict.detect_traffic_conflict(lat_now, lon_now, alt_now, vx_now, vy_now, vz_now);
|
||||
|
||||
printf("conflict_detected %d \n", adsb_conflict._conflict_detected);
|
||||
|
||||
printf("------------------------------\n");
|
||||
printf("------------------------------\n \n");
|
||||
|
||||
EXPECT_TRUE(adsb_conflict._conflict_detected == traffic.in_conflict);
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
|
||||
TEST_F(AdsbConflictTest, trafficAlerts)
|
||||
{
|
||||
|
||||
struct traffic_buffer_s used_buffer;
|
||||
used_buffer.icao_address.push_back(2345);
|
||||
used_buffer.icao_address.push_back(1234);
|
||||
used_buffer.icao_address.push_back(1897);
|
||||
used_buffer.icao_address.push_back(0567);
|
||||
used_buffer.icao_address.push_back(8685);
|
||||
used_buffer.icao_address.push_back(5000);
|
||||
|
||||
used_buffer.timestamp.push_back(3_s);
|
||||
used_buffer.timestamp.push_back(800_s);
|
||||
used_buffer.timestamp.push_back(100_s);
|
||||
used_buffer.timestamp.push_back(20000_s);
|
||||
used_buffer.timestamp.push_back(6000_s);
|
||||
used_buffer.timestamp.push_back(6587_s);
|
||||
|
||||
struct traffic_buffer_s full_buffer;
|
||||
full_buffer.icao_address.push_back(2345);
|
||||
full_buffer.icao_address.push_back(1234);
|
||||
full_buffer.icao_address.push_back(1897);
|
||||
full_buffer.icao_address.push_back(0567);
|
||||
full_buffer.icao_address.push_back(8685);
|
||||
full_buffer.icao_address.push_back(5000);
|
||||
full_buffer.icao_address.push_back(0000);
|
||||
full_buffer.icao_address.push_back(2);
|
||||
full_buffer.icao_address.push_back(589742397);
|
||||
full_buffer.icao_address.push_back(99999);
|
||||
|
||||
full_buffer.timestamp.push_back(1_s);
|
||||
full_buffer.timestamp.push_back(800_s);
|
||||
full_buffer.timestamp.push_back(100_s);
|
||||
full_buffer.timestamp.push_back(20000_s);
|
||||
full_buffer.timestamp.push_back(6000_s);
|
||||
full_buffer.timestamp.push_back(19000_s);
|
||||
full_buffer.timestamp.push_back(5000_s);
|
||||
full_buffer.timestamp.push_back(2_s);
|
||||
full_buffer.timestamp.push_back(1000_s);
|
||||
full_buffer.timestamp.push_back(58943_s);
|
||||
|
||||
|
||||
|
||||
struct traffic_buffer_s empty_buffer = {};
|
||||
|
||||
|
||||
TestAdsbConflict adsb_conflict;
|
||||
|
||||
adsb_conflict.set_traffic_buffer(used_buffer);
|
||||
|
||||
bool conflict_detected = false;
|
||||
adsb_conflict.set_conflict(conflict_detected);
|
||||
adsb_conflict._transponder_report.icao_address = 00001;
|
||||
adsb_conflict.get_traffic_state();
|
||||
|
||||
printf("adsb_conflict._traffic_state %d \n", adsb_conflict._traffic_state);
|
||||
EXPECT_TRUE(adsb_conflict._traffic_state == TRAFFIC_STATE::NO_CONFLICT);
|
||||
|
||||
conflict_detected = true;
|
||||
adsb_conflict.set_conflict(conflict_detected);
|
||||
adsb_conflict._transponder_report.icao_address = 9876;
|
||||
adsb_conflict.get_traffic_state();
|
||||
|
||||
printf("adsb_conflict._traffic_state %d \n", adsb_conflict._traffic_state);
|
||||
EXPECT_TRUE(adsb_conflict._traffic_state == TRAFFIC_STATE::ADD_CONFLICT);
|
||||
|
||||
adsb_conflict.set_traffic_buffer(empty_buffer);
|
||||
|
||||
conflict_detected = true;
|
||||
adsb_conflict.set_conflict(conflict_detected);
|
||||
adsb_conflict._transponder_report.icao_address = 9876;
|
||||
adsb_conflict.get_traffic_state();
|
||||
|
||||
printf("adsb_conflict._traffic_state %d \n", adsb_conflict._traffic_state);
|
||||
EXPECT_TRUE(adsb_conflict._traffic_state == TRAFFIC_STATE::ADD_CONFLICT);
|
||||
|
||||
adsb_conflict.set_traffic_buffer(full_buffer);
|
||||
|
||||
conflict_detected = true;
|
||||
adsb_conflict.set_conflict(conflict_detected);
|
||||
adsb_conflict._transponder_report.icao_address = 7777;
|
||||
adsb_conflict.get_traffic_state();
|
||||
|
||||
printf("adsb_conflict._traffic_state %d \n", adsb_conflict._traffic_state);
|
||||
EXPECT_TRUE(adsb_conflict._traffic_state == TRAFFIC_STATE::BUFFER_FULL);
|
||||
|
||||
conflict_detected = false;
|
||||
adsb_conflict.set_conflict(conflict_detected);
|
||||
adsb_conflict._transponder_report.icao_address = 7777;
|
||||
adsb_conflict.get_traffic_state();
|
||||
|
||||
printf("adsb_conflict._traffic_state %d \n", adsb_conflict._traffic_state);
|
||||
EXPECT_TRUE(adsb_conflict._traffic_state == TRAFFIC_STATE::NO_CONFLICT);
|
||||
|
||||
conflict_detected = false;
|
||||
adsb_conflict.set_conflict(conflict_detected);
|
||||
adsb_conflict._transponder_report.icao_address = 8685;
|
||||
adsb_conflict.get_traffic_state();
|
||||
|
||||
printf("adsb_conflict._traffic_state %d \n", adsb_conflict._traffic_state);
|
||||
EXPECT_TRUE(adsb_conflict._traffic_state == TRAFFIC_STATE::REMOVE_OLD_CONFLICT);
|
||||
|
||||
adsb_conflict.set_traffic_buffer(used_buffer);
|
||||
|
||||
conflict_detected = false;
|
||||
adsb_conflict.set_conflict(conflict_detected);
|
||||
adsb_conflict._transponder_report.icao_address = 8685;
|
||||
adsb_conflict.get_traffic_state();
|
||||
|
||||
printf("adsb_conflict._traffic_state %d \n", adsb_conflict._traffic_state);
|
||||
EXPECT_TRUE(adsb_conflict._traffic_state == TRAFFIC_STATE::REMOVE_OLD_CONFLICT);
|
||||
|
||||
}
|
||||
|
||||
TEST_F(AdsbConflictTest, trafficReminder)
|
||||
{
|
||||
|
||||
struct traffic_buffer_s used_buffer;
|
||||
used_buffer.icao_address.push_back(2345);
|
||||
used_buffer.icao_address.push_back(1234);
|
||||
used_buffer.icao_address.push_back(1897);
|
||||
used_buffer.icao_address.push_back(0567);
|
||||
used_buffer.icao_address.push_back(8685);
|
||||
used_buffer.icao_address.push_back(5000);
|
||||
|
||||
used_buffer.timestamp.push_back(3_s);
|
||||
used_buffer.timestamp.push_back(800_s);
|
||||
used_buffer.timestamp.push_back(100_s);
|
||||
used_buffer.timestamp.push_back(20000_s);
|
||||
used_buffer.timestamp.push_back(6000_s);
|
||||
used_buffer.timestamp.push_back(6587_s);
|
||||
|
||||
struct traffic_buffer_s full_buffer;
|
||||
full_buffer.icao_address.push_back(2345);
|
||||
full_buffer.icao_address.push_back(1234);
|
||||
full_buffer.icao_address.push_back(1897);
|
||||
full_buffer.icao_address.push_back(0567);
|
||||
full_buffer.icao_address.push_back(8685);
|
||||
full_buffer.icao_address.push_back(5000);
|
||||
full_buffer.icao_address.push_back(0000);
|
||||
full_buffer.icao_address.push_back(2);
|
||||
full_buffer.icao_address.push_back(589742397);
|
||||
full_buffer.icao_address.push_back(99999);
|
||||
|
||||
full_buffer.timestamp.push_back(1_s);
|
||||
full_buffer.timestamp.push_back(800_s);
|
||||
full_buffer.timestamp.push_back(100_s);
|
||||
full_buffer.timestamp.push_back(20000_s);
|
||||
full_buffer.timestamp.push_back(6000_s);
|
||||
full_buffer.timestamp.push_back(19000_s);
|
||||
full_buffer.timestamp.push_back(5000_s);
|
||||
full_buffer.timestamp.push_back(2_s);
|
||||
full_buffer.timestamp.push_back(1000_s);
|
||||
full_buffer.timestamp.push_back(58943_s);
|
||||
|
||||
|
||||
TestAdsbConflict adsb_conflict;
|
||||
|
||||
adsb_conflict.set_traffic_buffer(used_buffer);
|
||||
|
||||
bool conflict_detected = true;
|
||||
adsb_conflict.set_conflict(conflict_detected);
|
||||
adsb_conflict._transponder_report.icao_address = 8685;
|
||||
adsb_conflict.get_traffic_state();
|
||||
|
||||
printf("adsb_conflict._traffic_state %d \n", adsb_conflict._traffic_state);
|
||||
EXPECT_TRUE(adsb_conflict._traffic_state == TRAFFIC_STATE::REMIND_CONFLICT);
|
||||
|
||||
conflict_detected = true;
|
||||
adsb_conflict.set_conflict(conflict_detected);
|
||||
adsb_conflict._transponder_report.icao_address = 8685;
|
||||
adsb_conflict.get_traffic_state();
|
||||
|
||||
printf("adsb_conflict._traffic_state %d \n", adsb_conflict._traffic_state);
|
||||
EXPECT_TRUE(adsb_conflict._traffic_state == TRAFFIC_STATE::NO_CONFLICT);
|
||||
|
||||
adsb_conflict.set_traffic_buffer(full_buffer);
|
||||
|
||||
conflict_detected = true;
|
||||
adsb_conflict.set_conflict(conflict_detected);
|
||||
adsb_conflict._transponder_report.icao_address = 8685;
|
||||
adsb_conflict.get_traffic_state();
|
||||
|
||||
printf("adsb_conflict._traffic_state %d \n", adsb_conflict._traffic_state);
|
||||
EXPECT_TRUE(adsb_conflict._traffic_state == TRAFFIC_STATE::REMIND_CONFLICT);
|
||||
|
||||
conflict_detected = true;
|
||||
adsb_conflict.set_conflict(conflict_detected);
|
||||
adsb_conflict._transponder_report.icao_address = 8685;
|
||||
adsb_conflict.get_traffic_state();
|
||||
|
||||
printf("adsb_conflict._traffic_state %d \n", adsb_conflict._traffic_state);
|
||||
EXPECT_TRUE(adsb_conflict._traffic_state == TRAFFIC_STATE::NO_CONFLICT);
|
||||
|
||||
conflict_detected = false;
|
||||
adsb_conflict.set_conflict(conflict_detected);
|
||||
adsb_conflict._transponder_report.icao_address = 8685;
|
||||
adsb_conflict.get_traffic_state();
|
||||
|
||||
printf("adsb_conflict._traffic_state %d \n", adsb_conflict._traffic_state);
|
||||
EXPECT_TRUE(adsb_conflict._traffic_state == TRAFFIC_STATE::REMOVE_OLD_CONFLICT);
|
||||
|
||||
conflict_detected = true;
|
||||
adsb_conflict.set_conflict(conflict_detected);
|
||||
adsb_conflict._transponder_report.icao_address = 7777;
|
||||
adsb_conflict.get_traffic_state();
|
||||
|
||||
printf("adsb_conflict._traffic_state %d \n", adsb_conflict._traffic_state);
|
||||
EXPECT_TRUE(adsb_conflict._traffic_state == TRAFFIC_STATE::ADD_CONFLICT);
|
||||
|
||||
}
|
||||
File diff suppressed because it is too large
Load Diff
@@ -31,4 +31,8 @@
|
||||
#
|
||||
############################################################################
|
||||
|
||||
px4_add_library(adsb adsb.cpp)
|
||||
px4_add_library(adsb AdsbConflict.cpp)
|
||||
|
||||
target_link_libraries(adsb PUBLIC geo)
|
||||
|
||||
px4_add_functional_gtest(SRC AdsbConflictTest.cpp LINKLIBS adsb)
|
||||
|
||||
@@ -1,32 +0,0 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2022 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
@@ -0,0 +1,646 @@
|
||||
{
|
||||
"cells": [
|
||||
{
|
||||
"cell_type": "code",
|
||||
"execution_count": 2340,
|
||||
"metadata": {},
|
||||
"outputs": [],
|
||||
"source": [
|
||||
"from plotly.offline import init_notebook_mode, iplot\n",
|
||||
"\n",
|
||||
"import pandas as pd\n",
|
||||
"import plotly.graph_objects as go\n",
|
||||
"import math\n",
|
||||
"import numpy as np\n",
|
||||
"np.set_printoptions(threshold=sys.maxsize)\n",
|
||||
"import random\n",
|
||||
"from decimal import Decimal"
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "code",
|
||||
"execution_count": 2341,
|
||||
"metadata": {},
|
||||
"outputs": [],
|
||||
"source": [
|
||||
"\n",
|
||||
"def to_lonlat(point_xy, origin):\n",
|
||||
" r = 6371000\n",
|
||||
"\n",
|
||||
" o_lat = math.radians(Decimal(origin[0]))\n",
|
||||
" o_lon = math.radians(Decimal(origin[1]))\n",
|
||||
" cos_phi = math.cos(o_lat)\n",
|
||||
" lat = Decimal(point_xy[1])/Decimal(r) + Decimal(o_lat)\n",
|
||||
" lon = Decimal(point_xy[0])/Decimal((r*cos_phi)) + Decimal(o_lon)\n",
|
||||
" return {'lon': math.degrees(lon), 'lat': math.degrees(lat)}\n",
|
||||
"\n",
|
||||
"\n",
|
||||
"def seg_lonlat(seg):\n",
|
||||
" return [to_lonlat(i,origin) for i in seg]\n",
|
||||
"\n",
|
||||
"def plot_segment(fig,seg):\n",
|
||||
"\n",
|
||||
" df = pd.DataFrame(seg)\n",
|
||||
" fig.add_trace(go.Scattermapbox(\n",
|
||||
" mode = \"markers+lines\",\n",
|
||||
" lon = df['lon'],\n",
|
||||
" lat = df['lat'],\n",
|
||||
" marker = {'size': 10}))\n",
|
||||
"\n",
|
||||
"def wrap(x, low, high):\n",
|
||||
" if (low <= x and x < high):\n",
|
||||
" return x\n",
|
||||
"\n",
|
||||
" range = high - low\n",
|
||||
" inv_range = 1.0/range\n",
|
||||
" num_wraps = math.floor((x - low) * inv_range)\n",
|
||||
" return x - (range * num_wraps)\n",
|
||||
"\n",
|
||||
"\n",
|
||||
"def waypoint_from_heading_and_distance(lat_traffic, lon_traffic, bearing, dist):\n",
|
||||
" r = 6371000\n",
|
||||
" bearing = wrap(bearing, 0, 2*math.pi)\n",
|
||||
" radius_ratio = dist / r\n",
|
||||
"\n",
|
||||
" lat_traffic_rad = math.radians(lat_traffic)\n",
|
||||
" lon_traffic_rad = math.radians(lon_traffic)\n",
|
||||
"\n",
|
||||
" lat_end = math.asin(math.sin(lat_traffic_rad) * math.cos(radius_ratio) +\n",
|
||||
" math.cos(lat_traffic_rad) * math.sin(radius_ratio) * math.cos(bearing))\n",
|
||||
"\n",
|
||||
" lon_end = lon_traffic_rad + math.atan2(math.sin(bearing) * math.sin(radius_ratio) * math.cos(\n",
|
||||
" lat_traffic_rad), math.cos(radius_ratio) - math.sin(lat_traffic_rad) * math.sin(lat_end))\n",
|
||||
"\n",
|
||||
" lat_end = math.degrees(lat_end)\n",
|
||||
" lon_end = math.degrees(lon_end)\n",
|
||||
"\n",
|
||||
" return lat_end, lon_end, bearing, radius_ratio\n",
|
||||
"\n",
|
||||
"\n",
|
||||
"def get_bearing_to_next_waypoint(lat_start, lon_start, lat_end, lon_end):\n",
|
||||
"\n",
|
||||
" lat_start_rad = math.radians(lat_start)\n",
|
||||
" lat_end_rad = math.radians(lat_end)\n",
|
||||
"\n",
|
||||
" cos_lat_end = math.cos(lat_end_rad)\n",
|
||||
" d_lon = math.radians(lon_end - lon_start)\n",
|
||||
"\n",
|
||||
" y = (math.sin(d_lon) * cos_lat_end)\n",
|
||||
" x = (math.cos(lat_start_rad) * math.sin(lat_end_rad) -\n",
|
||||
" math.sin(lat_start_rad) * cos_lat_end * math.cos(d_lon))\n",
|
||||
"\n",
|
||||
" return wrap(math.atan2(y, x), -math.pi, math.pi)\n",
|
||||
"\n",
|
||||
"\n",
|
||||
"def get_distance_to_next_waypoint(lat_uav, lon_uav, lat_end, lon_end):\n",
|
||||
"\n",
|
||||
" r = 6371000\n",
|
||||
"\n",
|
||||
" lat_uav_rad = math.radians(lat_uav)\n",
|
||||
" lat_end_rad = math.radians(lat_end)\n",
|
||||
"\n",
|
||||
" d_lat = lat_end_rad - lat_uav_rad\n",
|
||||
" d_lon = math.radians(lon_end) - math.radians(lon_uav)\n",
|
||||
"\n",
|
||||
" a = math.sin(d_lat / 2.0) * math.sin(d_lat / 2.0) + math.sin(d_lon / 2.0) * math.sin(d_lon / 2.0) * math.cos(lat_uav_rad) * math.cos(\n",
|
||||
" lat_end_rad)\n",
|
||||
"\n",
|
||||
" c = math.atan2(math.sqrt(a), math.sqrt(1.0 - a))\n",
|
||||
"\n",
|
||||
" return r * 2.0 * c\n",
|
||||
"\n",
|
||||
"\n",
|
||||
"def get_distance_to_line(lat_uav, lon_uav, lat_traffic, lon_traffic, lat_end, lon_end):\n",
|
||||
"\n",
|
||||
" return_value = 1\n",
|
||||
" crosstrack_distance = 0.0\n",
|
||||
"\n",
|
||||
" dist_to_end = get_distance_to_next_waypoint(\n",
|
||||
" lat_uav, lon_uav, lat_end, lon_end)\n",
|
||||
"\n",
|
||||
" if (dist_to_end < 0.1):\n",
|
||||
" return_value = 0 \n",
|
||||
"\n",
|
||||
" bearing_end = get_bearing_to_next_waypoint(\n",
|
||||
" lat_uav, lon_uav, lat_end, lon_end)\n",
|
||||
" bearing_track = get_bearing_to_next_waypoint(\n",
|
||||
" lat_traffic, lon_traffic, lat_end, lon_end)\n",
|
||||
" bearing_diff = wrap(bearing_track - bearing_end, -math.pi, math.pi)\n",
|
||||
"\n",
|
||||
" if (bearing_diff > math.pi/2 or bearing_diff < -math.pi/2):\n",
|
||||
" return_value = 0\n",
|
||||
"\n",
|
||||
" crosstrack_distance = (dist_to_end) * math.sin(bearing_diff)\n",
|
||||
"\n",
|
||||
" return return_value, crosstrack_distance \n"
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "code",
|
||||
"execution_count": 2342,
|
||||
"metadata": {},
|
||||
"outputs": [],
|
||||
"source": [
|
||||
"#uav location \n",
|
||||
"origin=[32.617013,-96.490564]\n",
|
||||
"h = 0 #x\n",
|
||||
"k = 0 #y\n",
|
||||
"uav_z = 1000\n",
|
||||
"\n",
|
||||
"vx_now = 0\n",
|
||||
"vy_now = 0\n",
|
||||
"vz_now = 0\n",
|
||||
"\n",
|
||||
"crosstrack_separation = 500\n",
|
||||
"vertical_separation = 500\n",
|
||||
"collision_time_threshold = 60.0\n",
|
||||
"\n",
|
||||
"rot_step = 1.86\n",
|
||||
"\n"
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "code",
|
||||
"execution_count": 2343,
|
||||
"metadata": {},
|
||||
"outputs": [],
|
||||
"source": [
|
||||
"lines=[\n",
|
||||
" [[10000,10000],[h,k]],\n",
|
||||
" [[1000,-1000],[h,k]],\n",
|
||||
" [[-5000,-4000],[h,k]],\n",
|
||||
" [[-3000,3000],[h,k]],\n",
|
||||
" [[-200,200],[h,k]],\n",
|
||||
" [[0,0],[h,k]],\n",
|
||||
" [[50,100],[h,k]],\n",
|
||||
" [[1000000,9000000],[h,k]],\n",
|
||||
" [[800,30],[h,k]],\n",
|
||||
" [[7000,3],[h,k]],\n",
|
||||
"]\n",
|
||||
"\n",
|
||||
"fig = go.Figure()\n",
|
||||
" \n",
|
||||
"[plot_segment(fig,seg_lonlat(i)) for i in lines]\n",
|
||||
"\n",
|
||||
"fig.update_layout(\n",
|
||||
" margin ={'l':0,'t':0,'b':0,'r':0},\n",
|
||||
" mapbox = {\n",
|
||||
" 'center': {'lon': origin[1], 'lat': origin[0]},\n",
|
||||
" 'style': \"stamen-terrain\",\n",
|
||||
" 'zoom': 13})\n",
|
||||
"\n",
|
||||
"fig.write_html('traffic.html', auto_open=True)"
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "code",
|
||||
"execution_count": 2344,
|
||||
"metadata": {},
|
||||
"outputs": [],
|
||||
"source": [
|
||||
"\n",
|
||||
"dist_xy = []\n",
|
||||
"\n",
|
||||
"for i in range(len(lines)):\n",
|
||||
" r = math.sqrt(lines[i][0][0]*lines[i][0][0] + lines[i][0][1]*lines[i][0][1])\n",
|
||||
" dist_xy.append(r) "
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "code",
|
||||
"execution_count": 2345,
|
||||
"metadata": {},
|
||||
"outputs": [],
|
||||
"source": [
|
||||
"a = [seg_lonlat(i) for i in lines]\n",
|
||||
"\n",
|
||||
"lat = []\n",
|
||||
"lon = []\n",
|
||||
"address = []\n",
|
||||
"\n",
|
||||
"for i in range(0,len(a)):\n",
|
||||
" #print(str(a[i][0]['lat'])+\",\"+str(a[i][0]['lon']))\n",
|
||||
" lat.append(a[i][0]['lat'])\n",
|
||||
" lon.append(a[i][0]['lon'])\n",
|
||||
" address.append(random.randint(1000,9999))\n"
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "code",
|
||||
"execution_count": 2364,
|
||||
"metadata": {},
|
||||
"outputs": [
|
||||
{
|
||||
"name": "stdout",
|
||||
"output_type": "stream",
|
||||
"text": [
|
||||
"[6751, 2839, 2431, 3222, 2171, 7666, 4249, 8438, 4110, 5798]\n"
|
||||
]
|
||||
}
|
||||
],
|
||||
"source": [
|
||||
"print(address)"
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "code",
|
||||
"execution_count": 2346,
|
||||
"metadata": {},
|
||||
"outputs": [],
|
||||
"source": [
|
||||
"heading = np.arange(-math.pi, math.pi, math.radians(rot_step)) "
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "code",
|
||||
"execution_count": 2347,
|
||||
"metadata": {},
|
||||
"outputs": [],
|
||||
"source": [
|
||||
"headings = []\n",
|
||||
"\n",
|
||||
"ct_dist = []\n",
|
||||
"ct_distance_conflict_threshold = []\n",
|
||||
"\n",
|
||||
"vertical_arr = []\n",
|
||||
"vertical_check = []\n",
|
||||
"\n",
|
||||
"dist_xyz = []\n",
|
||||
"traffic_vel = []\n",
|
||||
"time_check = []\n",
|
||||
"collision_times = []"
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "code",
|
||||
"execution_count": 2348,
|
||||
"metadata": {},
|
||||
"outputs": [],
|
||||
"source": [
|
||||
"for j in range(len(dist_xy)):\n",
|
||||
"\n",
|
||||
" prediction_dist = dist_xy[j] + 1000 \n",
|
||||
"\n",
|
||||
" for i in heading:\n",
|
||||
"\n",
|
||||
" headings.append(i) \n",
|
||||
"\n",
|
||||
" lat_end, lon_end, bearing, radius_ratio = waypoint_from_heading_and_distance(lat[j],lon[j],i,prediction_dist)\n",
|
||||
" \n",
|
||||
"\n",
|
||||
" return_value, ct = get_distance_to_line(origin[0],origin[1],lat[j],lon[j],lat_end, lon_end)\n",
|
||||
"\n",
|
||||
" ct = abs(ct)\n",
|
||||
"\n",
|
||||
" #print(lat_end,lon_end, bearing, radius_ratio, return_value, ct)\n",
|
||||
"\n",
|
||||
" ct_dist.append(ct)\n",
|
||||
" if(ct < crosstrack_separation and return_value):\n",
|
||||
" ct_distance_conflict_threshold.append(True)\n",
|
||||
" else:\n",
|
||||
" ct_distance_conflict_threshold.append(False)\n",
|
||||
"\n",
|
||||
" vertical_separation_check = random.choice([True, False])\n",
|
||||
" if (vertical_separation_check):\n",
|
||||
" z_diff = random.randrange(0, vertical_separation-1)\n",
|
||||
" vertical_check.append(True)\n",
|
||||
" else:\n",
|
||||
" z_diff = random.randrange(vertical_separation+1, 1000)\n",
|
||||
" vertical_check.append(False)\n",
|
||||
" \n",
|
||||
" sign = random.choice([-1, 1])\n",
|
||||
" vertical_arr.append(sign*z_diff+uav_z)\n",
|
||||
"\n",
|
||||
" xyz = math.sqrt(z_diff*z_diff + dist_xy[j]*dist_xy[j])\n",
|
||||
" dist_xyz.append(xyz)\n",
|
||||
"\n",
|
||||
" collision_time_check = random.choice([True, False])\n",
|
||||
" if (collision_time_check):\n",
|
||||
" time_to_collision = random.randrange(1, collision_time_threshold)\n",
|
||||
" time_check.append(True)\n",
|
||||
" collision_times.append(time_to_collision)\n",
|
||||
" else:\n",
|
||||
" time_to_collision = random.randrange(collision_time_threshold+1, 10*collision_time_threshold)\n",
|
||||
" time_check.append(False)\n",
|
||||
" collision_times.append(time_to_collision)\n",
|
||||
"\n",
|
||||
" vel = xyz/time_to_collision\n",
|
||||
" traffic_vel.append(math.sqrt((vel*vel)/2))\n",
|
||||
"\n",
|
||||
" "
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "code",
|
||||
"execution_count": 2349,
|
||||
"metadata": {},
|
||||
"outputs": [],
|
||||
"source": [
|
||||
"lat_traffic = np.array([])\n",
|
||||
"\n",
|
||||
"for i in lat:\n",
|
||||
" for j in range(len(heading)):\n",
|
||||
" lat_now = np.array([i])\n",
|
||||
" lat_traffic = np.concatenate([lat_traffic, lat_now])\n",
|
||||
"\n",
|
||||
"\n",
|
||||
"lon_traffic = np.array([])\n",
|
||||
"\n",
|
||||
"for i in lon:\n",
|
||||
" for j in range(len(heading)):\n",
|
||||
" lon_now = np.array([i])\n",
|
||||
" lon_traffic = np.concatenate([lon_traffic, lon_now])\n"
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "code",
|
||||
"execution_count": 2350,
|
||||
"metadata": {},
|
||||
"outputs": [],
|
||||
"source": [
|
||||
"in_conflict = []\n",
|
||||
"\n",
|
||||
"for i in range(len(time_check)):\n",
|
||||
" if((time_check[i]== True) and (vertical_check[i] == True) and (ct_distance_conflict_threshold[i] == True)):\n",
|
||||
" in_conflict.append(True)\n",
|
||||
" else:\n",
|
||||
" in_conflict.append(False)"
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "code",
|
||||
"execution_count": 2351,
|
||||
"metadata": {},
|
||||
"outputs": [],
|
||||
"source": [
|
||||
"#print(vertical_check)"
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "code",
|
||||
"execution_count": 2352,
|
||||
"metadata": {},
|
||||
"outputs": [
|
||||
{
|
||||
"name": "stdout",
|
||||
"output_type": "stream",
|
||||
"text": [
|
||||
"946\n"
|
||||
]
|
||||
}
|
||||
],
|
||||
"source": [
|
||||
"print(vertical_arr[78])"
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "code",
|
||||
"execution_count": 2353,
|
||||
"metadata": {},
|
||||
"outputs": [],
|
||||
"source": [
|
||||
"#print(headings)"
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "code",
|
||||
"execution_count": 2354,
|
||||
"metadata": {},
|
||||
"outputs": [
|
||||
{
|
||||
"name": "stdout",
|
||||
"output_type": "stream",
|
||||
"text": [
|
||||
"13903.21572374496\n"
|
||||
]
|
||||
}
|
||||
],
|
||||
"source": [
|
||||
"print(ct_dist[78])"
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "code",
|
||||
"execution_count": 2355,
|
||||
"metadata": {},
|
||||
"outputs": [],
|
||||
"source": [
|
||||
"#print(ct_distance_conflict_threshold)"
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "code",
|
||||
"execution_count": 2356,
|
||||
"metadata": {},
|
||||
"outputs": [
|
||||
{
|
||||
"name": "stdout",
|
||||
"output_type": "stream",
|
||||
"text": [
|
||||
"False\n"
|
||||
]
|
||||
}
|
||||
],
|
||||
"source": [
|
||||
"print(time_check[78])"
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "code",
|
||||
"execution_count": 2357,
|
||||
"metadata": {},
|
||||
"outputs": [
|
||||
{
|
||||
"data": {
|
||||
"text/plain": [
|
||||
"456"
|
||||
]
|
||||
},
|
||||
"execution_count": 2357,
|
||||
"metadata": {},
|
||||
"output_type": "execute_result"
|
||||
}
|
||||
],
|
||||
"source": [
|
||||
"collision_times[78]"
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "code",
|
||||
"execution_count": 2358,
|
||||
"metadata": {},
|
||||
"outputs": [
|
||||
{
|
||||
"data": {
|
||||
"text/plain": [
|
||||
"21.929984429241845"
|
||||
]
|
||||
},
|
||||
"execution_count": 2358,
|
||||
"metadata": {},
|
||||
"output_type": "execute_result"
|
||||
}
|
||||
],
|
||||
"source": [
|
||||
"traffic_vel[78]"
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "code",
|
||||
"execution_count": 2359,
|
||||
"metadata": {},
|
||||
"outputs": [
|
||||
{
|
||||
"name": "stdout",
|
||||
"output_type": "stream",
|
||||
"text": [
|
||||
"False\n"
|
||||
]
|
||||
}
|
||||
],
|
||||
"source": [
|
||||
"print(in_conflict[78])"
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "code",
|
||||
"execution_count": 2360,
|
||||
"metadata": {},
|
||||
"outputs": [
|
||||
{
|
||||
"name": "stdout",
|
||||
"output_type": "stream",
|
||||
"text": [
|
||||
"158\n"
|
||||
]
|
||||
}
|
||||
],
|
||||
"source": [
|
||||
"print(np.count_nonzero(in_conflict))"
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "code",
|
||||
"execution_count": 2361,
|
||||
"metadata": {},
|
||||
"outputs": [
|
||||
{
|
||||
"name": "stdout",
|
||||
"output_type": "stream",
|
||||
"text": [
|
||||
"dist_xy 10\n",
|
||||
"lat_traffic 1940\n",
|
||||
"lon_traffic 1940\n"
|
||||
]
|
||||
}
|
||||
],
|
||||
"source": [
|
||||
"print(\"dist_xy\", len(dist_xy))\n",
|
||||
"print(\"lat_traffic\", len(lat_traffic))\n",
|
||||
"print(\"lon_traffic\", len(lon_traffic))\n",
|
||||
"\n"
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "code",
|
||||
"execution_count": 2362,
|
||||
"metadata": {},
|
||||
"outputs": [],
|
||||
"source": [
|
||||
"\"\"\" \t\n",
|
||||
"\tstruct traffic_data_s {\n",
|
||||
"\tdouble lat_traffic\n",
|
||||
"\tdouble lon_traffic\n",
|
||||
"\tfloat alt_traffic\n",
|
||||
"\tfloat heading_traffic\n",
|
||||
"\tfloat vxy_traffic\n",
|
||||
"\tfloat vz_traffic\n",
|
||||
"\tbool in_conflict\n",
|
||||
"\"\"\"\n",
|
||||
"\n",
|
||||
"traffic_data = np.zeros(shape=(7, len(lat_traffic)))\n",
|
||||
"\n",
|
||||
"traffic_data = np.concatenate([traffic_data,[lat_traffic]])\n",
|
||||
"\n",
|
||||
"traffic_data = np.concatenate([traffic_data,[lon_traffic]])\n",
|
||||
"\n",
|
||||
"\n",
|
||||
"alt_traffic = np.asarray(vertical_arr)\n",
|
||||
"traffic_data = np.concatenate([traffic_data,[alt_traffic]])\n",
|
||||
"\n",
|
||||
"headings_arr = np.asarray(headings)\n",
|
||||
"traffic_data = np.concatenate([traffic_data,[headings_arr]])\n",
|
||||
"\n",
|
||||
"vxy_traffic = np.asarray(traffic_vel)\n",
|
||||
"traffic_data = np.concatenate([traffic_data,[vxy_traffic]])\n",
|
||||
"\n",
|
||||
"vz_traffic = np.asarray(traffic_vel)\n",
|
||||
"traffic_data = np.concatenate([traffic_data,[vz_traffic]])\n",
|
||||
"\n",
|
||||
"in_conflict_arr = np.asarray(in_conflict)\n",
|
||||
"traffic_data = np.concatenate([traffic_data,[in_conflict_arr]])\n",
|
||||
"\n",
|
||||
"for i in range(7):\n",
|
||||
"\ttraffic_data = np.delete(traffic_data,0,0)\n",
|
||||
"\n"
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "code",
|
||||
"execution_count": 2363,
|
||||
"metadata": {},
|
||||
"outputs": [
|
||||
{
|
||||
"name": "stdout",
|
||||
"output_type": "stream",
|
||||
"text": [
|
||||
"(1940, 7)\n"
|
||||
]
|
||||
}
|
||||
],
|
||||
"source": [
|
||||
"np.savetxt(\"foo.csv\", traffic_data.transpose(), delimiter=\",\",newline='}, \\n {')\n",
|
||||
"print(traffic_data.transpose().shape)"
|
||||
]
|
||||
}
|
||||
],
|
||||
"metadata": {
|
||||
"kernelspec": {
|
||||
"display_name": "Python 3.8.10 64-bit",
|
||||
"language": "python",
|
||||
"name": "python3"
|
||||
},
|
||||
"language_info": {
|
||||
"codemirror_mode": {
|
||||
"name": "ipython",
|
||||
"version": 3
|
||||
},
|
||||
"file_extension": ".py",
|
||||
"mimetype": "text/x-python",
|
||||
"name": "python",
|
||||
"nbconvert_exporter": "python",
|
||||
"pygments_lexer": "ipython3",
|
||||
"version": "3.8.10"
|
||||
},
|
||||
"toc": {
|
||||
"base_numbering": 1,
|
||||
"nav_menu": {},
|
||||
"number_sections": true,
|
||||
"sideBar": true,
|
||||
"skip_h1_title": false,
|
||||
"title_cell": "Table of Contents",
|
||||
"title_sidebar": "Contents",
|
||||
"toc_cell": false,
|
||||
"toc_position": {},
|
||||
"toc_section_display": true,
|
||||
"toc_window_display": false
|
||||
},
|
||||
"vscode": {
|
||||
"interpreter": {
|
||||
"hash": "e7370f93d1d0cde622a1f8e1c04877d8463912d04d973331ad4851f04de6915a"
|
||||
}
|
||||
}
|
||||
},
|
||||
"nbformat": 4,
|
||||
"nbformat_minor": 4
|
||||
}
|
||||
Reference in New Issue
Block a user