diff options
| author | Lorenz Meier <lorenz@px4.io> | 2015-04-18 09:53:48 +0200 |
|---|---|---|
| committer | Lorenz Meier <lorenz@px4.io> | 2015-04-18 09:53:48 +0200 |
| commit | c4dc5b2d6ab589cc0cd24455107735e938f7df09 (patch) | |
| tree | c437645d5ac25f19270d404735a1a90a6eb2de35 | |
| parent | 08c5ecba7a18c4e1d7fc91f8ff6278a27f7344f0 (diff) | |
| parent | 230c0b95e32b84f72cd8a9cc6dbff15026eefa14 (diff) | |
| download | px4-firmware-c4dc5b2d6ab589cc0cd24455107735e938f7df09.tar.gz px4-firmware-c4dc5b2d6ab589cc0cd24455107735e938f7df09.tar.bz2 px4-firmware-c4dc5b2d6ab589cc0cd24455107735e938f7df09.zip | |
Merge pull request #2028 from dagar/rangewarning
GF range warning limit mavlink critical messages
| -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; |