diff --git a/src/lib/adsb/AdsbConflict.cpp b/src/lib/adsb/AdsbConflict.cpp index cb40f27a7d..8fa11a8f2a 100644 --- a/src/lib/adsb/AdsbConflict.cpp +++ b/src/lib/adsb/AdsbConflict.cpp @@ -108,12 +108,14 @@ void AdsbConflict::remove_icao_address_from_conflict_list(int traffic_index) { _traffic_buffer.icao_address.remove(traffic_index); _traffic_buffer.timestamp.remove(traffic_index); + PX4_INFO("icao_address removed. Buffer Size: %d", (int)_traffic_buffer.timestamp.size()); } void AdsbConflict::add_icao_address_from_conflict_list(uint32_t icao_address) { _traffic_buffer.timestamp.push_back(hrt_absolute_time()); _traffic_buffer.icao_address.push_back(icao_address); + PX4_INFO("icao_address added. Buffer Size: %d", (int)_traffic_buffer.timestamp.size()); } void AdsbConflict::get_traffic_state() @@ -129,7 +131,6 @@ void AdsbConflict::get_traffic_state() if (old_conflict && _conflict_detected) { old_conflict_warning_expired = (hrt_elapsed_time(&_traffic_buffer.timestamp[traffic_index]) > CONFLICT_WARNING_TIMEOUT); - } if (new_traffic && _conflict_detected && !_traffic_buffer_full) { @@ -154,65 +155,56 @@ void AdsbConflict::get_traffic_state() } +void AdsbConflict::remove_expired_conflicts() +{ + for (uint8_t traffic_index = 0; traffic_index < _traffic_buffer.timestamp.size();) { + if (hrt_elapsed_time(&_traffic_buffer.timestamp[traffic_index]) > TRAFFIC_CONFLICT_LIFETIME) { + events::send(events::ID("navigator_traffic_expired"), events::Log::Notice, + "Traffic Conflict {1} Expired and removed from buffer", + _traffic_buffer.icao_address[traffic_index]); + remove_icao_address_from_conflict_list(traffic_index); + + } else { + traffic_index++; + } + } +} bool AdsbConflict::handle_traffic_conflict() { get_traffic_state(); - char uas_id[UTM_GUID_MSG_LENGTH]; //GUID of incoming UTM messages - - //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", _transponder_report.uas_id[PX4_GUID_BYTE_LENGTH - 5 + i]); - } - - uint64_t uas_id_int = 0; - - for (int i = 0; i < 8; i++) { - uas_id_int |= (uint64_t)(_transponder_report.uas_id[PX4_GUID_BYTE_LENGTH - i - 1]) << (i * 8); - } - - bool take_action = false; switch (_traffic_state) { case TRAFFIC_STATE::ADD_CONFLICT: case TRAFFIC_STATE::REMIND_CONFLICT: { - - take_action = send_traffic_warning(math::degrees(_transponder_report.heading) + 180, - (int)fabsf(_crosstrack_error.distance), _transponder_report.flags, uas_id, + take_action = send_traffic_warning((int)(math::degrees(_transponder_report.heading) + 180.f), + (int)fabsf(_crosstrack_error.distance), _transponder_report.flags, _transponder_report.callsign, - uas_id_int); - + _transponder_report.icao_address); } break; case TRAFFIC_STATE::REMOVE_OLD_CONFLICT: { - - mavlink_log_critical(&_mavlink_log_pub, "TRAFFIC UPDATE: %s is no longer in conflict!", - _transponder_report.flags & transponder_report_s::PX4_ADSB_FLAGS_VALID_CALLSIGN ? - _transponder_report.callsign : uas_id); - - events::send(events::ID("navigator_traffic_resolved"), events::Log::Critical, "Traffic Conflict Resolved", - uas_id_int); - + events::send(events::ID("navigator_traffic_resolved"), events::Log::Notice, + "Traffic Conflict Resolved {1}!", + _transponder_report.icao_address); + _last_traffic_warning_time = hrt_absolute_time(); } break; case TRAFFIC_STATE::BUFFER_FULL: { - if (_traffic_state_previous != TRAFFIC_STATE::BUFFER_FULL) { - PX4_WARN("Too much traffic! Showing all messages from now on"); + if ((_traffic_state_previous != TRAFFIC_STATE::BUFFER_FULL) + && (hrt_elapsed_time(&_last_buffer_full_warning_time) > TRAFFIC_WARNING_TIMESTEP)) { + events::send(events::ID("buffer_full"), events::Log::Notice, "Too much traffic! Showing all messages from now on"); + _last_buffer_full_warning_time = hrt_absolute_time(); } - //stop buffering incoming conflicts - take_action = send_traffic_warning(math::degrees(_transponder_report.heading) + 180, - (int)fabsf(_crosstrack_error.distance), _transponder_report.flags, uas_id, - _transponder_report.callsign, - uas_id_int); - + //disable conflict warnings when buffer is full } break; @@ -242,49 +234,67 @@ void AdsbConflict::set_conflict_detection_params(float crosstrack_separation, fl bool AdsbConflict::send_traffic_warning(int traffic_direction, int traffic_seperation, uint16_t tr_flags, - char uas_id[UTM_GUID_MSG_LENGTH], char tr_callsign[UTM_CALLSIGN_LENGTH], uint64_t uas_id_int) + char tr_callsign[UTM_CALLSIGN_LENGTH], uint32_t icao_address) { switch (_conflict_detection_params.traffic_avoidance_mode) { case 0: { - 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); + + if (tr_flags & transponder_report_s::PX4_ADSB_FLAGS_VALID_CALLSIGN) { + + PX4_WARN("Traffic alert - UTM callsign %s! Separation Distance %d, Heading %d, ICAO Address %d", + tr_callsign, + traffic_seperation, + traffic_direction, (int)icao_address); + + } + + + _last_traffic_warning_time = hrt_absolute_time(); + break; } case 1: { - mavlink_log_critical(&_mavlink_log_pub, "Warning TRAFFIC %s! dst %d, hdg %d\t", - tr_flags & transponder_report_s::PX4_ADSB_FLAGS_VALID_CALLSIGN ? tr_callsign : uas_id, - traffic_seperation, - traffic_direction); + + + if (tr_flags & transponder_report_s::PX4_ADSB_FLAGS_VALID_CALLSIGN) { + + PX4_WARN("Traffic alert - UTM callsign %s! Separation Distance %d, Heading %d, ICAO Address %d", + tr_callsign, + traffic_seperation, + traffic_direction, (int)icao_address); + + } + /* EVENT * @description - * - ID: {1} - * - Distance: {2m} - * - Direction: {3} degrees + * - ICAO Address: {1} + * - Traffic Separation Distance: {2m} + * - Heading: {3} degrees */ - events::send(events::ID("navigator_traffic"), events::Log::Critical, "Traffic alert", - uas_id_int, traffic_seperation, traffic_direction); + events::send(events::ID("navigator_traffic"), events::Log::Notice, + "Traffic alert - ICAO Address {1}! Separation Distance {2}, Heading {3}", + icao_address, traffic_seperation, traffic_direction); + + _last_traffic_warning_time = hrt_absolute_time(); + break; } case 2: { - mavlink_log_critical(&_mavlink_log_pub, "TRAFFIC: %s Returning home! dst %d, hdg %d\t", - tr_flags & transponder_report_s::PX4_ADSB_FLAGS_VALID_CALLSIGN ? tr_callsign : uas_id, - traffic_seperation, - traffic_direction); /* EVENT * @description - * - ID: {1} - * - Distance: {2m} - * - Direction: {3} degrees + * - ICAO Address: {1} + * - Traffic Separation Distance: {2m} + * - Heading: {3} degrees */ - events::send(events::ID("navigator_traffic_rtl"), events::Log::Critical, - "Traffic alert, returning home", - uas_id_int, traffic_seperation, traffic_direction); + events::send(events::ID("navigator_traffic_rtl"), events::Log::Notice, + "Traffic alert - ICAO Address {1}! Separation Distance {2}, Heading {3}, returning home", + icao_address, traffic_seperation, traffic_direction); + + _last_traffic_warning_time = hrt_absolute_time(); return true; @@ -292,19 +302,17 @@ bool AdsbConflict::send_traffic_warning(int traffic_direction, int traffic_seper } case 3: { - mavlink_log_critical(&_mavlink_log_pub, "TRAFFIC: %s Landing! dst %d, hdg % d\t", - tr_flags & transponder_report_s::PX4_ADSB_FLAGS_VALID_CALLSIGN ? tr_callsign : uas_id, - traffic_seperation, - traffic_direction); /* EVENT * @description - * - ID: {1} - * - Distance: {2m} - * - Direction: {3} degrees + * - ICAO Address: {1} + * - Traffic Separation Distance: {2m} + * - Heading: {3} degrees */ - events::send(events::ID("navigator_traffic_land"), events::Log::Critical, - "Traffic alert, landing", - uas_id_int, traffic_seperation, traffic_direction); + events::send(events::ID("navigator_traffic_land"), events::Log::Notice, + "Traffic alert - ICAO Address {1}! Separation Distance {2}, Heading {3}, landing", + icao_address, traffic_seperation, traffic_direction); + + _last_traffic_warning_time = hrt_absolute_time(); return true; @@ -313,19 +321,18 @@ bool AdsbConflict::send_traffic_warning(int traffic_direction, int traffic_seper } case 4: { - mavlink_log_critical(&_mavlink_log_pub, "TRAFFIC: %s Holding position! dst %d, hdg %d\t", - tr_flags & transponder_report_s::PX4_ADSB_FLAGS_VALID_CALLSIGN ? tr_callsign : uas_id, - traffic_seperation, - traffic_direction); /* EVENT * @description - * - ID: {1} - * - Distance: {2m} - * - Direction: {3} degrees + * - ICAO Address: {1} + * - Traffic Separation Distance: {2m} + * - Heading: {3} degrees */ - events::send(events::ID("navigator_traffic_hold"), events::Log::Critical, - "Traffic alert, holding position", - uas_id_int, traffic_seperation, traffic_direction); + events::send(events::ID("navigator_traffic_hold"), events::Log::Notice, + "Traffic alert - ICAO Address {1}! Separation Distance {2}, Heading {3}, holding position", + icao_address, traffic_seperation, traffic_direction); + + _last_traffic_warning_time = hrt_absolute_time(); + return true; @@ -391,8 +398,7 @@ void AdsbConflict::fake_traffic(const char *callsign, float distance, float dire void AdsbConflict::run_fake_traffic(double &lat_uav, double &lon_uav, float &alt_uav) { - - /* + //Test with buffer size of 10 //first conflict fake_traffic("LX001", 5, 1.0f, 0.0f, 0.0f, 90000.0f, 90000.0f, transponder_report_s::ADSB_EMITTER_TYPE_LIGHT, 1, lat_uav, lon_uav, @@ -402,19 +408,20 @@ void AdsbConflict::run_fake_traffic(double &lat_uav, double &lon_uav, fake_traffic("LX002", 5, 1.0f, 0.0f, 0.0f, 90000.0f, 90000.0f, transponder_report_s::ADSB_EMITTER_TYPE_LIGHT, 2, lat_uav, lon_uav, alt_uav); - fake_traffic("LX0002", 5, 1.0f, 0.0f, 0.0f, 90000.0f, 90000.0f, + fake_traffic("LX002", 5, 1.0f, 0.0f, 0.0f, 90000.0f, 90000.0f, transponder_report_s::ADSB_EMITTER_TYPE_LIGHT, 2, lat_uav, lon_uav, alt_uav); - fake_traffic("LX0002", 5, 1.0f, 0.0f, 0.0f, 90000.0f, 90000.0f, + fake_traffic("LX002", 5, 1.0f, 0.0f, 0.0f, 90000.0f, 90000.0f, transponder_report_s::ADSB_EMITTER_TYPE_LIGHT, 2, lat_uav, lon_uav, alt_uav); //stop spamming - + //new conflicts fake_traffic("LX003", 5, 1.0f, 0.0f, 0.0f, 90000.0f, 90000.0f, transponder_report_s::ADSB_EMITTER_TYPE_LIGHT, 3, lat_uav, lon_uav, alt_uav); + fake_traffic("LX004", 5, 1.0f, 0.0f, 0.0f, 90000.0f, 90000.0f, transponder_report_s::ADSB_EMITTER_TYPE_LIGHT, 4, lat_uav, lon_uav, alt_uav); @@ -445,6 +452,9 @@ void AdsbConflict::run_fake_traffic(double &lat_uav, double &lon_uav, alt_uav); //buffer full + + //buffer full conflicts + fake_traffic("LX011", 5, 1.0f, 0.0f, 0.0f, 90000.0f, 90000.0f, transponder_report_s::ADSB_EMITTER_TYPE_LIGHT, 11, lat_uav, lon_uav, alt_uav); @@ -458,7 +468,7 @@ void AdsbConflict::run_fake_traffic(double &lat_uav, double &lon_uav, alt_uav); - //end conflict + //end conflicts fake_traffic("LX001", 5000, 1.0f, 0.0f, 0.0f, 90000.0f, 90000.0f, transponder_report_s::ADSB_EMITTER_TYPE_LIGHT, 1, lat_uav, lon_uav, alt_uav); @@ -474,30 +484,29 @@ void AdsbConflict::run_fake_traffic(double &lat_uav, double &lon_uav, transponder_report_s::ADSB_EMITTER_TYPE_LIGHT, 4, lat_uav, lon_uav, alt_uav); - //new conflicts with space in buffer fake_traffic("LX013", 5, 1.0f, 0.0f, 0.0f, 90000.0f, 90000.0f, transponder_report_s::ADSB_EMITTER_TYPE_LIGHT, 13, lat_uav, lon_uav, alt_uav); - + //spam fake_traffic("LX013", 5, 1.0f, 0.0f, 0.0f, 90000.0f, 90000.0f, transponder_report_s::ADSB_EMITTER_TYPE_LIGHT, 13, lat_uav, lon_uav, alt_uav); - + //new conflict fake_traffic("LX014", 5, 1.0f, 0.0f, 0.0f, 90000.0f, 90000.0f, transponder_report_s::ADSB_EMITTER_TYPE_LIGHT, 14, lat_uav, lon_uav, alt_uav); - + //spam fake_traffic("LX014", 5, 1.0f, 0.0f, 0.0f, 90000.0f, 90000.0f, transponder_report_s::ADSB_EMITTER_TYPE_LIGHT, 14, lat_uav, lon_uav, alt_uav); - + //new conflict fake_traffic("LX015", 5, 1.0f, 0.0f, 0.0f, 90000.0f, 90000.0f, transponder_report_s::ADSB_EMITTER_TYPE_LIGHT, 15, lat_uav, lon_uav, alt_uav); + for (size_t i = 0; i < _traffic_buffer.icao_address.size(); i++) { PX4_INFO("%u ", static_cast(_traffic_buffer.icao_address[i])); } - */ } diff --git a/src/lib/adsb/AdsbConflict.h b/src/lib/adsb/AdsbConflict.h index b00d04f4db..9942a6b921 100644 --- a/src/lib/adsb/AdsbConflict.h +++ b/src/lib/adsb/AdsbConflict.h @@ -46,7 +46,6 @@ #include #include -#include #include #include @@ -57,14 +56,15 @@ using namespace time_literals; static constexpr uint8_t NAVIGATOR_MAX_TRAFFIC{10}; -static constexpr uint8_t UTM_GUID_MSG_LENGTH{11}; - static constexpr uint8_t UTM_CALLSIGN_LENGTH{9}; static constexpr uint64_t CONFLICT_WARNING_TIMEOUT{60_s}; static constexpr float TRAFFIC_TO_UAV_DISTANCE_EXTENSION{1000.0f}; +static constexpr uint64_t TRAFFIC_WARNING_TIMESTEP{60_s}; //limits the max warning rate when traffic conflict buffer is full + +static constexpr uint64_t TRAFFIC_CONFLICT_LIFETIME{120_s}; //limits the time a conflict can be in the buffer without being seen (as a conflict) struct traffic_data_s { double lat_traffic; @@ -118,7 +118,7 @@ public: bool send_traffic_warning(int traffic_direction, int traffic_seperation, uint16_t tr_flags, - char uas_id[UTM_GUID_MSG_LENGTH], char tr_callsign[UTM_CALLSIGN_LENGTH], uint64_t uas_id_int); + char tr_callsign[UTM_CALLSIGN_LENGTH], uint32_t icao_address); transponder_report_s _transponder_report{}; @@ -132,6 +132,8 @@ public: void run_fake_traffic(double &lat_uav, double &lon_uav, float &alt_uav); + void remove_expired_conflicts(); + bool _conflict_detected{false}; TRAFFIC_STATE _traffic_state{TRAFFIC_STATE::NO_CONFLICT}; @@ -144,15 +146,15 @@ protected: private: - orb_advert_t _mavlink_log_pub{nullptr}; - crosstrack_error_s _crosstrack_error{}; - transponder_report_s tr{}; orb_advert_t fake_traffic_report_publisher = orb_advertise(ORB_ID(transponder_report), &tr); TRAFFIC_STATE _traffic_state_previous{TRAFFIC_STATE::NO_CONFLICT}; + hrt_abstime _last_traffic_warning_time{0}; + + hrt_abstime _last_buffer_full_warning_time{0}; }; diff --git a/src/lib/adsb/AdsbConflictTest.cpp b/src/lib/adsb/AdsbConflictTest.cpp index d83ad82414..5d6b6ccbe6 100644 --- a/src/lib/adsb/AdsbConflictTest.cpp +++ b/src/lib/adsb/AdsbConflictTest.cpp @@ -50,7 +50,7 @@ TEST_F(AdsbConflictTest, detectTrafficConflict) for (uint32_t i = 0; i < traffic_dataset_size; i++) { - printf("---------------%d--------------\n", i); + //printf("---------------%d--------------\n", i); struct traffic_data_s traffic = traffic_dataset[i]; @@ -64,10 +64,10 @@ TEST_F(AdsbConflictTest, detectTrafficConflict) adsb_conflict.detect_traffic_conflict(lat_now, lon_now, alt_now, vx_now, vy_now, vz_now); - printf("conflict_detected %d \n", adsb_conflict._conflict_detected); + //printf("conflict_detected %d \n", adsb_conflict._conflict_detected); - printf("------------------------------\n"); - printf("------------------------------\n \n"); + //printf("------------------------------\n"); + //printf("------------------------------\n \n"); EXPECT_TRUE(adsb_conflict._conflict_detected == traffic.in_conflict); } @@ -191,7 +191,6 @@ TEST_F(AdsbConflictTest, trafficAlerts) TEST_F(AdsbConflictTest, trafficReminder) { - struct traffic_buffer_s used_buffer; used_buffer.icao_address.push_back(2345); used_buffer.icao_address.push_back(1234); @@ -201,11 +200,11 @@ TEST_F(AdsbConflictTest, trafficReminder) used_buffer.icao_address.push_back(5000); used_buffer.timestamp.push_back(3_s); - used_buffer.timestamp.push_back(800_s); + used_buffer.timestamp.push_back(80_s); + used_buffer.timestamp.push_back(10_s); + used_buffer.timestamp.push_back(1000_s); used_buffer.timestamp.push_back(100_s); - used_buffer.timestamp.push_back(20000_s); - used_buffer.timestamp.push_back(6000_s); - used_buffer.timestamp.push_back(6587_s); + used_buffer.timestamp.push_back(187_s); struct traffic_buffer_s full_buffer; full_buffer.icao_address.push_back(2345); @@ -220,15 +219,15 @@ TEST_F(AdsbConflictTest, trafficReminder) full_buffer.icao_address.push_back(99999); full_buffer.timestamp.push_back(1_s); - full_buffer.timestamp.push_back(800_s); - full_buffer.timestamp.push_back(100_s); - full_buffer.timestamp.push_back(20000_s); - full_buffer.timestamp.push_back(6000_s); - full_buffer.timestamp.push_back(19000_s); - full_buffer.timestamp.push_back(5000_s); - full_buffer.timestamp.push_back(2_s); + full_buffer.timestamp.push_back(80_s); + full_buffer.timestamp.push_back(10_s); full_buffer.timestamp.push_back(1000_s); - full_buffer.timestamp.push_back(58943_s); + full_buffer.timestamp.push_back(100_s); + full_buffer.timestamp.push_back(900_s); + full_buffer.timestamp.push_back(500_s); + full_buffer.timestamp.push_back(2_s); + full_buffer.timestamp.push_back(100_s); + full_buffer.timestamp.push_back(5843_s); TestAdsbConflict adsb_conflict; diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 2467554e97..a3b17a0634 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -1440,7 +1440,6 @@ Mavlink::configure_streams_to_default(const char *configure_single_stream) configure_stream_local("SERVO_OUTPUT_RAW_0", 1.0f); configure_stream_local("SYS_STATUS", 1.0f); configure_stream_local("TIME_ESTIMATE_TO_TARGET", 1.0f); - configure_stream_local("UTM_GLOBAL_POSITION", 0.5f); configure_stream_local("VFR_HUD", 4.0f); configure_stream_local("VIBRATION", 0.1f); configure_stream_local("WIND_COV", 0.5f); @@ -1510,7 +1509,6 @@ Mavlink::configure_streams_to_default(const char *configure_single_stream) configure_stream_local("SYSTEM_TIME", 1.0f); configure_stream_local("TIME_ESTIMATE_TO_TARGET", 1.0f); configure_stream_local("TRAJECTORY_REPRESENTATION_WAYPOINTS", 5.0f); - configure_stream_local("UTM_GLOBAL_POSITION", 1.0f); configure_stream_local("VFR_HUD", 10.0f); configure_stream_local("VIBRATION", 0.5f); configure_stream_local("WIND_COV", 10.0f); @@ -1576,7 +1574,6 @@ Mavlink::configure_streams_to_default(const char *configure_single_stream) configure_stream_local("SERVO_OUTPUT_RAW_0", 1.0f); configure_stream_local("SYS_STATUS", 5.0f); configure_stream_local("TRAJECTORY_REPRESENTATION_WAYPOINTS", 5.0f); - configure_stream_local("UTM_GLOBAL_POSITION", 1.0f); configure_stream_local("VFR_HUD", 4.0f); configure_stream_local("VIBRATION", 0.5f); configure_stream_local("WIND_COV", 1.0f); @@ -1675,7 +1672,6 @@ Mavlink::configure_streams_to_default(const char *configure_single_stream) configure_stream_local("SYS_STATUS", 1.0f); configure_stream_local("SYSTEM_TIME", 1.0f); configure_stream_local("TIME_ESTIMATE_TO_TARGET", 1.0f); - configure_stream_local("UTM_GLOBAL_POSITION", 1.0f); configure_stream_local("VFR_HUD", 20.0f); configure_stream_local("VIBRATION", 2.5f); configure_stream_local("WIND_COV", 10.0f); @@ -1760,7 +1756,6 @@ Mavlink::configure_streams_to_default(const char *configure_single_stream) configure_stream_local("SYSTEM_TIME", 2.0f); configure_stream_local("TIME_ESTIMATE_TO_TARGET", 1.0f); configure_stream_local("TRAJECTORY_REPRESENTATION_WAYPOINTS", 5.0f); - configure_stream_local("UTM_GLOBAL_POSITION", 1.0f); configure_stream_local("VFR_HUD", 4.0f); configure_stream_local("VIBRATION", 0.5f); configure_stream_local("WIND_COV", 1.0f); diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index bcd5328618..3356010b90 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -150,7 +150,6 @@ # include "streams/SCALED_PRESSURE3.hpp" # include "streams/UAVIONIX_ADSB_OUT_CFG.hpp" # include "streams/UAVIONIX_ADSB_OUT_DYNAMIC.hpp" -# include "streams/UTM_GLOBAL_POSITION.hpp" #endif // !CONSTRAINED_FLASH // ensure PX4 rotation enum and MAV_SENSOR_ROTATION align @@ -419,9 +418,6 @@ static const StreamListItem streams_list[] = { #if defined(ADSB_VEHICLE_HPP) create_stream_list_item(), #endif // ADSB_VEHICLE_HPP -#if defined(UTM_GLOBAL_POSITION_HPP) - create_stream_list_item(), -#endif // UTM_GLOBAL_POSITION_HPP #if defined(COLLISION_HPP) create_stream_list_item(), #endif // COLLISION_HPP diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 928d744b47..0385c6a1eb 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -224,10 +224,6 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg) handle_message_adsb_vehicle(msg); break; - case MAVLINK_MSG_ID_UTM_GLOBAL_POSITION: - handle_message_utm_global_position(msg); - break; - case MAVLINK_MSG_ID_COLLISION: handle_message_collision(msg); break; @@ -2540,91 +2536,6 @@ MavlinkReceiver::handle_message_adsb_vehicle(mavlink_message_t *msg) _transponder_report_pub.publish(t); } -void -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); - - bool is_self_published = false; - - -#ifndef BOARD_HAS_NO_UUID - px4_guid_t px4_guid; - board_get_px4_guid(px4_guid); - is_self_published = sizeof(px4_guid) == sizeof(utm_pos.uas_id) - && memcmp(px4_guid, utm_pos.uas_id, sizeof(px4_guid_t)) == 0; -#else - - is_self_published = msg->sysid == _mavlink->get_system_id(); -#endif /* BOARD_HAS_NO_UUID */ - - - //Ignore selfpublished UTM messages - if (is_self_published) { - return; - } - - // 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; - - transponder_report_s t{}; - t.timestamp = hrt_absolute_time(); - mav_array_memcpy(t.uas_id, utm_pos.uas_id, PX4_GUID_BYTE_LENGTH); - t.icao_address = msg->sysid; - 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; -} - void MavlinkReceiver::handle_message_collision(mavlink_message_t *msg) { diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 8e2f24e157..d5ea7ac22d 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -1273,6 +1273,9 @@ void Navigator::check_traffic() } } } + + _adsb_conflict.remove_expired_conflicts(); + } bool Navigator::abort_landing() @@ -1544,7 +1547,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 4 fake transponder_report_s uORB messages"); + PRINT_MODULE_USAGE_COMMAND_DESCR("fake_traffic", "publishes 24 fake transponder_report_s uORB messages"); PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); return 0;