forked from Archive/PX4-Autopilot
[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
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
|
||||
}
|
||||
|
|
@ -49,10 +49,6 @@
|
|||
using matrix::wrap_pi;
|
||||
using matrix::wrap_2pi;
|
||||
|
||||
#ifndef hrt_absolute_time
|
||||
# define hrt_absolute_time() (0)
|
||||
#endif
|
||||
|
||||
/*
|
||||
* Azimuthal Equidistant Projection
|
||||
* formulas according to: http://mathworld.wolfram.com/AzimuthalEquidistantProjection.html
|
||||
|
|
@ -146,6 +142,7 @@ void waypoint_from_heading_and_distance(double lat_start, double lon_start, floa
|
|||
double *lat_target, double *lon_target)
|
||||
{
|
||||
bearing = wrap_2pi(bearing);
|
||||
|
||||
double radius_ratio = static_cast<double>(dist) / CONSTANTS_RADIUS_OF_EARTH;
|
||||
|
||||
double lat_start_rad = math::radians(lat_start);
|
||||
|
|
@ -218,7 +215,7 @@ void add_vector_to_global_position(double lat_now, double lon_now, float v_n, fl
|
|||
|
||||
// Additional functions - @author Doug Weibel <douglas.weibel@colorado.edu>
|
||||
|
||||
int get_distance_to_line(struct crosstrack_error_s *crosstrack_error, double lat_now, double lon_now,
|
||||
int get_distance_to_line(struct crosstrack_error_s &crosstrack_error, double lat_now, double lon_now,
|
||||
double lat_start, double lon_start, double lat_end, double lon_end)
|
||||
{
|
||||
// This function returns the distance to the nearest point on the track line. Distance is positive if current
|
||||
|
|
@ -226,9 +223,9 @@ int get_distance_to_line(struct crosstrack_error_s *crosstrack_error, double lat
|
|||
// headed towards the end point.
|
||||
|
||||
int return_value = -1; // Set error flag, cleared when valid result calculated.
|
||||
crosstrack_error->past_end = false;
|
||||
crosstrack_error->distance = 0.0f;
|
||||
crosstrack_error->bearing = 0.0f;
|
||||
crosstrack_error.past_end = false;
|
||||
crosstrack_error.distance = 0.0f;
|
||||
crosstrack_error.bearing = 0.0f;
|
||||
|
||||
float dist_to_end = get_distance_to_next_waypoint(lat_now, lon_now, lat_end, lon_end);
|
||||
|
||||
|
|
@ -243,18 +240,18 @@ int get_distance_to_line(struct crosstrack_error_s *crosstrack_error, double lat
|
|||
|
||||
// Return past_end = true if past end point of line
|
||||
if (bearing_diff > M_PI_2_F || bearing_diff < -M_PI_2_F) {
|
||||
crosstrack_error->past_end = true;
|
||||
crosstrack_error.past_end = true;
|
||||
return_value = 0;
|
||||
return return_value;
|
||||
}
|
||||
|
||||
crosstrack_error->distance = (dist_to_end) * sinf(bearing_diff);
|
||||
crosstrack_error.distance = (dist_to_end) * sinf(bearing_diff);
|
||||
|
||||
if (sinf(bearing_diff) >= 0) {
|
||||
crosstrack_error->bearing = wrap_pi(bearing_track - M_PI_2_F);
|
||||
crosstrack_error.bearing = wrap_pi(bearing_track - M_PI_2_F);
|
||||
|
||||
} else {
|
||||
crosstrack_error->bearing = wrap_pi(bearing_track + M_PI_2_F);
|
||||
crosstrack_error.bearing = wrap_pi(bearing_track + M_PI_2_F);
|
||||
}
|
||||
|
||||
return_value = 0;
|
||||
|
|
|
|||
|
|
@ -136,7 +136,7 @@ void get_vector_to_next_waypoint_fast(double lat_now, double lon_now, double lat
|
|||
void add_vector_to_global_position(double lat_now, double lon_now, float v_n, float v_e, double *lat_res,
|
||||
double *lon_res);
|
||||
|
||||
int get_distance_to_line(struct crosstrack_error_s *crosstrack_error, double lat_now, double lon_now,
|
||||
int get_distance_to_line(struct crosstrack_error_s &crosstrack_error, double lat_now, double lon_now,
|
||||
double lat_start, double lon_start, double lat_end, double lon_end);
|
||||
|
||||
int get_distance_to_arc(struct crosstrack_error_s *crosstrack_error, double lat_now, double lon_now,
|
||||
|
|
|
|||
|
|
@ -52,6 +52,7 @@ px4_add_module(
|
|||
vtol_takeoff.cpp
|
||||
DEPENDS
|
||||
geo
|
||||
adsb
|
||||
geofence_breach_avoidance
|
||||
motion_planning
|
||||
mission_feasibility_checker
|
||||
|
|
|
|||
|
|
@ -55,6 +55,7 @@
|
|||
|
||||
#include "GeofenceBreachAvoidance/geofence_breach_avoidance.h"
|
||||
|
||||
#include <lib/adsb/AdsbConflict.h>
|
||||
#include <lib/perf/perf_counter.h>
|
||||
#include <px4_platform_common/module.h>
|
||||
#include <px4_platform_common/module_params.h>
|
||||
|
|
@ -129,29 +130,14 @@ public:
|
|||
*/
|
||||
void publish_vehicle_cmd(vehicle_command_s *vcmd);
|
||||
|
||||
/**
|
||||
* Generate an artificial traffic indication
|
||||
*
|
||||
* @param distance Horizontal distance to this vehicle
|
||||
* @param direction Direction in earth frame from this vehicle in radians
|
||||
* @param traffic_heading Travel direction of the traffic in earth frame in radians
|
||||
* @param altitude_diff Altitude difference, positive is up
|
||||
* @param hor_velocity Horizontal velocity of traffic, in m/s
|
||||
* @param ver_velocity Vertical velocity of traffic, in m/s
|
||||
* @param emitter_type, Type of vehicle, as a number
|
||||
*/
|
||||
void fake_traffic(const char *callsign, float distance, float direction, float traffic_heading, float altitude_diff,
|
||||
float hor_velocity, float ver_velocity, int emitter_type);
|
||||
|
||||
/**
|
||||
* Check nearby traffic for potential collisions
|
||||
*/
|
||||
void check_traffic();
|
||||
|
||||
/**
|
||||
* Buffer for air traffic to control the amount of messages sent to a user
|
||||
*/
|
||||
bool buffer_air_traffic(uint32_t icao_address);
|
||||
void take_traffic_conflict_action();
|
||||
|
||||
void run_fake_traffic();
|
||||
|
||||
/**
|
||||
* Setters
|
||||
|
|
@ -327,11 +313,6 @@ public:
|
|||
|
||||
private:
|
||||
|
||||
struct traffic_buffer_s {
|
||||
uint32_t icao_address;
|
||||
hrt_abstime timestamp;
|
||||
};
|
||||
|
||||
int _local_pos_sub{-1};
|
||||
int _mission_sub{-1};
|
||||
int _vehicle_status_sub{-1};
|
||||
|
|
@ -395,6 +376,7 @@ private:
|
|||
Land _land; /**< class for handling land commands */
|
||||
PrecLand _precland; /**< class for handling precision land commands */
|
||||
RTL _rtl; /**< class that handles RTL */
|
||||
AdsbConflict _adsb_conflict; /**< class that handles ADSB conflict avoidance */
|
||||
|
||||
NavigatorMode *_navigation_mode{nullptr}; /**< abstract pointer to current navigation mode class */
|
||||
NavigatorMode *_navigation_mode_array[NAVIGATOR_MODE_ARRAY_SIZE] {}; /**< array of navigation modes */
|
||||
|
|
@ -445,8 +427,9 @@ private:
|
|||
(ParamFloat<px4::params::NAV_MC_ALT_RAD>) _param_nav_mc_alt_rad, /**< acceptance rad for multicopter alt */
|
||||
(ParamInt<px4::params::NAV_FORCE_VT>) _param_nav_force_vt, /**< acceptance radius for multicopter alt */
|
||||
(ParamInt<px4::params::NAV_TRAFF_AVOID>) _param_nav_traff_avoid, /**< avoiding other aircraft is enabled */
|
||||
(ParamFloat<px4::params::NAV_TRAFF_A_RADU>) _param_nav_traff_a_radu, /**< avoidance Distance Unmanned*/
|
||||
(ParamFloat<px4::params::NAV_TRAFF_A_RADM>) _param_nav_traff_a_radm, /**< avoidance Distance Manned*/
|
||||
(ParamFloat<px4::params::NAV_TRAFF_A_HOR>) _param_nav_traff_a_hor_ct, /**< avoidance Distance Crosstrack*/
|
||||
(ParamFloat<px4::params::NAV_TRAFF_A_VER>) _param_nav_traff_a_ver, /**< avoidance Distance Vertical*/
|
||||
(ParamInt<px4::params::NAV_TRAFF_COLL_T>) _param_nav_traff_collision_time,
|
||||
(ParamFloat<px4::params::NAV_MIN_LTR_ALT>) _param_min_ltr_alt, /**< minimum altitude in Loiter mode*/
|
||||
|
||||
// non-navigator parameters: Mission (MIS_*)
|
||||
|
|
|
|||
|
|
@ -51,6 +51,7 @@
|
|||
#include <dataman/dataman.h>
|
||||
#include <drivers/drv_hrt.h>
|
||||
#include <lib/geo/geo.h>
|
||||
#include <lib/adsb/AdsbConflict.h>
|
||||
#include <lib/mathlib/mathlib.h>
|
||||
#include <px4_platform_common/px4_config.h>
|
||||
#include <px4_platform_common/defines.h>
|
||||
|
|
@ -107,6 +108,10 @@ Navigator::Navigator() :
|
|||
// Update the timeout used in mission_block (which can't hold it's own parameters)
|
||||
_mission.set_payload_deployment_timeout(_param_mis_payload_delivery_timeout.get());
|
||||
|
||||
_adsb_conflict.set_conflict_detection_params(_param_nav_traff_a_hor_ct.get(),
|
||||
_param_nav_traff_a_ver.get(),
|
||||
_param_nav_traff_collision_time.get(), _param_nav_traff_avoid.get());
|
||||
|
||||
reset_triplets();
|
||||
}
|
||||
|
||||
|
|
@ -1202,281 +1207,68 @@ void Navigator::load_fence_from_file(const char *filename)
|
|||
_geofence.loadFromFile(filename);
|
||||
}
|
||||
|
||||
void Navigator::fake_traffic(const char *callsign, float distance, float direction, float traffic_heading,
|
||||
float altitude_diff, float hor_velocity, float ver_velocity, int emitter_type)
|
||||
void Navigator::take_traffic_conflict_action()
|
||||
{
|
||||
double lat{0.0};
|
||||
double lon{0.0};
|
||||
|
||||
waypoint_from_heading_and_distance(get_global_position()->lat, get_global_position()->lon, direction, distance, &lat,
|
||||
&lon);
|
||||
float alt = get_global_position()->alt + altitude_diff;
|
||||
vehicle_command_s vcmd = {};
|
||||
|
||||
// float vel_n = get_global_position()->vel_n;
|
||||
// float vel_e = get_global_position()->vel_e;
|
||||
// float vel_d = get_global_position()->vel_d;
|
||||
switch (_adsb_conflict._conflict_detection_params.traffic_avoidance_mode) {
|
||||
|
||||
transponder_report_s tr{};
|
||||
tr.timestamp = hrt_absolute_time();
|
||||
tr.icao_address = 1234;
|
||||
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;
|
||||
case 2: {
|
||||
_rtl.set_return_alt_min(true);
|
||||
vcmd.command = vehicle_command_s::VEHICLE_CMD_NAV_RETURN_TO_LAUNCH;
|
||||
publish_vehicle_cmd(&vcmd);
|
||||
break;
|
||||
}
|
||||
|
||||
#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
|
||||
case 3: {
|
||||
vcmd.command = vehicle_command_s::VEHICLE_CMD_NAV_LAND;
|
||||
publish_vehicle_cmd(&vcmd);
|
||||
break;
|
||||
|
||||
for (int i = 0; i < PX4_GUID_BYTE_LENGTH ; i++) {
|
||||
tr.uas_id[i] = 0xe0 + i; //simulate GUID
|
||||
}
|
||||
|
||||
case 4: {
|
||||
|
||||
vcmd.command = vehicle_command_s::VEHICLE_CMD_NAV_LOITER_UNLIM;
|
||||
publish_vehicle_cmd(&vcmd);
|
||||
break;
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
#endif /* BOARD_HAS_NO_UUID */
|
||||
}
|
||||
|
||||
uORB::Publication<transponder_report_s> tr_pub{ORB_ID(transponder_report)};
|
||||
tr_pub.publish(tr);
|
||||
void Navigator::run_fake_traffic()
|
||||
{
|
||||
|
||||
_adsb_conflict.run_fake_traffic(get_global_position()->lat, get_global_position()->lon,
|
||||
get_global_position()->alt);
|
||||
}
|
||||
|
||||
void Navigator::check_traffic()
|
||||
{
|
||||
double lat = get_global_position()->lat;
|
||||
double lon = get_global_position()->lon;
|
||||
float alt = get_global_position()->alt;
|
||||
|
||||
// TODO for non-multirotors predicting the future
|
||||
// position as accurately as possible will become relevant
|
||||
// float vel_n = get_global_position()->vel_n;
|
||||
// float vel_e = get_global_position()->vel_e;
|
||||
// float vel_d = get_global_position()->vel_d;
|
||||
if (_traffic_sub.updated()) {
|
||||
|
||||
bool changed = _traffic_sub.updated();
|
||||
|
||||
char uas_id[11]; //GUID of incoming UTM messages
|
||||
|
||||
float NAVTrafficAvoidUnmanned = _param_nav_traff_a_radu.get();
|
||||
float NAVTrafficAvoidManned = _param_nav_traff_a_radm.get();
|
||||
float horizontal_separation = NAVTrafficAvoidManned;
|
||||
float vertical_separation = NAVTrafficAvoidManned;
|
||||
|
||||
while (changed) {
|
||||
|
||||
//vehicle_status_s vs{};
|
||||
transponder_report_s tr{};
|
||||
_traffic_sub.copy(&tr);
|
||||
_traffic_sub.copy(&_adsb_conflict._transponder_report);
|
||||
|
||||
uint16_t required_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;
|
||||
|
||||
if ((tr.flags & required_flags) != required_flags) {
|
||||
changed = _traffic_sub.updated();
|
||||
continue;
|
||||
}
|
||||
if ((_adsb_conflict._transponder_report.flags & required_flags) == required_flags) {
|
||||
|
||||
//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", tr.uas_id[PX4_GUID_BYTE_LENGTH - 5 + i]);
|
||||
}
|
||||
_adsb_conflict.detect_traffic_conflict(get_global_position()->lat, get_global_position()->lon,
|
||||
get_global_position()->alt, _local_pos.vx, _local_pos.vy, _local_pos.vz);
|
||||
|
||||
uint64_t uas_id_int = 0;
|
||||
|
||||
for (int i = 0; i < 8; i++) {
|
||||
uas_id_int |= (uint64_t)(tr.uas_id[PX4_GUID_BYTE_LENGTH - i - 1]) << (i * 8);
|
||||
}
|
||||
|
||||
//Manned/Unmanned Vehicle Seperation Distance
|
||||
if (tr.emitter_type == transponder_report_s::ADSB_EMITTER_TYPE_UAV) {
|
||||
horizontal_separation = NAVTrafficAvoidUnmanned;
|
||||
vertical_separation = NAVTrafficAvoidUnmanned;
|
||||
}
|
||||
|
||||
float d_hor, d_vert;
|
||||
get_distance_to_point_global_wgs84(lat, lon, alt,
|
||||
tr.lat, tr.lon, tr.altitude, &d_hor, &d_vert);
|
||||
|
||||
|
||||
// predict final altitude (positive is up) in prediction time frame
|
||||
float end_alt = tr.altitude + (d_vert / tr.hor_velocity) * tr.ver_velocity;
|
||||
|
||||
// Predict until the vehicle would have passed this system at its current speed
|
||||
float prediction_distance = d_hor + 1000.0f;
|
||||
|
||||
// If the altitude is not getting close to us, do not calculate
|
||||
// the horizontal separation.
|
||||
// Since commercial flights do most of the time keep flight levels
|
||||
// check for the current and for the predicted flight level.
|
||||
// we also make the implicit assumption that this system is on the lowest
|
||||
// flight level close to ground in the
|
||||
// (end_alt - horizontal_separation < alt) condition. If this system should
|
||||
// ever be used in normal airspace this implementation would anyway be
|
||||
// inappropriate as it should be replaced with a TCAS compliant solution.
|
||||
|
||||
if ((fabsf(alt - tr.altitude) < vertical_separation) || ((end_alt - horizontal_separation) < alt)) {
|
||||
|
||||
double end_lat, end_lon;
|
||||
waypoint_from_heading_and_distance(tr.lat, tr.lon, tr.heading, prediction_distance, &end_lat, &end_lon);
|
||||
|
||||
struct crosstrack_error_s cr;
|
||||
|
||||
if (!get_distance_to_line(&cr, lat, lon, tr.lat, tr.lon, end_lat, end_lon)) {
|
||||
|
||||
if (!cr.past_end && (fabsf(cr.distance) < horizontal_separation)) {
|
||||
|
||||
bool action_needed = buffer_air_traffic(tr.icao_address);
|
||||
|
||||
if (action_needed) {
|
||||
// direction of traffic in human-readable 0..360 degree in earth frame
|
||||
int traffic_direction = math::degrees(tr.heading) + 180;
|
||||
int traffic_seperation = (int)fabsf(cr.distance);
|
||||
|
||||
switch (_param_nav_traff_avoid.get()) {
|
||||
|
||||
case 0: {
|
||||
/* Ignore */
|
||||
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: {
|
||||
/* Warn only */
|
||||
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: {
|
||||
/* RTL Mode */
|
||||
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);
|
||||
|
||||
// set the return altitude to minimum
|
||||
_rtl.set_return_alt_min(true);
|
||||
|
||||
// ask the commander to execute an RTL
|
||||
vehicle_command_s vcmd = {};
|
||||
vcmd.command = vehicle_command_s::VEHICLE_CMD_NAV_RETURN_TO_LAUNCH;
|
||||
publish_vehicle_cmd(&vcmd);
|
||||
break;
|
||||
}
|
||||
|
||||
case 3: {
|
||||
/* Land Mode */
|
||||
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);
|
||||
|
||||
// ask the commander to land
|
||||
vehicle_command_s vcmd = {};
|
||||
vcmd.command = vehicle_command_s::VEHICLE_CMD_NAV_LAND;
|
||||
publish_vehicle_cmd(&vcmd);
|
||||
break;
|
||||
|
||||
}
|
||||
|
||||
case 4: {
|
||||
/* Position hold */
|
||||
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);
|
||||
|
||||
// ask the commander to Loiter
|
||||
vehicle_command_s vcmd = {};
|
||||
vcmd.command = vehicle_command_s::VEHICLE_CMD_NAV_LOITER_UNLIM;
|
||||
publish_vehicle_cmd(&vcmd);
|
||||
break;
|
||||
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
if (_adsb_conflict.handle_traffic_conflict()) {
|
||||
take_traffic_conflict_action();
|
||||
}
|
||||
}
|
||||
|
||||
changed = _traffic_sub.updated();
|
||||
}
|
||||
}
|
||||
|
||||
bool Navigator::buffer_air_traffic(uint32_t icao_address)
|
||||
{
|
||||
bool action_needed = true;
|
||||
|
||||
if (_traffic_buffer.icao_address == icao_address) {
|
||||
|
||||
if (hrt_elapsed_time(&_traffic_buffer.timestamp) > 60_s) {
|
||||
_traffic_buffer.timestamp = hrt_absolute_time();
|
||||
|
||||
} else {
|
||||
action_needed = false;
|
||||
}
|
||||
|
||||
} else {
|
||||
_traffic_buffer.timestamp = hrt_absolute_time();
|
||||
_traffic_buffer.icao_address = icao_address;
|
||||
}
|
||||
|
||||
return action_needed;
|
||||
}
|
||||
|
||||
bool Navigator::abort_landing()
|
||||
{
|
||||
// only abort if currently landing and position controller status updated
|
||||
|
|
@ -1519,12 +1311,9 @@ int Navigator::custom_command(int argc, char *argv[])
|
|||
return 0;
|
||||
|
||||
} else if (!strcmp(argv[0], "fake_traffic")) {
|
||||
get_instance()->fake_traffic("LX007", 500, 1.0f, -1.0f, 100.0f, 90.0f, 0.001f,
|
||||
transponder_report_s::ADSB_EMITTER_TYPE_LIGHT);
|
||||
get_instance()->fake_traffic("LX55", 1000, 0, 0, 100.0f, 90.0f, 0.001f, transponder_report_s::ADSB_EMITTER_TYPE_SMALL);
|
||||
get_instance()->fake_traffic("LX20", 15000, 1.0f, -1.0f, 280.0f, 90.0f, 0.001f,
|
||||
transponder_report_s::ADSB_EMITTER_TYPE_LARGE);
|
||||
get_instance()->fake_traffic("UAV", 10, 1.0f, -2.0f, 10.0f, 10.0f, 0.01f, transponder_report_s::ADSB_EMITTER_TYPE_UAV);
|
||||
|
||||
get_instance()->run_fake_traffic();
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -128,23 +128,20 @@ PARAM_DEFINE_FLOAT(NAV_MC_ALT_RAD, 0.8f);
|
|||
PARAM_DEFINE_INT32(NAV_TRAFF_AVOID, 1);
|
||||
|
||||
/**
|
||||
* Set NAV TRAFFIC AVOID RADIUS MANNED
|
||||
* Set NAV TRAFFIC AVOID horizontal distance
|
||||
*
|
||||
* Defines the Radius where NAV TRAFFIC AVOID is Called
|
||||
* For Manned Aviation
|
||||
* Defines a crosstrack horizontal distance
|
||||
*
|
||||
* @unit m
|
||||
* @min 500
|
||||
*
|
||||
* @group Mission
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(NAV_TRAFF_A_RADM, 500);
|
||||
PARAM_DEFINE_FLOAT(NAV_TRAFF_A_HOR, 500);
|
||||
|
||||
/**
|
||||
* Set NAV TRAFFIC AVOID RADIUS
|
||||
* Set NAV TRAFFIC AVOID vertical distance
|
||||
*
|
||||
* Defines the Radius where NAV TRAFFIC AVOID is Called
|
||||
* For Unmanned Aviation
|
||||
*
|
||||
* @unit m
|
||||
* @min 10
|
||||
|
|
@ -152,7 +149,20 @@ PARAM_DEFINE_FLOAT(NAV_TRAFF_A_RADM, 500);
|
|||
*
|
||||
* @group Mission
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(NAV_TRAFF_A_RADU, 10);
|
||||
PARAM_DEFINE_FLOAT(NAV_TRAFF_A_VER, 500);
|
||||
|
||||
/**
|
||||
* Estimated time until collision
|
||||
*
|
||||
* Minimum acceptable time until collsion.
|
||||
* Assumes constant speed over 3d distance.
|
||||
*
|
||||
* @unit s
|
||||
* @min 1
|
||||
* @max 900000000
|
||||
* @group Mission
|
||||
*/
|
||||
PARAM_DEFINE_INT32(NAV_TRAFF_COLL_T, 60);
|
||||
|
||||
/**
|
||||
* Force VTOL mode takeoff and land
|
||||
|
|
|
|||
Loading…
Reference in New Issue