From 2d6758a39db911ff554018aba1bff35a7560fd9a Mon Sep 17 00:00:00 2001 From: Julian Kent Date: Tue, 12 Jan 2021 14:18:22 +0100 Subject: [PATCH] Cleanup of dataman mocks and check interval definition --- .../GeofenceBreachAvoidanceTest.cpp | 1 + .../GeofenceBreachAvoidance/dataman_mocks.hpp | 100 +++++++++++++++++ .../GeofenceBreachAvoidance/fake_geofence.hpp | 106 ++---------------- .../geofence_breach_avoidance.cpp | 14 ++- .../geofence_breach_avoidance.h | 2 + src/modules/navigator/navigator_main.cpp | 5 +- 6 files changed, 124 insertions(+), 104 deletions(-) create mode 100644 src/modules/navigator/GeofenceBreachAvoidance/dataman_mocks.hpp diff --git a/src/modules/navigator/GeofenceBreachAvoidance/GeofenceBreachAvoidanceTest.cpp b/src/modules/navigator/GeofenceBreachAvoidance/GeofenceBreachAvoidanceTest.cpp index f3c6483a0f9..904d22200a4 100644 --- a/src/modules/navigator/GeofenceBreachAvoidance/GeofenceBreachAvoidanceTest.cpp +++ b/src/modules/navigator/GeofenceBreachAvoidance/GeofenceBreachAvoidanceTest.cpp @@ -34,6 +34,7 @@ #include #include "geofence_breach_avoidance.h" #include "fake_geofence.hpp" +#include "dataman_mocks.hpp" #include using namespace matrix; diff --git a/src/modules/navigator/GeofenceBreachAvoidance/dataman_mocks.hpp b/src/modules/navigator/GeofenceBreachAvoidance/dataman_mocks.hpp new file mode 100644 index 00000000000..e6605b525d9 --- /dev/null +++ b/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 +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;}; +} + diff --git a/src/modules/navigator/GeofenceBreachAvoidance/fake_geofence.hpp b/src/modules/navigator/GeofenceBreachAvoidance/fake_geofence.hpp index d276d2d6cd6..3a1ff00a02a 100644 --- a/src/modules/navigator/GeofenceBreachAvoidance/fake_geofence.hpp +++ b/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 @@ -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) { - flag = false; + if (_flag_on_left) { + _flag_on_left = 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) { - flag = false; + if (_flag_on_right) { + _flag_on_right = 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 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;}; -} diff --git a/src/modules/navigator/GeofenceBreachAvoidance/geofence_breach_avoidance.cpp b/src/modules/navigator/GeofenceBreachAvoidance/geofence_breach_avoidance.cpp index 691a2f97083..c965e6fbcb2 100644 --- a/src/modules/navigator/GeofenceBreachAvoidance/geofence_breach_avoidance.cpp +++ b/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 * - vertical_vel_abs; // navigator runs at 5Hz + _multirotor_vertical_braking_distance = predictor.getCurrentPosition() + (GEOFENCE_CHECK_INTERVAL_US / 1e6f) * + vertical_vel_abs; _multirotor_vertical_braking_distance = matrix::sign(_climbrate) * _multirotor_vertical_braking_distance; diff --git a/src/modules/navigator/GeofenceBreachAvoidance/geofence_breach_avoidance.h b/src/modules/navigator/GeofenceBreachAvoidance/geofence_breach_avoidance.h index 4cce9045626..1a6c63ea20e 100644 --- a/src/modules/navigator/GeofenceBreachAvoidance/geofence_breach_avoidance.h +++ b/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 diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 28a618e6cb4..061dd5e0640 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/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();