fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-15 19:47:36 +08:00
Code Issues Packages Projects Releases Wiki Activity

[navigator] Tone Down traffic warnings

Browse Source 
- 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
2022-11-17 06:37:55 -05:00
committed by Beat Küng
parent 89548e4f9e
commit 9de52bb5ec
13 changed files with 3635 additions and 333 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/navigator/CMakeLists.txt
+1
View File
@@ -52,6 +52,7 @@ px4_add_module(
vtol_takeoff.cpp
DEPENDS
geo
adsb
geofence_breach_avoidance
motion_planning
mission_feasibility_checker
src/modules/navigator/navigator.h
+8 -25
View File
@@ -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_*)
src/modules/navigator/navigator_main.cpp
+43 -254
View File
@@ -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;
}
src/modules/navigator/navigator_params.c
+18 -8
View File
@@ -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