ADSB traffic avoidance improvements using UTM_GLOBAL_POSITION (#13159)

* 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

msg/transponder_report.msg

@@ -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
@@ -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

src/modules/mavlink/mavlink_receiver.cpp

@@ -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
+	px4_guid_t px4_guid;
-	float vx = utm_pos.vx / 100.0f;
+#ifndef BOARD_HAS_NO_UUID
-	float vy = utm_pos.vy / 100.0f;
+	board_get_px4_guid(px4_guid);
-	float vz = utm_pos.vz / 100.0f;
+#else
+	// TODO Fill ID with something reasonable
+	memset(&px4_guid[0], 0, sizeof(px4_guid));
+#endif /* BOARD_HAS_NO_UUID */
 
-	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
+	//Ignore selfpublished UTM messages
-	// an 8-bit int, or if this is the first communication.
+	if (sizeof(px4_guid) == sizeof(utm_pos.uas_id) && memcmp(px4_guid, utm_pos.uas_id, sizeof(px4_guid_t)) != 0) {
-	// 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) {
+		// Convert cm/s to m/s
-		time_passed = 0;
+		float vx = utm_pos.vx / 100.0f;
+		float vy = utm_pos.vy / 100.0f;
+		float vz = utm_pos.vz / 100.0f;
 
-	} else if (time_passed > UINT8_MAX) {
+		transponder_report_s t{};
-		time_passed = UINT8_MAX;
+		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;
+		}
+
+		t.tslc = (uint8_t) time_passed;
+
+		t.flags = 0;
+
+		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_ALTITUDE_AVAILABLE) {
+			t.flags |= transponder_report_s::PX4_ADSB_FLAGS_VALID_ALTITUDE;
+		}
+
+		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;
+		}
+
+		// 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;
 	}
-
-	t.tslc = (uint8_t) time_passed;
-
-	t.flags = 0;
-
-	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_ALTITUDE_AVAILABLE) {
-		t.flags |= transponder_report_s::PX4_ADSB_FLAGS_VALID_ALTITUDE;
-	}
-
-	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;
-	}
-
-	// 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

src/modules/navigator/navigator.h

@@ -129,9 +129,10 @@ public:
 	 * @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
@@ -258,6 +259,7 @@ public:
 	 */
 	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++; }
@@ -305,6 +307,8 @@ private:
 		_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_*)

src/modules/navigator/navigator_main.cpp

@@ -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,
@@ -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);
 }
@@ -948,11 +964,16 @@ void Navigator::check_traffic()
 
 	bool changed = _traffic_sub.updated();
 
-	float horizontal_separation = 500;
+	char uas_id[11]; //GUID of incoming UTM messages
-	float vertical_separation = 500;
+
+	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);
 
@@ -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);
@@ -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 */
+							/* Ignore */
-							PX4_WARN("TRAFFIC %s, hdg: %d", tr.flags & transponder_report_s::PX4_ADSB_FLAGS_VALID_CALLSIGN ? tr.callsign :
+							PX4_WARN("TRAFFIC %s! dst %d, hdg %d",
-								 "unknown",
+								 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",
+							/* Warn only */
-									     tr.flags & transponder_report_s::PX4_ADSB_FLAGS_VALID_CALLSIGN ? tr.callsign : "unknown",
+							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",
+							/* RTL Mode */
-									     tr.flags & transponder_report_s::PX4_ADSB_FLAGS_VALID_CALLSIGN ? tr.callsign : "unknown",
+							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
@@ -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;
+
+						}
 					}
 				}
 			}
@@ -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("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);
+					     transponder_report_s::ADSB_EMITTER_TYPE_LIGHT);
-		get_instance()->fake_traffic("LX20", 15000, 1.0f, -1.0f, 280.0f, 90.0f, 0.001f);
+		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;
 	}
 
@@ -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;

src/modules/navigator/navigator_params.c

@@ -121,11 +121,39 @@ PARAM_DEFINE_FLOAT(NAV_MC_ALT_RAD, 0.8f);
  * @value 1 Warn only
  * @value 2 Return mode
  * @value 3 Land mode
+ * @value 4 Position Hold mode
  *
  * @group Mission
  */
 PARAM_DEFINE_INT32(NAV_TRAFF_AVOID, 1);
 
+/**
+ * Set NAV TRAFFIC AVOID RADIUS MANNED
+ *
+ * Defines the Radius where NAV TRAFFIC AVOID is Called
+ * For Manned Aviation
+ *
+ * @unit m
+ * @min 500
+ *
+ * @group Mission
+ */
+PARAM_DEFINE_FLOAT(NAV_TRAFF_A_RADM, 500);
+
+/**
+ * Set NAV TRAFFIC AVOID RADIUS
+ *
+ * Defines the Radius where NAV TRAFFIC AVOID is Called
+ * For Unmanned Aviation
+ *
+ * @unit m
+ * @min 10
+ * @max 500
+ *
+ * @group Mission
+ */
+PARAM_DEFINE_FLOAT(NAV_TRAFF_A_RADU, 10);
+
 /**
  * Airfield home Lat
  *
@@ -170,3 +198,5 @@ PARAM_DEFINE_FLOAT(NAV_AH_ALT, 600.0f);
  * @group Mission
  */
 PARAM_DEFINE_INT32(NAV_FORCE_VT, 1);
+
+