fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-25 02:23:33 +08:00
Code Issues Packages Projects Releases Wiki Activity

navigator: switch to events

Browse Source
This commit is contained in:
Beat Küng
2021-09-11 14:08:41 -04:00
committed by Daniel Agar
parent 5ac43e7236
commit 1f73294ad7
8 changed files with 224 additions and 62 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/navigator/navigator_main.cpp
+72 -14
View File
@@ -54,6 +54,7 @@
#include <lib/mathlib/mathlib.h>
#include <px4_platform_common/px4_config.h>
#include <px4_platform_common/defines.h>
#include <px4_platform_common/events.h>
#include <px4_platform_common/posix.h>
#include <px4_platform_common/tasks.h>
#include <systemlib/mavlink_log.h>
@@ -362,7 +363,9 @@ Navigator::run()
rep->next.valid = false;
} else {
mavlink_log_critical(&_mavlink_log_pub, "Reposition is outside geofence");
mavlink_log_critical(&_mavlink_log_pub, "Reposition is outside geofence\t");
events::send(events::ID("navigator_reposition_outside_geofence"), {events::Log::Error, events::LogInternal::Info},
"Reposition is outside geofence");
}
// CMD_DO_REPOSITION is acknowledged by commander
@@ -533,10 +536,13 @@ Navigator::run()
case RTL::RTL_CLOSEST:
if (rtl_activated) {
if (rtl_type() == RTL::RTL_LAND) {
mavlink_log_info(get_mavlink_log_pub(), "RTL LAND activated");
mavlink_log_info(get_mavlink_log_pub(), "RTL LAND activated\t");
events::send(events::ID("navigator_rtl_landing_activated"), events::Log::Info, "RTL activated");
} else {
mavlink_log_info(get_mavlink_log_pub(), "RTL Closest landing point activated");
mavlink_log_info(get_mavlink_log_pub(), "RTL Closest landing point activated\t");
events::send(events::ID("navigator_rtl_closest_point_activated"), events::Log::Info,
"RTL to closest landing point activated");
}
}
@@ -577,7 +583,9 @@ Navigator::run()
}
if (rtl_activated) {
mavlink_log_info(get_mavlink_log_pub(), "RTL Mission activated, continue mission");
mavlink_log_info(get_mavlink_log_pub(), "RTL Mission activated, continue mission\t");
events::send(events::ID("navigator_rtl_mission_activated"), events::Log::Info,
"RTL Mission activated, continue mission");
}
navigation_mode_new = &_mission;
@@ -599,14 +607,18 @@ Navigator::run()
}
if (rtl_activated) {
mavlink_log_info(get_mavlink_log_pub(), "RTL Mission activated, fly mission in reverse");
mavlink_log_info(get_mavlink_log_pub(), "RTL Mission activated, fly mission in reverse\t");
events::send(events::ID("navigator_rtl_mission_activated_rev"), events::Log::Info,
"RTL Mission activated, fly mission in reverse");
}
navigation_mode_new = &_mission;
} else {
if (rtl_activated) {
mavlink_log_info(get_mavlink_log_pub(), "RTL Mission activated, fly to home");
mavlink_log_info(get_mavlink_log_pub(), "RTL Mission activated, fly to home\t");
events::send(events::ID("navigator_rtl_mission_activated_home"), events::Log::Info,
"RTL Mission activated, fly to home");
}
navigation_mode_new = &_rtl;
@@ -617,7 +629,8 @@ Navigator::run()
default:
if (rtl_activated) {
mavlink_log_info(get_mavlink_log_pub(), "RTL HOME activated");
mavlink_log_info(get_mavlink_log_pub(), "RTL HOME activated\t");
events::send(events::ID("navigator_rtl_home_activated"), events::Log::Info, "RTL activated");
}
navigation_mode_new = &_rtl;
@@ -794,7 +807,9 @@ void Navigator::geofence_breach_check(bool &have_geofence_position_data)
/* Issue a warning about the geofence violation once and only if we are armed */
if (!_geofence_violation_warning_sent && _vstatus.arming_state == vehicle_status_s::ARMING_STATE_ARMED) {
mavlink_log_critical(&_mavlink_log_pub, "Approaching on Geofence");
mavlink_log_critical(&_mavlink_log_pub, "Approaching on Geofence\t");
events::send(events::ID("navigator_approach_geofence"), {events::Log::Warning, events::LogInternal::Info},
"Approaching on Geofence");
// we have predicted a geofence violation and if the action is to loiter then
// demand a reposition to a location which is inside the geofence
@@ -1154,6 +1169,12 @@ void Navigator::check_traffic()
snprintf(&uas_id[i * 2], sizeof(uas_id) - i * 2, "%02x", tr.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)(tr.uas_id[PX4_GUID_BYTE_LENGTH - i - 1]) << (i * 8);
}
//Manned/Unmanned Vehicle Seperation Distance
if (tr.emitter_type == transponder_report_s::ADSB_EMITTER_TYPE_UAV) {
horizontal_separation = NAVTrafficAvoidUnmanned;
@@ -1212,19 +1233,36 @@ void Navigator::check_traffic()
case 1: {
/* Warn only */
mavlink_log_critical(&_mavlink_log_pub, "Warning TRAFFIC %s! dst %d, hdg %d",
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);
/* EVENT
* @description
* - ID: {1}
* - Distance: {2m}
* - Direction: {3} degrees
*/
events::send<uint64_t, int32_t, int16_t>(events::ID("navigator_traffic"), events::Log::Critical, "Traffic alert",
uas_id_int, traffic_seperation, traffic_direction);
break;
}
case 2: {
/* RTL Mode */
mavlink_log_critical(&_mavlink_log_pub, "TRAFFIC: %s Returning home! dst %d, hdg %d",
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
*/
events::send<uint64_t, int32_t, int16_t>(events::ID("navigator_traffic_rtl"), events::Log::Critical,
"Traffic alert, returning home",
uas_id_int, traffic_seperation, traffic_direction);
// set the return altitude to minimum
_rtl.set_return_alt_min(true);
@@ -1238,10 +1276,19 @@ void Navigator::check_traffic()
case 3: {
/* Land Mode */
mavlink_log_critical(&_mavlink_log_pub, "TRAFFIC: %s Landing! dst %d, hdg % d",
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
*/
events::send<uint64_t, int32_t, int16_t>(events::ID("navigator_traffic_land"), events::Log::Critical,
"Traffic alert, landing",
uas_id_int, traffic_seperation, traffic_direction);
// ask the commander to land
vehicle_command_s vcmd = {};
@@ -1253,10 +1300,19 @@ void Navigator::check_traffic()
case 4: {
/* Position hold */
mavlink_log_critical(&_mavlink_log_pub, "TRAFFIC: %s Holding position! dst %d, hdg %d",
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
*/
events::send<uint64_t, int32_t, int16_t>(events::ID("navigator_traffic_hold"), events::Log::Critical,
"Traffic alert, holding position",
uas_id_int, traffic_seperation, traffic_direction);
// ask the commander to Loiter
vehicle_command_s vcmd = {};
@@ -1375,12 +1431,14 @@ Navigator::publish_mission_result()
}
void
Navigator::set_mission_failure(const char *reason)
Navigator::set_mission_failure_heading_timeout()
{
if (!_mission_result.failure) {
_mission_result.failure = true;
set_mission_result_updated();
mavlink_log_critical(&_mavlink_log_pub, "%s", reason);
mavlink_log_critical(&_mavlink_log_pub, "unable to reach heading within timeout\t");
events::send(events::ID("navigator_mission_failure_heading"), events::Log::Critical,
"Mission failure: unable to reach heading within timeout");
}
}