Navigator: Add the ability to do a low-altitude RTL to avoid traffic

This is a first baseline implementation for collision avoidance with aircraft equipped with transponders. Since the drone-to-drone and drone-to-airplane system will be regulated by a standard, we do carefully avoid to do anything too fancy. This implementation allows to reduce altitude to get close to ground and land immediately or RTL. This is similar to what a human pilot would do and allows to get out of manned airspace which is at higher altitudes.

msg/transponder_report.msg

@@ -3,7 +3,7 @@ float64 lat 		# Latitude, expressed as degrees
 float64 lon 		# Longitude, expressed as degrees
 uint8 altitude_type	# Type from ADSB_ALTITUDE_TYPE enum
 float32 altitude 	# Altitude(ASL) in meters
-float32 heading 	# Course over ground in radians
+float32 heading 	# Course over ground in radians, -pi to +pi, 0 is north
 float32 hor_velocity	# The horizontal velocity in m/s
 float32 ver_velocity 	# The vertical velocity in m/s, positive is up
 char[9] callsign	# The callsign, 8+null

src/modules/navigator/navigator.h

@@ -36,6 +36,7 @@
  * @author Julian Oes <julian@oes.ch>
  * @author Anton Babushkin <anton.babushkin@me.com>
  * @author Thomas Gubler <thomasgubler@gmail.com>
+ * @author Lorenz Meier <lorenz@px4.io>
  */
 
 #ifndef NAVIGATOR_H
@@ -108,6 +109,24 @@ 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
+	 */
+	void		fake_traffic(const char *callsign, float distance, float direction, float traffic_heading, float altitude_diff,
+				     float hor_velocity, float ver_velocity);
+
+	/**
+	 * Check nearby traffic for potential collisions
+	 */
+	void		check_traffic();
+
 	/**
 	 * Setters
 	 */
@@ -242,7 +261,8 @@ private:
 	int		_param_update_sub{-1};		/**< param update subscription */
 	int		_sensor_combined_sub{-1};	/**< sensor combined subscription */
 	int		_vehicle_command_sub{-1};	/**< vehicle commands (onboard and offboard) */
-	int		_vstatus_sub{-1};		/**< vehicle status subscription */
+	int		_vstatus_sub{-1};			/**< vehicle status subscription */
+	int		_traffic_sub{-1};			/**< traffic subscription */
 
 	orb_advert_t	_geofence_result_pub{nullptr};
 	orb_advert_t	_mavlink_log_pub{nullptr};	/**< the uORB advert to send messages over mavlink */
@@ -302,6 +322,7 @@ private:
 	control::BlockParamFloat _param_fw_alt_acceptance_radius;	/**< acceptance radius for fixedwing altitude */
 	control::BlockParamFloat _param_mc_alt_acceptance_radius;	/**< acceptance radius for multicopter altitude */
 	control::BlockParamInt _param_force_vtol;	/**< acceptance radius for multicopter altitude */
+	control::BlockParamInt _param_traffic_avoidance_mode;	/**< avoiding other aircraft is enabled */
 
 	// non-navigator parameters
 	control::BlockParamFloat _param_loiter_min_alt;

src/modules/navigator/navigator_main.cpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2013-2016 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2013-2017 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
@@ -36,7 +36,7 @@
  * Handles mission items, geo fencing and failsafe navigation behavior.
  * Published the position setpoint triplet for the position controller.
  *
- * @author Lorenz Meier <lm@inf.ethz.ch>
+ * @author Lorenz Meier <lorenz@px4.io>
  * @author Jean Cyr <jean.m.cyr@gmail.com>
  * @author Julian Oes <julian@oes.ch>
  * @author Anton Babushkin <anton.babushkin@me.com>
@@ -66,6 +66,7 @@
 #include <uORB/topics/vehicle_command.h>
 #include <uORB/topics/vehicle_command_ack.h>
 #include <uORB/topics/vehicle_status.h>
+#include <uORB/topics/transponder_report.h>
 #include <uORB/uORB.h>
 
 /**
@@ -102,6 +103,7 @@ Navigator::Navigator() :
 	_param_fw_alt_acceptance_radius(this, "FW_ALT_RAD"),
 	_param_mc_alt_acceptance_radius(this, "MC_ALT_RAD"),
 	_param_force_vtol(this, "FORCE_VT"),
+	_param_traffic_avoidance_mode(this, "TRAFF_AVOID"),
 	// non-navigator params
 	_param_loiter_min_alt(this, "MIS_LTRMIN_ALT", false)
 {
@@ -251,6 +253,7 @@ Navigator::task_main()
 	_offboard_mission_sub = orb_subscribe(ORB_ID(offboard_mission));
 	_param_update_sub = orb_subscribe(ORB_ID(parameter_update));
 	_vehicle_command_sub = orb_subscribe(ORB_ID(vehicle_command));
+	_traffic_sub = orb_subscribe(ORB_ID(transponder_report));
 
 	/* copy all topics first time */
 	vehicle_status_update();
@@ -566,6 +569,9 @@ Navigator::task_main()
 			}
 		}
 
+		/* Check for traffic */
+		check_traffic();
+
 		/* Check geofence violation */
 		static hrt_abstime last_geofence_check = 0;
 
@@ -882,6 +888,135 @@ Navigator::load_fence_from_file(const char *filename)
 	_geofence.loadFromFile(filename);
 }
 
+/**
+ * Creates a fake traffic measurement with supplied parameters.
+ *
+ */
+void Navigator::fake_traffic(const char *callsign, float distance, float direction, float traffic_heading,
+			     float altitude_diff, float hor_velocity, float ver_velocity)
+{
+	double lat, lon;
+	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;
+
+	// float vel_n = get_global_position()->vel_n;
+	// float vel_e = get_global_position()->vel_e;
+	// float vel_d = get_global_position()->vel_d;
+
+	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
+	strcpy(&tr.callsign[0], callsign);
+	tr.emitter_type = 0; // Type from ADSB_EMITTER_TYPE enum
+	tr.tslc = 2; // Time since last communication in seconds
+	tr.flags = 0; // Flags to indicate various statuses including valid data fields
+	tr.squawk = 6667;
+
+	orb_advert_t h = orb_advertise_queue(ORB_ID(transponder_report), &tr, transponder_report_s::ORB_QUEUE_LENGTH);
+	(void)orb_unadvertise(h);
+}
+
+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;
+
+	bool changed;
+	orb_check(_traffic_sub, &changed);
+
+	float horizontal_separation = 500;
+	float vertical_separation = 500;
+
+	while (changed) {
+
+		transponder_report_s tr;
+		orb_copy(ORB_ID(transponder_report), _traffic_sub, &tr);
+
+		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)) {
+
+					// direction of traffic in human-readable 0..360 degree in earth frame
+					int traffic_direction = math::degrees(tr.heading) + 180;
+
+					switch (_param_traffic_avoidance_mode.get()) {
+
+					case 0: {
+							/* ignore */
+							PX4_WARN("TRAFFIC %s, hdg: %d", tr.callsign, traffic_direction);
+							break;
+						}
+
+					case 1: {
+							mavlink_log_critical(&_mavlink_log_pub, "WARNING TRAFFIC %s at heading %d, land immediately", tr.callsign,
+									     traffic_direction);
+							break;
+						}
+
+					case 2: {
+							mavlink_log_critical(&_mavlink_log_pub, "AVOIDING TRAFFIC %s heading %d, returning home", tr.callsign,
+									     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;
+						}
+					}
+				}
+			}
+		}
+
+		orb_check(_traffic_sub, &changed);
+	}
+}
+
 bool
 Navigator::abort_landing()
 {
@@ -952,6 +1087,11 @@ int navigator_main(int argc, char *argv[])
 	} else if (!strcmp(argv[1], "fencefile")) {
 		navigator::g_navigator->load_fence_from_file(GEOFENCE_FILENAME);
 
+	} else if (!strcmp(argv[1], "fake_traffic")) {
+		navigator::g_navigator->fake_traffic("LX007", 500, 1.0f, -1.0f, 100.0f, 90.0f, 0.001f);
+		navigator::g_navigator->fake_traffic("LX55", 1000, 0, 0, 100.0f, 90.0f, 0.001f);
+		navigator::g_navigator->fake_traffic("LX20", 15000, 1.0f, -1.0f, 280.0f, 90.0f, 0.001f);
+
 	} else {
 		usage();
 		return 1;

src/modules/navigator/navigator_params.c

@@ -139,6 +139,21 @@ PARAM_DEFINE_INT32(NAV_DLL_ACT, 0);
  */
 PARAM_DEFINE_INT32(NAV_RCL_ACT, 2);
 
+/**
+ * Set traffic avoidance mode
+ *
+ * Enabling this will allow the system to respond
+ * to transponder data from e.g. ADSB transponders
+ *
+ * @value 0 Disabled
+ * @value 1 Warn only
+ * @value 2 Return to Land
+ * @value 3 Land immediately
+ *
+ * @group Mission
+ */
+PARAM_DEFINE_INT32(NAV_TRAFF_AVOID, 1)
+
 /**
  * Airfield home Lat
  *

src/modules/navigator/rtl.cpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2013-2016 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2013-2017 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
@@ -92,7 +92,8 @@ RTL::on_activation()
 	if (_navigator->get_land_detected()->landed) {
 		_rtl_state = RTL_STATE_LANDED;
 
-	} else if (_navigator->get_global_position()->alt < (_navigator->get_home_position()->alt + get_rtl_altitude())) {
+	} else if ((_rtl_alt_min
+		    || _navigator->get_global_position()->alt < (_navigator->get_home_position()->alt + get_rtl_altitude()))) {
 
 		/* if lower than return altitude, climb up first */
 		_rtl_state = RTL_STATE_CLIMB;
@@ -118,6 +119,12 @@ RTL::on_active()
 	}
 }
 
+void
+RTL::set_return_alt_min(bool min)
+{
+	_rtl_alt_min = min;
+}
+
 void
 RTL::set_rtl_item()
 {
@@ -276,6 +283,7 @@ RTL::set_rtl_item()
 
 	case RTL_STATE_LANDED: {
 			set_idle_item(&_mission_item);
+			set_return_alt_min(false);
 			break;
 		}
 

src/modules/navigator/rtl.h

@@ -1,6 +1,6 @@
 /***************************************************************************
  *
- *   Copyright (c) 2013-2014 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2013-2017 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
@@ -63,6 +63,8 @@ public:
 	void on_activation() override;
 	void on_active() override;
 
+	void set_return_alt_min(bool min);
+
 private:
 	/**
 	 * Set the RTL item
@@ -91,6 +93,8 @@ private:
 		RTL_STATE_LANDED,
 	} _rtl_state{RTL_STATE_NONE};
 
+	bool _rtl_alt_min{false};
+
 	control::BlockParamFloat _param_return_alt;
 	control::BlockParamFloat _param_descend_alt;
 	control::BlockParamFloat _param_land_delay;