diff options
author | Daniel Agar <daniel@agar.ca> | 2015-04-16 22:57:21 -0400 |
---|---|---|
committer | Daniel Agar <daniel@agar.ca> | 2015-04-17 20:15:43 -0400 |
commit | 230c0b95e32b84f72cd8a9cc6dbff15026eefa14 (patch) | |
tree | c437645d5ac25f19270d404735a1a90a6eb2de35 /src | |
parent | 08c5ecba7a18c4e1d7fc91f8ff6278a27f7344f0 (diff) | |
download | px4-firmware-230c0b95e32b84f72cd8a9cc6dbff15026eefa14.tar.gz px4-firmware-230c0b95e32b84f72cd8a9cc6dbff15026eefa14.tar.bz2 px4-firmware-230c0b95e32b84f72cd8a9cc6dbff15026eefa14.zip |
GF range warning limit mavlink critical messages
-only send a mavlink critical message every 3 seconds
Diffstat (limited to 'src')
-rw-r--r-- | src/modules/navigator/geofence.cpp | 21 | ||||
-rw-r--r-- | src/modules/navigator/geofence.h | 4 |
2 files changed, 21 insertions, 4 deletions
diff --git a/src/modules/navigator/geofence.cpp b/src/modules/navigator/geofence.cpp index 31c4db61a..1b6610734 100644 --- a/src/modules/navigator/geofence.cpp +++ b/src/modules/navigator/geofence.cpp @@ -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; } } diff --git a/src/modules/navigator/geofence.h b/src/modules/navigator/geofence.h index effce9e97..96764cc97 100644 --- a/src/modules/navigator/geofence.h +++ b/src/modules/navigator/geofence.h @@ -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; |