fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-07-18 06:20:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

navigator: reduce traffic alert messages

Browse Source
This commit is contained in:
Igor Mišić
2021-07-07 18:08:27 +02:00
committed by Beat Küng
parent 496e3fe2e0
commit 376450ccf0
2 changed files with 95 additions and 57 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/navigator/navigator.h
+12
View File
@@ -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<px4::params::MIS_YAW_ERR>) _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();
src/modules/navigator/navigator_main.cpp
+83 -57
View File
@@ -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()
{