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

GF range warning limit mavlink critical messages

Browse Source 
-only send a mavlink critical message every 3 seconds
This commit is contained in:
Daniel Agar 2015-04-16 22:57:21 -04:00
parent 08c5ecba7a
commit 230c0b95e3
2 changed files with 21 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

21
src/modules/navigator/geofence.cpp
View File

@ -51,12 +51,15 @@
#include <unistd.h>
#include <mavlink/mavlink_log.h>
#include <geo/geo.h>
#include <drivers/drv_hrt.h>
#define GEOFENCE_OFF 0
#define GEOFENCE_FILE_ONLY 1
#define GEOFENCE_MAX_DISTANCES_ONLY 2
#define GEOFENCE_FILE_AND_MAX_DISTANCES 3
#define GEOFENCE_RANGE_WARNING_LIMIT 3000000
/* Oddly, ERROR is not defined for C++ */
#ifdef ERROR
@ -69,6 +72,8 @@ Geofence::Geofence() :
_fence_pub(-1),
_home_pos{},
_home_pos_set(false),
_last_horizontal_range_warning(0),
_last_vertical_range_warning(0),
_altitude_min(0),
_altitude_max(0),
_verticesCount(0),
@ -146,14 +151,22 @@ bool Geofence::inside(double lat, double lon, float altitude)
&dist_xy, &dist_z);
if (max_vertical_distance > 0 && (dist_z > max_vertical_distance)) {
mavlink_log_critical(_mavlinkFd, "Geofence exceeded max vertical distance by %.0f m",
(double)(dist_z - max_vertical_distance));
if (hrt_elapsed_time(&_last_vertical_range_warning) > GEOFENCE_RANGE_WARNING_LIMIT) {
mavlink_log_critical(_mavlinkFd, "Geofence exceeded max vertical distance by %.1f m",
(double)(dist_z - max_vertical_distance));
_last_vertical_range_warning = hrt_absolute_time();
}
return false;
}
if (max_horizontal_distance > 0 && (dist_xy > max_horizontal_distance)) {
mavlink_log_critical(_mavlinkFd, "Geofence exceeded max horizontal distance by %.0f m",
(double)(dist_xy - max_horizontal_distance));
if (hrt_elapsed_time(&_last_horizontal_range_warning) > GEOFENCE_RANGE_WARNING_LIMIT) {
mavlink_log_critical(_mavlinkFd, "Geofence exceeded max horizontal distance by %.1f m",
(double)(dist_xy - max_horizontal_distance));
_last_horizontal_range_warning = hrt_absolute_time();
}
return false;
}
}

4
src/modules/navigator/geofence.h
View File

@ -48,6 +48,7 @@
#include <uORB/topics/home_position.h>
#include <controllib/blocks.hpp>
#include <controllib/block/BlockParam.hpp>
#include <drivers/drv_hrt.h>
#define GEOFENCE_FILENAME "/fs/microsd/etc/geofence.txt"
@ -109,6 +110,9 @@ private:
home_position_s _home_pos;
bool _home_pos_set;
hrt_abstime _last_horizontal_range_warning;
hrt_abstime _last_vertical_range_warning;
float _altitude_min;
float _altitude_max;