From a1a002f9b6d4714249c3e327dc8e827230c091f7 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 30 Oct 2017 22:41:06 +0100 Subject: [PATCH] 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 | 2 +- src/modules/navigator/navigator.h | 23 +++- src/modules/navigator/navigator_main.cpp | 144 ++++++++++++++++++++++- src/modules/navigator/navigator_params.c | 15 +++ src/modules/navigator/rtl.cpp | 12 +- src/modules/navigator/rtl.h | 6 +- 6 files changed, 195 insertions(+), 7 deletions(-) diff --git a/msg/transponder_report.msg b/msg/transponder_report.msg index fb249c58a5..477214d3f4 100644 --- a/msg/transponder_report.msg +++ b/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 diff --git a/src/modules/navigator/navigator.h b/src/modules/navigator/navigator.h index f39606b045..5c701d64c1 100644 --- a/src/modules/navigator/navigator.h +++ b/src/modules/navigator/navigator.h @@ -36,6 +36,7 @@ * @author Julian Oes * @author Anton Babushkin * @author Thomas Gubler + * @author Lorenz Meier */ #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; diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 338c10745b..daf03b8c66 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/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 + * @author Lorenz Meier * @author Jean Cyr * @author Julian Oes * @author Anton Babushkin @@ -66,6 +66,7 @@ #include #include #include +#include #include /** @@ -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; diff --git a/src/modules/navigator/navigator_params.c b/src/modules/navigator/navigator_params.c index 904833c057..f5aa272fff 100644 --- a/src/modules/navigator/navigator_params.c +++ b/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 * diff --git a/src/modules/navigator/rtl.cpp b/src/modules/navigator/rtl.cpp index ef59d21f1d..d9814e0c29 100644 --- a/src/modules/navigator/rtl.cpp +++ b/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; } diff --git a/src/modules/navigator/rtl.h b/src/modules/navigator/rtl.h index 78163828b8..2e233eef2c 100644 --- a/src/modules/navigator/rtl.h +++ b/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;