diff --git a/src/modules/navigator/navigator.h b/src/modules/navigator/navigator.h index 3b52358a1b..da33c8058e 100644 --- a/src/modules/navigator/navigator.h +++ b/src/modules/navigator/navigator.h @@ -141,6 +141,11 @@ public: */ void check_traffic(); + /** + * Buffer for air traffic to control the amount of messages sent to a user + */ + bool buffer_air_traffic(uint32_t icao_address); + /** * Setters */ @@ -329,6 +334,11 @@ private: (ParamFloat) _param_mis_yaw_err ) + struct traffic_buffer_s { + uint32_t icao_address; + hrt_abstime timestamp; + }; + int _local_pos_sub{-1}; int _mission_sub{-1}; int _vehicle_status_sub{-1}; @@ -415,6 +425,8 @@ private: bool _mission_landing_in_progress{false}; // this flag gets set if the mission is currently executing on a landing pattern // if mission mode is inactive, this flag will be cleared after 2 seconds + traffic_buffer_s _traffic_buffer{}; + // update subscriptions void params_update(); diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 9306e69a4c..24160fbb39 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -1192,75 +1192,79 @@ void Navigator::check_traffic() 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; - int traffic_seperation = (int)fabsf(cr.distance); + bool action_needed = buffer_air_traffic(tr.icao_address); - switch (_param_nav_traff_avoid.get()) { + if (action_needed) { + // 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); - case 0: { - /* 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; - } + switch (_param_nav_traff_avoid.get()) { - case 1: { - /* 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 0: { + /* 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 2: { - /* 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); + case 1: { + /* 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; + } - // set the return altitude to minimum - _rtl.set_return_alt_min(true); + case 2: { + /* 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); - // 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; - } + // set the return altitude to minimum + _rtl.set_return_alt_min(true); - 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 execute an RTL + vehicle_command_s vcmd = {}; + vcmd.command = vehicle_command_s::VEHICLE_CMD_NAV_RETURN_TO_LAUNCH; + publish_vehicle_cmd(&vcmd); + break; + } - // ask the commander to land - vehicle_command_s vcmd = {}; - vcmd.command = vehicle_command_s::VEHICLE_CMD_NAV_LAND; - 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; + 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; + + } } } } @@ -1271,6 +1275,28 @@ void Navigator::check_traffic() } } +bool +Navigator::buffer_air_traffic(uint32_t icao_address) +{ + bool action_needed = true; + + if (_traffic_buffer.icao_address == icao_address) { + + if (hrt_elapsed_time(&_traffic_buffer.timestamp) > 60_s) { + _traffic_buffer.timestamp = hrt_absolute_time(); + + } else { + action_needed = false; + } + + } else { + _traffic_buffer.timestamp = hrt_absolute_time(); + _traffic_buffer.icao_address = icao_address; + } + + return action_needed; +} + bool Navigator::abort_landing() {