From 230c0b95e32b84f72cd8a9cc6dbff15026eefa14 Mon Sep 17 00:00:00 2001
From: Daniel Agar <daniel@agar.ca>
Date: Thu, 16 Apr 2015 22:57:21 -0400
Subject: [PATCH] GF range warning limit mavlink critical messages

-only send a mavlink critical message every 3 seconds
---
 src/modules/navigator/geofence.cpp | 21 +++++++++++++++++----
 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 31c4db61a5..1b66107347 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 effce9e976..96764cc976 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;