Treat UAVS diffrently from manned aviation

src/modules/mavlink/mavlink_receiver.cpp

@@ -2229,7 +2229,7 @@ MavlinkReceiver::handle_message_utm_global_position(mavlink_message_t *msg)
 	// 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?
+	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.

src/modules/navigator/CMakeLists.txt

@@ -34,6 +34,10 @@
 px4_add_module(
 	MODULE modules__navigator
 	MAIN navigator
+	COMPILE_FLAGS
+		-Wno-cast-align # TODO: fix and enable
+	INCLUDES
+		${PX4_SOURCE_DIR}/mavlink/include/mavlink
 	SRCS
 		navigator_main.cpp
 		navigator_mode.cpp

src/modules/navigator/navigator.h

@@ -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
@@ -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++; }
@@ -301,6 +303,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_*)

src/modules/navigator/navigator_main.cpp

@@ -57,6 +57,7 @@
 #include <px4_posix.h>
 #include <px4_tasks.h>
 #include <systemlib/mavlink_log.h>
+#include <v2.0/common/mavlink.h>
 
 /**
  * navigator app start / stop handling function
@@ -894,7 +895,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,
@@ -917,7 +918,7 @@ 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 |
@@ -943,8 +944,11 @@ void Navigator::check_traffic()
 
 	bool changed = _traffic_sub.updated();
 
-	float horizontal_separation = 500;
-	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) {
 
@@ -960,6 +964,13 @@ void Navigator::check_traffic()
 			continue;
 		}
 
+
+		//Manned/Unmanned Vehicle Seperation Distance
+		if (tr.emitter_type == 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);
@@ -994,28 +1005,37 @@ void Navigator::check_traffic()
 
 					// direction of traffic in human-readable 0..360 degree in earth frame
 					int traffic_direction = math::degrees(tr.heading) + 180;
+					double traffic_seperation = 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 :
+							PX4_WARN("TRAFFIC %s, hdg: %d, hrzl dst:: %f, type: %d",
+								 tr.flags & transponder_report_s::PX4_ADSB_FLAGS_VALID_CALLSIGN ? tr.callsign :
 								 "unknown",
-								 traffic_direction);
+								 traffic_direction,
+								 traffic_seperation,
+								 tr.emitter_type);
 							break;
 						}
 
 					case 1: {
-							mavlink_log_critical(&_mavlink_log_pub, "WARNING TRAFFIC %s at heading %d, land immediately",
+							/*Warn only*/
+							mavlink_log_critical(&_mavlink_log_pub, "WARNING TRAFFIC %s at heading %d, hrzl dst: %f, type: %d",
 									     tr.flags & transponder_report_s::PX4_ADSB_FLAGS_VALID_CALLSIGN ? tr.callsign : "unknown",
-									     traffic_direction);
+									     traffic_direction,
+									     traffic_seperation,
+									     tr.emitter_type);
 							break;
 						}
 
 					case 2: {
-							mavlink_log_critical(&_mavlink_log_pub, "AVOIDING TRAFFIC %s heading %d, returning home",
+							mavlink_log_critical(&_mavlink_log_pub, "AVOIDING TRAFFIC %s heading %d, returning home! hrzl dst: %f, type: %d",
 									     tr.flags & transponder_report_s::PX4_ADSB_FLAGS_VALID_CALLSIGN ? tr.callsign : "unknown",
-									     traffic_direction);
+									     traffic_direction,
+									     traffic_seperation,
+									     tr.emitter_type);
 
 							// set the return altitude to minimum
 							_rtl.set_return_alt_min(true);
@@ -1079,9 +1099,10 @@ 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, ADSB_EMITTER_TYPE_LIGHT);
+		get_instance()->fake_traffic("LX55", 1000, 0, 0, 100.0f, 90.0f, 0.001f, ADSB_EMITTER_TYPE_SMALL);
+		get_instance()->fake_traffic("LX20", 15000, 1.0f, -1.0f, 280.0f, 90.0f, 0.001f, ADSB_EMITTER_TYPE_LARGE);
+		get_instance()->fake_traffic("DRONE", 10, 1.0f, -1.0f, 10.0f, 10.0f, 0.001f, ADSB_EMITTER_TYPE_UAV);
 		return 0;
 	}
 
@@ -1190,7 +1211,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

@@ -120,12 +120,38 @@ PARAM_DEFINE_FLOAT(NAV_MC_ALT_RAD, 0.8f);
  * @value 0 Disabled
  * @value 1 Warn only
  * @value 2 Return mode
- * @value 3 Land 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 +196,5 @@ PARAM_DEFINE_FLOAT(NAV_AH_ALT, 600.0f);
  * @group Mission
  */
 PARAM_DEFINE_INT32(NAV_FORCE_VT, 1);
+
+