Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-28 10:46:33 +08:00
Code Issues Actions 27 Packages Projects Releases Wiki Activity

Cleanup of dataman mocks and check interval definition

Browse Source
This commit is contained in:
Julian Kent
2021-01-12 14:18:22 +01:00
committed by Julian Kent
parent a97a4d8bb8
commit 2d6758a39d
6 changed files with 124 additions and 104 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/navigator/GeofenceBreachAvoidance/GeofenceBreachAvoidanceTest.cpp
+1
View File
@@ -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
+100
View File
@@ -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
+12 -94
View File
@@ -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) {
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<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
+8 -6
View File
@@ -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;
src/modules/navigator/GeofenceBreachAvoidance/geofence_breach_avoidance.h
+2
View File
@@ -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
+1 -4
View File
@@ -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();