fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-11-07 01:41:36 +08:00
Code Issues Packages Projects Releases Wiki Activity

Fix enum and param update

Browse Source
This commit is contained in:
Julian Kent
2021-01-13 10:26:10 +01:00
committed by Julian Kent
parent c745c8bb45
commit a97a4d8bb8
5 changed files with 22 additions and 23 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/navigator/GeofenceBreachAvoidance/GeofenceBreachAvoidanceTest.cpp
+10 -10
View File
@@ -51,7 +51,7 @@ public:
TEST_F(GeofenceBreachAvoidanceTest, waypointFromBearingAndDistance) TEST_F(GeofenceBreachAvoidanceTest, waypointFromBearingAndDistance)
{ {
GeofenceBreachAvoidance gf_avoidance; GeofenceBreachAvoidance gf_avoidance(nullptr);
struct map_projection_reference_s ref = {}; struct map_projection_reference_s ref = {};
Vector2d home_global(42.1, 8.2); Vector2d home_global(42.1, 8.2);
map_projection_init(&ref, home_global(0), home_global(1)); map_projection_init(&ref, home_global(0), home_global(1));
@@ -86,7 +86,7 @@ TEST_F(GeofenceBreachAvoidanceTest, waypointFromBearingAndDistance)
TEST_F(GeofenceBreachAvoidanceTest, generateLoiterPointForFixedWing) TEST_F(GeofenceBreachAvoidanceTest, generateLoiterPointForFixedWing)
{ {
GeofenceBreachAvoidance gf_avoidance; GeofenceBreachAvoidance gf_avoidance(nullptr);
FakeGeofence geo; FakeGeofence geo;
struct map_projection_reference_s ref = {}; struct map_projection_reference_s ref = {};
Vector2d home_global(42.1, 8.2); Vector2d home_global(42.1, 8.2);
@@ -109,7 +109,7 @@ TEST_F(GeofenceBreachAvoidanceTest, generateLoiterPointForFixedWing)
EXPECT_FLOAT_EQ(loiter_point_lat_lon(1), loiter_point_lat_lon_expected(1)); EXPECT_FLOAT_EQ(loiter_point_lat_lon(1), loiter_point_lat_lon_expected(1));
geo.setProbeFunctionBehavior(FakeGeofence::LEFT_INSIDE_RIGHT_OUTSIDE); geo.setProbeFunctionBehavior(FakeGeofence::ProbeFunction::LEFT_INSIDE_RIGHT_OUTSIDE);
loiter_point_lat_lon = gf_avoidance.generateLoiterPointForFixedWing(gf_violation, &geo); loiter_point_lat_lon = gf_avoidance.generateLoiterPointForFixedWing(gf_violation, &geo);
loiter_point_lat_lon_expected = gf_avoidance.waypointFromBearingAndDistance(home_global, -M_PI_F * 0.5f, 20.0f); loiter_point_lat_lon_expected = gf_avoidance.waypointFromBearingAndDistance(home_global, -M_PI_F * 0.5f, 20.0f);
@@ -117,7 +117,7 @@ TEST_F(GeofenceBreachAvoidanceTest, generateLoiterPointForFixedWing)
EXPECT_FLOAT_EQ(loiter_point_lat_lon(0), loiter_point_lat_lon_expected(0)); EXPECT_FLOAT_EQ(loiter_point_lat_lon(0), loiter_point_lat_lon_expected(0));
EXPECT_FLOAT_EQ(loiter_point_lat_lon(1), loiter_point_lat_lon_expected(1)); EXPECT_FLOAT_EQ(loiter_point_lat_lon(1), loiter_point_lat_lon_expected(1));
geo.setProbeFunctionBehavior(FakeGeofence::RIGHT_INSIDE_LEFT_OUTSIDE); geo.setProbeFunctionBehavior(FakeGeofence::ProbeFunction::RIGHT_INSIDE_LEFT_OUTSIDE);
loiter_point_lat_lon = gf_avoidance.generateLoiterPointForFixedWing(gf_violation, &geo); loiter_point_lat_lon = gf_avoidance.generateLoiterPointForFixedWing(gf_violation, &geo);
loiter_point_lat_lon_expected = gf_avoidance.waypointFromBearingAndDistance(home_global, M_PI_F * 0.5f, 20.0f); loiter_point_lat_lon_expected = gf_avoidance.waypointFromBearingAndDistance(home_global, M_PI_F * 0.5f, 20.0f);
@@ -134,7 +134,7 @@ TEST_F(GeofenceBreachAvoidanceTest, generateLoiterPointForFixedWing)
TEST_F(GeofenceBreachAvoidanceTest, generateLoiterPointForMultirotor) TEST_F(GeofenceBreachAvoidanceTest, generateLoiterPointForMultirotor)
{ {
GeofenceBreachAvoidance gf_avoidance; GeofenceBreachAvoidance gf_avoidance(nullptr);
FakeGeofence geo; FakeGeofence geo;
struct map_projection_reference_s ref = {}; struct map_projection_reference_s ref = {};
Vector2d home_global(42.1, 8.2); Vector2d home_global(42.1, 8.2);
@@ -163,7 +163,7 @@ TEST_F(GeofenceBreachAvoidanceTest, generateLoiterPointForMultirotor)
gf_avoidance.setHorizontalVelocity(1000.0f); gf_avoidance.setHorizontalVelocity(1000.0f);
gf_avoidance.computeBrakingDistanceMultirotor(); gf_avoidance.computeBrakingDistanceMultirotor();
geo.setProbeFunctionBehavior(FakeGeofence::GF_BOUNDARY_20M_AHEAD); geo.setProbeFunctionBehavior(FakeGeofence::ProbeFunction::GF_BOUNDARY_20M_AHEAD);
Vector2d loiter_point = gf_avoidance.generateLoiterPointForMultirotor(gf_violation, &geo); Vector2d loiter_point = gf_avoidance.generateLoiterPointForMultirotor(gf_violation, &geo);
@@ -197,7 +197,7 @@ TEST_F(GeofenceBreachAvoidanceTest, generateLoiterPointForMultirotor)
TEST_F(GeofenceBreachAvoidanceTest, generateLoiterAltitudeForFixedWing) TEST_F(GeofenceBreachAvoidanceTest, generateLoiterAltitudeForFixedWing)
{ {
GeofenceBreachAvoidance gf_avoidance; GeofenceBreachAvoidance gf_avoidance(nullptr);
const float current_alt_amsl = 100.0f; const float current_alt_amsl = 100.0f;
const float vertical_test_point_dist = 10.0f; const float vertical_test_point_dist = 10.0f;
@@ -219,7 +219,7 @@ TEST_F(GeofenceBreachAvoidanceTest, generateLoiterAltitudeForFixedWing)
TEST_F(GeofenceBreachAvoidanceTest, generateLoiterAltitudeForMulticopter) TEST_F(GeofenceBreachAvoidanceTest, generateLoiterAltitudeForMulticopter)
{ {
GeofenceBreachAvoidance gf_avoidance; GeofenceBreachAvoidance gf_avoidance(nullptr);
const float climbrate = 10.0f; const float climbrate = 10.0f;
const float current_alt_amsl = 100.0f; const float current_alt_amsl = 100.0f;
geofence_violation_type_u gf_violation; geofence_violation_type_u gf_violation;
@@ -243,7 +243,7 @@ TEST_F(GeofenceBreachAvoidanceTest, generateLoiterAltitudeForMulticopter)
TEST_F(GeofenceBreachAvoidanceTest, maxDistToHomeViolationMulticopter) TEST_F(GeofenceBreachAvoidanceTest, maxDistToHomeViolationMulticopter)
{ {
GeofenceBreachAvoidance gf_avoidance; GeofenceBreachAvoidance gf_avoidance(nullptr);
FakeGeofence geo; FakeGeofence geo;
struct map_projection_reference_s ref = {}; struct map_projection_reference_s ref = {};
Vector2d home_global(42.1, 8.2); Vector2d home_global(42.1, 8.2);
@@ -276,7 +276,7 @@ TEST_F(GeofenceBreachAvoidanceTest, maxDistToHomeViolationMulticopter)
TEST_F(GeofenceBreachAvoidanceTest, maxDistToHomeViolationFixedWing) TEST_F(GeofenceBreachAvoidanceTest, maxDistToHomeViolationFixedWing)
{ {
GeofenceBreachAvoidance gf_avoidance; GeofenceBreachAvoidance gf_avoidance(nullptr);
FakeGeofence geo; FakeGeofence geo;
struct map_projection_reference_s ref = {}; struct map_projection_reference_s ref = {};
Vector2d home_global(42.1, 8.2); Vector2d home_global(42.1, 8.2);
src/modules/navigator/GeofenceBreachAvoidance/fake_geofence.hpp
+8 -8
View File
@@ -51,22 +51,22 @@ public:
virtual ~FakeGeofence() {}; virtual ~FakeGeofence() {};
virtual bool isInsidePolygonOrCircle(double lat, double lon, float altitude) bool isInsidePolygonOrCircle(double lat, double lon, float altitude) override
{ {
switch (_probe_function_behavior) { switch (_probe_function_behavior) {
case ALL_POINTS_OUTSIDE: { case ProbeFunction::ALL_POINTS_OUTSIDE: {
return _allPointsOutside(lat, lon, altitude); return _allPointsOutside(lat, lon, altitude);
} }
case LEFT_INSIDE_RIGHT_OUTSIDE: { case ProbeFunction::LEFT_INSIDE_RIGHT_OUTSIDE: {
return _left_inside_right_outside(lat, lon, altitude); return _left_inside_right_outside(lat, lon, altitude);
} }
case RIGHT_INSIDE_LEFT_OUTSIDE: { case ProbeFunction::RIGHT_INSIDE_LEFT_OUTSIDE: {
return _right_inside_left_outside(lat, lon, altitude); return _right_inside_left_outside(lat, lon, altitude);
} }
case GF_BOUNDARY_20M_AHEAD: { case ProbeFunction::GF_BOUNDARY_20M_AHEAD: {
return _gf_boundary_is_20m_ahead(lat, lon, altitude); return _gf_boundary_is_20m_ahead(lat, lon, altitude);
} }
@@ -75,19 +75,19 @@ public:
} }
} }
enum PROBE_FUNC_ENUM { enum class ProbeFunction {
ALL_POINTS_OUTSIDE = 0, ALL_POINTS_OUTSIDE = 0,
LEFT_INSIDE_RIGHT_OUTSIDE, LEFT_INSIDE_RIGHT_OUTSIDE,
RIGHT_INSIDE_LEFT_OUTSIDE, RIGHT_INSIDE_LEFT_OUTSIDE,
GF_BOUNDARY_20M_AHEAD GF_BOUNDARY_20M_AHEAD
}; };
void setProbeFunctionBehavior(PROBE_FUNC_ENUM func) {_probe_function_behavior = func;} void setProbeFunctionBehavior(ProbeFunction func) {_probe_function_behavior = func;}
private: private:
PROBE_FUNC_ENUM _probe_function_behavior = ALL_POINTS_OUTSIDE; ProbeFunction _probe_function_behavior = ProbeFunction::ALL_POINTS_OUTSIDE;
bool _allPointsOutside(double lat, double lon, float alt) bool _allPointsOutside(double lat, double lon, float alt)
{ {
src/modules/navigator/GeofenceBreachAvoidance/geofence_breach_avoidance.cpp
+2 -2
View File
@@ -38,8 +38,8 @@
using Vector2d = matrix::Vector2<double>; using Vector2d = matrix::Vector2<double>;
GeofenceBreachAvoidance::GeofenceBreachAvoidance() : GeofenceBreachAvoidance::GeofenceBreachAvoidance(ModuleParams *parent) :
ModuleParams(nullptr) ModuleParams(parent)
{ {
_paramHandle.param_mpc_jerk_max = param_find("MPC_JERK_MAX"); _paramHandle.param_mpc_jerk_max = param_find("MPC_JERK_MAX");
_paramHandle.param_mpc_acc_hor = param_find("MPC_ACC_HOR"); _paramHandle.param_mpc_acc_hor = param_find("MPC_ACC_HOR");
src/modules/navigator/GeofenceBreachAvoidance/geofence_breach_avoidance.h
+1 -1
View File
@@ -51,7 +51,7 @@ union geofence_violation_type_u {
class GeofenceBreachAvoidance : public ModuleParams class GeofenceBreachAvoidance : public ModuleParams
{ {
public: public:
GeofenceBreachAvoidance(); GeofenceBreachAvoidance(ModuleParams *parent);
~GeofenceBreachAvoidance() = default; ~GeofenceBreachAvoidance() = default;
src/modules/navigator/navigator_main.cpp
+1 -2
View File
@@ -78,6 +78,7 @@ Navigator::Navigator() :
ModuleParams(nullptr), ModuleParams(nullptr),
_loop_perf(perf_alloc(PC_ELAPSED, "navigator")), _loop_perf(perf_alloc(PC_ELAPSED, "navigator")),
_geofence(this), _geofence(this),
_gf_breach_avoidance(this),
_mission(this), _mission(this),
_loiter(this), _loiter(this),
_takeoff(this), _takeoff(this),
@@ -208,8 +209,6 @@ Navigator::run()
// update parameters from storage // update parameters from storage
params_update(); params_update();
_gf_breach_avoidance.updateParameters();
} }
_land_detected_sub.update(&_land_detected); _land_detected_sub.update(&_land_detected);