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

navigator: switch to events

Browse Source
This commit is contained in:
Beat Küng
2021-07-08 17:03:38 +02: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/geofence.cpp
+13 -5
View File
@@ -47,6 +47,7 @@
#include <drivers/drv_hrt.h>
#include <lib/geo/geo.h>
#include <systemlib/mavlink_log.h>
#include <px4_platform_common/events.h>
#include "navigator.h"
@@ -239,8 +240,11 @@ bool Geofence::isCloserThanMaxDistToHome(double lat, double lon, float altitude)
if (max_horizontal_distance > FLT_EPSILON && (dist_xy > max_horizontal_distance)) {
if (hrt_elapsed_time(&_last_horizontal_range_warning) > GEOFENCE_RANGE_WARNING_LIMIT) {
mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Maximum distance from home reached (%.5f)",
mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Maximum distance from home reached (%.5f)\t",
(double)max_horizontal_distance);
events::send<float>(events::ID("navigator_geofence_max_dist_from_home"), {events::Log::Critical, events::LogInternal::Warning},
"Geofence: maximum distance from home reached ({1:.0m})",
max_horizontal_distance);
_last_horizontal_range_warning = hrt_absolute_time();
}
@@ -264,8 +268,11 @@ bool Geofence::isBelowMaxAltitude(float altitude)
if (max_vertical_distance > FLT_EPSILON && (dist_z > max_vertical_distance)) {
if (hrt_elapsed_time(&_last_vertical_range_warning) > GEOFENCE_RANGE_WARNING_LIMIT) {
mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Maximum altitude above home reached (%.5f)",
mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Maximum altitude above home reached (%.5f)\t",
(double)max_vertical_distance);
events::send<float>(events::ID("navigator_geofence_max_alt_from_home"), {events::Log::Critical, events::LogInternal::Warning},
"Geofence: maximum altitude above home reached ({1:.0m_v})",
max_vertical_distance);
_last_vertical_range_warning = hrt_absolute_time();
}
@@ -553,7 +560,8 @@ Geofence::loadFromFile(const char *filename)
/* Check if import was successful */
if (gotVertical && pointCounter > 2) {
mavlink_log_info(_navigator->get_mavlink_log_pub(), "Geofence imported");
mavlink_log_info(_navigator->get_mavlink_log_pub(), "Geofence imported\t");
events::send(events::ID("navigator_geofence_imported"), events::Log::Info, "Geofence imported");
rc = PX4_OK;
/* do a second pass, now that we know the number of vertices */
@@ -573,8 +581,8 @@ Geofence::loadFromFile(const char *filename)
rc = dm_write(DM_KEY_FENCE_POINTS, 0, DM_PERSIST_POWER_ON_RESET, &stats, sizeof(mission_stats_entry_s));
} else {
PX4_ERR("Geofence: import error");
mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Geofence import error");
mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Geofence: import error\t");
events::send(events::ID("navigator_geofence_import_failed"), events::Log::Error, "Geofence: import error");
}
updateFence();