From 7dd949edb18af0fab812f49a7271e8122d46c75d Mon Sep 17 00:00:00 2001 From: Low Orbit Ion Cannon Date: Fri, 17 Jan 2020 20:58:28 +0100 Subject: [PATCH] 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 | 25 +++++ src/modules/mavlink/mavlink_receiver.cpp | 117 +++++++++++++---------- src/modules/navigator/navigator.h | 6 +- src/modules/navigator/navigator_main.cpp | 103 ++++++++++++++++---- src/modules/navigator/navigator_params.c | 30 ++++++ 5 files changed, 212 insertions(+), 69 deletions(-) diff --git a/msg/transponder_report.msg b/msg/transponder_report.msg index 99d2a332b3..32a5ef5850 100644 --- a/msg/transponder_report.msg +++ b/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 diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index ec3ddf3a31..936424c776 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/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 - float vx = utm_pos.vx / 100.0f; - float vy = utm_pos.vy / 100.0f; - float vz = utm_pos.vz / 100.0f; + 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 */ - 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; + //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) { - if (_last_utm_global_pos_com == 0) { - time_passed = 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; - } else if (time_passed > UINT8_MAX) { - time_passed = UINT8_MAX; + 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; + } + + 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 diff --git a/src/modules/navigator/navigator.h b/src/modules/navigator/navigator.h index 77d593ab4e..705e16c73b 100644 --- a/src/modules/navigator/navigator.h +++ b/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) _param_nav_force_vt, /**< acceptance radius for multicopter altitude */ (ParamInt) _param_nav_traff_avoid, /**< avoiding other aircraft is enabled */ + (ParamFloat) _param_nav_traff_a_radu, /**< avoidance Distance Unmanned*/ + (ParamFloat) _param_nav_traff_a_radm, /**< avoidance Distance Manned*/ // non-navigator parameters // Mission (MIS_*) diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 0b6fa51a1b..d0cb52a0d4 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/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 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; - 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); @@ -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 */ - 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 @@ -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("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; } @@ -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; diff --git a/src/modules/navigator/navigator_params.c b/src/modules/navigator/navigator_params.c index a1e042c46d..f5bdcde8b3 100644 --- a/src/modules/navigator/navigator_params.c +++ b/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); + +