Cleanup of dataman mocks and check interval definition

src/modules/navigator/GeofenceBreachAvoidance/GeofenceBreachAvoidanceTest.cpp

@@ -34,6 +34,7 @@
 #include <gtest/gtest.h>
 #include "geofence_breach_avoidance.h"
 #include "fake_geofence.hpp"
+#include "dataman_mocks.hpp"
 #include <parameters/param.h>
 
 using namespace matrix;

src/modules/navigator/GeofenceBreachAvoidance/dataman_mocks.hpp

@@ -0,0 +1,100 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2021 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+/**
+ * @file dataman_mocks.h
+ * Provides a minimal dataman implementation to compile against for testing
+ *
+ * @author Roman Bapst
+ * @author Julian Kent
+ */
+#pragma once
+
+#include <dataman/dataman.h>
+extern "C" {
+	__EXPORT ssize_t
+	dm_read(
+		dm_item_t item,			/* The item type to retrieve */
+		unsigned index,			/* The index of the item */
+		void *buffer,			/* Pointer to caller data buffer */
+		size_t buflen			/* Length in bytes of data to retrieve */
+	) {return 0;};
+
+	/** write to the data manager store */
+	__EXPORT ssize_t
+	dm_write(
+		dm_item_t  item,		/* The item type to store */
+		unsigned index,			/* The index of the item */
+		dm_persitence_t persistence,	/* The persistence level of this item */
+		const void *buffer,		/* Pointer to caller data buffer */
+		size_t buflen			/* Length in bytes of data to retrieve */
+	) {return 0;};
+
+	/**
+	 * Lock all items of a type. Can be used for atomic updates of multiple items (single items are always updated
+	 * atomically).
+	 * Note that this lock is independent from dm_read & dm_write calls.
+	 * @return 0 on success and lock taken, -1 on error (lock not taken, errno set)
+	 */
+	__EXPORT int
+	dm_lock(
+		dm_item_t item			/* The item type to lock */
+	) {return 0;};
+
+	/**
+	 * Try to lock all items of a type (@see sem_trywait()).
+	 * @return 0 if lock is taken, -1 otherwise (on error or if already locked. errno is set accordingly)
+	 */
+	__EXPORT int
+	dm_trylock(
+		dm_item_t item			/* The item type to lock */
+	) {return 0;};
+
+	/** Unlock all items of a type */
+	__EXPORT void
+	dm_unlock(
+		dm_item_t item			/* The item type to unlock */
+	) {};
+
+	/** Erase all items of this type */
+	__EXPORT int
+	dm_clear(
+		dm_item_t item			/* The item type to clear */
+	) {return 0;};
+
+	/** Tell the data manager about the type of the last reset */
+	__EXPORT int
+	dm_restart(
+		dm_reset_reason restart_type	/* The last reset type */
+	) {return 0;};
+}
+

src/modules/navigator/GeofenceBreachAvoidance/fake_geofence.hpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2013 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2021 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -37,7 +37,7 @@
  * @author Roman Bapst
  * @author Julian Kent
  */
-
+#pragma once
 
 #include"../geofence.h"
 #include <lib/mathlib/mathlib.h>
@@ -67,7 +67,7 @@ public:
 			}
 
 		case ProbeFunction::GF_BOUNDARY_20M_AHEAD: {
-				return _gf_boundary_is_20m_ahead(lat, lon, altitude);
+				return _gf_boundary_is_20m_north(lat, lon, altitude);
 			}
 
 		default:
@@ -89,6 +89,9 @@ private:
 
 	ProbeFunction _probe_function_behavior = ProbeFunction::ALL_POINTS_OUTSIDE;
 
+	bool _flag_on_left = true;
+	bool _flag_on_right = false;
+
 	bool _allPointsOutside(double lat, double lon, float alt)
 	{
 		return false;
@@ -96,10 +99,8 @@ private:
 
 	bool _left_inside_right_outside(double lat, double lon, float alt)
 	{
-		static int flag = true;
+		if (_flag_on_left) {
-
+			_flag_on_left = false;
-		if (flag) {
-			flag = false;
 			return true;
 
 		} else {
@@ -109,19 +110,17 @@ private:
 
 	bool _right_inside_left_outside(double lat, double lon, float alt)
 	{
-		static int flag = false;
+		if (_flag_on_right) {
-
+			_flag_on_right = false;
-		if (flag) {
-			flag = false;
 			return true;
 
 		} else {
-			flag = true;
+			_flag_on_right = true;
 			return false;
 		}
 	}
 
-	bool _gf_boundary_is_20m_ahead(double lat, double lon, float alt)
+	bool _gf_boundary_is_20m_north(double lat, double lon, float alt)
 	{
 		struct map_projection_reference_s ref = {};
 		matrix::Vector2<double> home_global(42.1, 8.2);
@@ -138,84 +137,3 @@ private:
 		return true;
 	}
 };
-
-typedef enum {
-	DM_PERSIST_POWER_ON_RESET = 0,	/* Data survives all resets */
-	DM_PERSIST_IN_FLIGHT_RESET,     /* Data survives in-flight resets only */
-	DM_PERSIST_VOLATILE             /* Data does not survive resets */
-} dm_persitence_t;
-
-typedef enum {
-	DM_KEY_SAFE_POINTS = 0,		/* Safe points coordinates, safe point 0 is home point */
-	DM_KEY_FENCE_POINTS,		/* Fence vertex coordinates */
-	DM_KEY_WAYPOINTS_OFFBOARD_0,	/* Mission way point coordinates sent over mavlink */
-	DM_KEY_WAYPOINTS_OFFBOARD_1,	/* (alternate between 0 and 1) */
-	DM_KEY_WAYPOINTS_ONBOARD,	/* Mission way point coordinates generated onboard */
-	DM_KEY_MISSION_STATE,		/* Persistent mission state */
-	DM_KEY_COMPAT,
-	DM_KEY_NUM_KEYS			/* Total number of item types defined */
-} dm_item_t;
-
-typedef enum {
-	DM_INIT_REASON_POWER_ON = 0,	/* Data survives resets */
-	DM_INIT_REASON_IN_FLIGHT,		/* Data survives in-flight resets only */
-	DM_INIT_REASON_VOLATILE			/* Data does not survive reset */
-} dm_reset_reason;
-
-extern "C" {
-	__EXPORT ssize_t
-	dm_read(
-		dm_item_t item,			/* The item type to retrieve */
-		unsigned index,			/* The index of the item */
-		void *buffer,			/* Pointer to caller data buffer */
-		size_t buflen			/* Length in bytes of data to retrieve */
-	) {return 0;};
-
-	/** write to the data manager store */
-	__EXPORT ssize_t
-	dm_write(
-		dm_item_t  item,		/* The item type to store */
-		unsigned index,			/* The index of the item */
-		dm_persitence_t persistence,	/* The persistence level of this item */
-		const void *buffer,		/* Pointer to caller data buffer */
-		size_t buflen			/* Length in bytes of data to retrieve */
-	) {return 0;};
-
-	/**
-	 * Lock all items of a type. Can be used for atomic updates of multiple items (single items are always updated
-	 * atomically).
-	 * Note that this lock is independent from dm_read & dm_write calls.
-	 * @return 0 on success and lock taken, -1 on error (lock not taken, errno set)
-	 */
-	__EXPORT int
-	dm_lock(
-		dm_item_t item			/* The item type to lock */
-	) {return 0;};
-
-	/**
-	 * Try to lock all items of a type (@see sem_trywait()).
-	 * @return 0 if lock is taken, -1 otherwise (on error or if already locked. errno is set accordingly)
-	 */
-	__EXPORT int
-	dm_trylock(
-		dm_item_t item			/* The item type to lock */
-	) {return 0;};
-
-	/** Unlock all items of a type */
-	__EXPORT void
-	dm_unlock(
-		dm_item_t item			/* The item type to unlock */
-	) {};
-
-	/** Erase all items of this type */
-	__EXPORT int
-	dm_clear(
-		dm_item_t item			/* The item type to clear */
-	) {return 0;};
-
-	/** Tell the data manager about the type of the last reset */
-	__EXPORT int
-	dm_restart(
-		dm_reset_reason restart_type	/* The last reset type */
-	) {return 0;};
-}

src/modules/navigator/GeofenceBreachAvoidance/geofence_breach_avoidance.cpp

@@ -53,6 +53,7 @@ GeofenceBreachAvoidance::GeofenceBreachAvoidance(ModuleParams *parent) :
 
 void GeofenceBreachAvoidance::updateParameters()
 {
+	ModuleParams::updateParams();
 	param_get(_paramHandle.param_mpc_jerk_max, &_params.param_mpc_jerk_max);
 	param_get(_paramHandle.param_mpc_acc_hor, &_params.param_mpc_acc_hor);
 	param_get(_paramHandle.param_mpc_acc_hor_max, &_params.param_mpc_acc_hor_max);
@@ -156,18 +157,18 @@ GeofenceBreachAvoidance::generateLoiterPointForMultirotor(geofence_violation_typ
 		float current_max = _test_point_distance;
 		Vector2d test_point;
 
-		// logarithmic search for the distance from the drone to the geofence in the given direction
+		// binary search for the distance from the drone to the geofence in the given direction
 		while (abs(current_max - current_min) > 0.5f) {
 			test_point = waypointFromBearingAndDistance(_current_pos_lat_lon, _test_point_bearing, current_distance);
 
 			if (!geofence->isInsidePolygonOrCircle(test_point(0), test_point(1), _current_alt_amsl)) {
 				current_max = current_distance;
-				current_distance = current_min + (current_max - current_min) * 0.5f;
 
 			} else {
 				current_min = current_distance;
-				current_distance = current_min + (current_max - current_min) * 0.5f;
 			}
+
+			current_distance = (current_max + current_min) * 0.5f;
 		}
 
 		test_point = waypointFromBearingAndDistance(_current_pos_lat_lon, _test_point_bearing, current_distance);
@@ -223,7 +224,8 @@ float GeofenceBreachAvoidance::computeBrakingDistanceMultirotor()
 	predictor.updateDurations(0.f);
 	predictor.updateTraj(predictor.getTotalTime());
 
-	_multirotor_braking_distance =  predictor.getCurrentPosition() + 0.2f * _velocity_hor_abs; // navigator runs at 5Hz
+	_multirotor_braking_distance =  predictor.getCurrentPosition() + (GEOFENCE_CHECK_INTERVAL_US / 1e6f) *
+					_velocity_hor_abs;
 
 	return _multirotor_braking_distance;
 }
@@ -241,8 +243,8 @@ float GeofenceBreachAvoidance::computeVerticalBrakingDistanceMultirotor()
 	predictor.updateDurations(0.f);
 	predictor.updateTraj(predictor.getTotalTime());
 
-	_multirotor_vertical_braking_distance =  predictor.getCurrentPosition() + 0.2f *
+	_multirotor_vertical_braking_distance =  predictor.getCurrentPosition() + (GEOFENCE_CHECK_INTERVAL_US / 1e6f) *
-			vertical_vel_abs; // navigator runs at 5Hz
+			vertical_vel_abs;
 
 	_multirotor_vertical_braking_distance = matrix::sign(_climbrate) * _multirotor_vertical_braking_distance;
 

src/modules/navigator/GeofenceBreachAvoidance/geofence_breach_avoidance.h

@@ -39,6 +39,8 @@
 
 class Geofence;
 
+#define GEOFENCE_CHECK_INTERVAL_US 200000
+
 union geofence_violation_type_u {
 	struct {
 		bool dist_to_home_exceeded: 1;	///< 0 - distance to home exceeded

src/modules/navigator/navigator_main.cpp

@@ -64,9 +64,6 @@
  * @ingroup apps
  */
 extern "C" __EXPORT int navigator_main(int argc, char *argv[]);
-
-#define GEOFENCE_CHECK_INTERVAL 200000
-
 using namespace time_literals;
 
 namespace navigator
@@ -700,7 +697,7 @@ void Navigator::geofence_breach_check(bool &have_geofence_position_data)
 
 	if (have_geofence_position_data &&
 	    (_geofence.getGeofenceAction() != geofence_result_s::GF_ACTION_NONE) &&
-	    (hrt_elapsed_time(&_last_geofence_check) > GEOFENCE_CHECK_INTERVAL)) {
+	    (hrt_elapsed_time(&_last_geofence_check) > GEOFENCE_CHECK_INTERVAL_US)) {
 
 		const position_controller_status_s &pos_ctrl_status = _position_controller_status_sub.get();