Skip to content

Commit

Permalink
ADSB traffic avoidance improvements using UTM_GLOBAL_POSITION (#13159)
Browse files Browse the repository at this point in the history 
 * Treat UAVS diffrently from manned aviation 
 * Added fake_traffic testing functionality,
 * Added NAV_TRAFF_AVOID Hold and Landmode
 * Added Behavior: HOLD Position to collision avoidance mode and implemented Landmode to collision avoidance.

Boards where no Hardware GUID is defined will send 0 as GUID.

Right now collision avoidance for more than one FMU without Hardware GUID is not possible.
We should consider adding a randomly generated HW GUID as a placeholder for legacy Boards
  • Loading branch information
@LowOrbitIonCannon @dagar
LowOrbitIonCannon authored and dagar committed Jan 17, 2020
1 parent 8d0402f commit 7dd949e
Show file tree
Hide file tree
Showing 5 changed files with 209 additions and 66 deletions.
25 changes: 25 additions & 0 deletions msg/transponder_report.msg
View file
Original file line number Diff line number Diff line change
Expand Up @@ -12,6 +12,7 @@ uint8 emitter_type # Type from ADSB_EMITTER_TYPE enum
uint8 tslc # Time since last communication in seconds
uint16 flags # Flags to indicate various statuses including valid data fields
uint16 squawk # Squawk code
uint8[18] uas_id # Unique UAS ID

# ADSB flags
uint16 PX4_ADSB_FLAGS_VALID_COORDS = 1
Expand All @@ -22,4 +23,28 @@ uint16 PX4_ADSB_FLAGS_VALID_CALLSIGN = 16
uint16 PX4_ADSB_FLAGS_VALID_SQUAWK = 32
uint16 PX4_ADSB_FLAGS_RETRANSLATE = 256

#ADSB Emitter Data:
#from mavlink/v2.0/common/common.h
uint16 ADSB_EMITTER_TYPE_NO_INFO=0
uint16 ADSB_EMITTER_TYPE_LIGHT=1
uint16 ADSB_EMITTER_TYPE_SMALL=2
uint16 ADSB_EMITTER_TYPE_LARGE=3
uint16 ADSB_EMITTER_TYPE_HIGH_VORTEX_LARGE=4
uint16 ADSB_EMITTER_TYPE_HEAVY=5
uint16 ADSB_EMITTER_TYPE_HIGHLY_MANUV=6
uint16 ADSB_EMITTER_TYPE_ROTOCRAFT=7
uint16 ADSB_EMITTER_TYPE_UNASSIGNED=8
uint16 ADSB_EMITTER_TYPE_GLIDER=9
uint16 ADSB_EMITTER_TYPE_LIGHTER_AIR=10
uint16 ADSB_EMITTER_TYPE_PARACHUTE=11
uint16 ADSB_EMITTER_TYPE_ULTRA_LIGHT=12
uint16 ADSB_EMITTER_TYPE_UNASSIGNED2=13
uint16 ADSB_EMITTER_TYPE_UAV=14
uint16 ADSB_EMITTER_TYPE_SPACE=15
uint16 ADSB_EMITTER_TYPE_UNASSGINED3=16
uint16 ADSB_EMITTER_TYPE_EMERGENCY_SURFACE=17
uint16 ADSB_EMITTER_TYPE_SERVICE_SURFACE=18
uint16 ADSB_EMITTER_TYPE_POINT_OBSTACLE=19
uint16 ADSB_EMITTER_TYPE_ENUM_END=20

uint8 ORB_QUEUE_LENGTH = 10
111 changes: 62 additions & 49 deletions src/modules/mavlink/mavlink_receiver.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -2299,63 +2299,76 @@ MavlinkReceiver::handle_message_utm_global_position(mavlink_message_t *msg)
mavlink_utm_global_position_t utm_pos;
mavlink_msg_utm_global_position_decode(msg, &utm_pos);

// Convert cm/s to m/s
float vx = utm_pos.vx / 100.0f;
float vy = utm_pos.vy / 100.0f;
float vz = utm_pos.vz / 100.0f;

transponder_report_s t{};
t.timestamp = hrt_absolute_time();
// TODO: ID
t.lat = utm_pos.lat * 1e-7;
t.lon = utm_pos.lon * 1e-7;
t.altitude = utm_pos.alt / 1000.0f;
t.altitude_type = ADSB_ALTITUDE_TYPE_GEOMETRIC;
// UTM_GLOBAL_POSIION uses NED (north, east, down) coordinates for velocity, in cm / s.
t.heading = atan2f(vy, vx);
t.hor_velocity = sqrtf(vy * vy + vx * vx);
t.ver_velocity = -vz;
// TODO: Callsign
// For now, set it to all 0s. This is a null-terminated string, so not explicitly giving it a null
// terminator could cause problems.
memset(&t.callsign[0], 0, sizeof(t.callsign));
t.emitter_type = ADSB_EMITTER_TYPE_NO_INFO; // TODO: Is this correct?

// The Mavlink docs do not specify what to do if tslc (time since last communication) is out of range of
// an 8-bit int, or if this is the first communication.
// Here, I assume that if this is the first communication, tslc = 0.
// If tslc > 255, then tslc = 255.
unsigned long time_passed = (t.timestamp - _last_utm_global_pos_com) / 1000000;
px4_guid_t px4_guid;
#ifndef BOARD_HAS_NO_UUID
board_get_px4_guid(px4_guid);
#else
// TODO Fill ID with something reasonable
memset(&px4_guid[0], 0, sizeof(px4_guid));
#endif /* BOARD_HAS_NO_UUID */


//Ignore selfpublished UTM messages
if (sizeof(px4_guid) == sizeof(utm_pos.uas_id) && memcmp(px4_guid, utm_pos.uas_id, sizeof(px4_guid_t)) != 0) {

// Convert cm/s to m/s
float vx = utm_pos.vx / 100.0f;
float vy = utm_pos.vy / 100.0f;
float vz = utm_pos.vz / 100.0f;

transponder_report_s t{};
t.timestamp = hrt_absolute_time();
mav_array_memcpy(t.uas_id, utm_pos.uas_id, sizeof(px4_guid_t));
t.lat = utm_pos.lat * 1e-7;
t.lon = utm_pos.lon * 1e-7;
t.altitude = utm_pos.alt / 1000.0f;
t.altitude_type = ADSB_ALTITUDE_TYPE_GEOMETRIC;
// UTM_GLOBAL_POSIION uses NED (north, east, down) coordinates for velocity, in cm / s.
t.heading = atan2f(vy, vx);
t.hor_velocity = sqrtf(vy * vy + vx * vx);
t.ver_velocity = -vz;
// TODO: Callsign
// For now, set it to all 0s. This is a null-terminated string, so not explicitly giving it a null
// terminator could cause problems.
memset(&t.callsign[0], 0, sizeof(t.callsign));
t.emitter_type = ADSB_EMITTER_TYPE_UAV; // TODO: Is this correct?x2?

// The Mavlink docs do not specify what to do if tslc (time since last communication) is out of range of
// an 8-bit int, or if this is the first communication.
// Here, I assume that if this is the first communication, tslc = 0.
// If tslc > 255, then tslc = 255.
unsigned long time_passed = (t.timestamp - _last_utm_global_pos_com) / 1000000;

if (_last_utm_global_pos_com == 0) {
time_passed = 0;

} else if (time_passed > UINT8_MAX) {
time_passed = UINT8_MAX;
}

if (_last_utm_global_pos_com == 0) {
time_passed = 0;
t.tslc = (uint8_t) time_passed;

} else if (time_passed > UINT8_MAX) {
time_passed = UINT8_MAX;
}
t.flags = 0;

t.tslc = (uint8_t) time_passed;
if (utm_pos.flags & UTM_DATA_AVAIL_FLAGS_POSITION_AVAILABLE) {
t.flags |= transponder_report_s::PX4_ADSB_FLAGS_VALID_COORDS;
}

t.flags = 0;
if (utm_pos.flags & UTM_DATA_AVAIL_FLAGS_ALTITUDE_AVAILABLE) {
t.flags |= transponder_report_s::PX4_ADSB_FLAGS_VALID_ALTITUDE;
}

if (utm_pos.flags & UTM_DATA_AVAIL_FLAGS_POSITION_AVAILABLE) {
t.flags |= transponder_report_s::PX4_ADSB_FLAGS_VALID_COORDS;
}
if (utm_pos.flags & UTM_DATA_AVAIL_FLAGS_HORIZONTAL_VELO_AVAILABLE) {
t.flags |= transponder_report_s::PX4_ADSB_FLAGS_VALID_HEADING;
t.flags |= transponder_report_s::PX4_ADSB_FLAGS_VALID_VELOCITY;
}

if (utm_pos.flags & UTM_DATA_AVAIL_FLAGS_ALTITUDE_AVAILABLE) {
t.flags |= transponder_report_s::PX4_ADSB_FLAGS_VALID_ALTITUDE;
}
// Note: t.flags has deliberately NOT set VALID_CALLSIGN or VALID_SQUAWK, because UTM_GLOBAL_POSITION does not
// provide these.
_transponder_report_pub.publish(t);

if (utm_pos.flags & UTM_DATA_AVAIL_FLAGS_HORIZONTAL_VELO_AVAILABLE) {
t.flags |= transponder_report_s::PX4_ADSB_FLAGS_VALID_HEADING;
t.flags |= transponder_report_s::PX4_ADSB_FLAGS_VALID_VELOCITY;
_last_utm_global_pos_com = t.timestamp;
}

// Note: t.flags has deliberately NOT set VALID_CALLSIGN or VALID_SQUAWK, because UTM_GLOBAL_POSITION does not
// provide these.
_transponder_report_pub.publish(t);

_last_utm_global_pos_com = t.timestamp;
}

void
Expand Down
6 changes: 5 additions & 1 deletion src/modules/navigator/navigator.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -129,9 +129,10 @@ class Navigator : public ModuleBase<Navigator>, public ModuleParams
* @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);
float hor_velocity, float ver_velocity, int emitter_type);

/**
* Check nearby traffic for potential collisions
Expand Down Expand Up @@ -258,6 +259,7 @@ class Navigator : public ModuleBase<Navigator>, public ModuleParams
*/
float get_yaw_acceptance(float mission_item_yaw);


orb_advert_t *get_mavlink_log_pub() { return &_mavlink_log_pub; }

void increment_mission_instance_count() { _mission_result.instance_count++; }
Expand Down Expand Up @@ -305,6 +307,8 @@ class Navigator : public ModuleBase<Navigator>, public ModuleParams
_param_nav_mc_alt_rad, /**< acceptance radius for multicopter altitude */
(ParamInt<px4::params::NAV_FORCE_VT>) _param_nav_force_vt, /**< acceptance radius for multicopter altitude */
(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*/

// non-navigator parameters
// Mission (MIS_*)
Expand Down
103 changes: 87 additions & 16 deletions src/modules/navigator/navigator_main.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -899,7 +899,7 @@ Navigator::load_fence_from_file(const char *filename)
*
*/
void Navigator::fake_traffic(const char *callsign, float distance, float direction, float traffic_heading,
float altitude_diff, float hor_velocity, float ver_velocity)
float altitude_diff, float hor_velocity, float ver_velocity, int emitter_type)
{
double lat, lon;
waypoint_from_heading_and_distance(get_global_position()->lat, get_global_position()->lon, direction, distance, &lat,
Expand All @@ -922,14 +922,30 @@ void Navigator::fake_traffic(const char *callsign, float distance, float directi
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 = 0; // Type from ADSB_EMITTER_TYPE enum
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::PX4_ADSB_FLAGS_VALID_CALLSIGN; // Flags to indicate various statuses including valid data fields
(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 */

uORB::PublicationQueued<transponder_report_s> tr_pub{ORB_ID(transponder_report)};
tr_pub.publish(tr);
}
Expand All @@ -948,11 +964,16 @@ void Navigator::check_traffic()

bool changed = _traffic_sub.updated();

float horizontal_separation = 500;
float vertical_separation = 500;
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);

Expand All @@ -965,6 +986,17 @@ void Navigator::check_traffic()
continue;
}

//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]);
}

//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);
Expand Down Expand Up @@ -999,27 +1031,33 @@ void Navigator::check_traffic()

// 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, hdg: %d", tr.flags & transponder_report_s::PX4_ADSB_FLAGS_VALID_CALLSIGN ? tr.callsign :
"unknown",
/* 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: {
mavlink_log_critical(&_mavlink_log_pub, "WARNING TRAFFIC %s at heading %d, land immediately",
tr.flags & transponder_report_s::PX4_ADSB_FLAGS_VALID_CALLSIGN ? tr.callsign : "unknown",
/* Warn only */
mavlink_log_critical(&_mavlink_log_pub, "Warning 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 2: {
mavlink_log_critical(&_mavlink_log_pub, "AVOIDING TRAFFIC %s heading %d, returning home",
tr.flags & transponder_report_s::PX4_ADSB_FLAGS_VALID_CALLSIGN ? tr.callsign : "unknown",
/* RTL Mode */
mavlink_log_critical(&_mavlink_log_pub, "TRAFFIC: %s Returning home! dst %d, hdg %d",
tr.flags & transponder_report_s::PX4_ADSB_FLAGS_VALID_CALLSIGN ? tr.callsign : uas_id,
traffic_seperation,
traffic_direction);

// set the return altitude to minimum
Expand All @@ -1031,6 +1069,36 @@ void Navigator::check_traffic()
publish_vehicle_cmd(&vcmd);
break;
}

case 3: {
/* Land Mode */
mavlink_log_critical(&_mavlink_log_pub, "TRAFFIC: %s Landing! dst %d, hdg % d",
tr.flags & transponder_report_s::PX4_ADSB_FLAGS_VALID_CALLSIGN ? tr.callsign : uas_id,
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",
tr.flags & transponder_report_s::PX4_ADSB_FLAGS_VALID_CALLSIGN ? tr.callsign : uas_id,
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;

}
}
}
}
Expand Down Expand Up @@ -1084,9 +1152,12 @@ 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);
get_instance()->fake_traffic("LX55", 1000, 0, 0, 100.0f, 90.0f, 0.001f);
get_instance()->fake_traffic("LX20", 15000, 1.0f, -1.0f, 280.0f, 90.0f, 0.001f);
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);
return 0;
}

Expand Down Expand Up @@ -1195,7 +1266,7 @@ controller.
PRINT_MODULE_USAGE_NAME("navigator", "controller");
PRINT_MODULE_USAGE_COMMAND("start");
PRINT_MODULE_USAGE_COMMAND_DESCR("fencefile", "load a geofence file from SD card, stored at etc/geofence.txt");
PRINT_MODULE_USAGE_COMMAND_DESCR("fake_traffic", "publishes 3 fake transponder_report_s uORB messages");
PRINT_MODULE_USAGE_COMMAND_DESCR("fake_traffic", "publishes 4 fake transponder_report_s uORB messages");
PRINT_MODULE_USAGE_DEFAULT_COMMANDS();

return 0;
Expand Down
Loading

0 comments on commit 7dd949e

Please sign in to comment.