diff --git a/src/modules/navigator/GeofenceBreachAvoidance/GeofenceBreachAvoidanceTest.cpp b/src/modules/navigator/GeofenceBreachAvoidance/GeofenceBreachAvoidanceTest.cpp index 896ab1f00d..f3c6483a0f 100644 --- a/src/modules/navigator/GeofenceBreachAvoidance/GeofenceBreachAvoidanceTest.cpp +++ b/src/modules/navigator/GeofenceBreachAvoidance/GeofenceBreachAvoidanceTest.cpp @@ -51,7 +51,7 @@ public: TEST_F(GeofenceBreachAvoidanceTest, waypointFromBearingAndDistance) { - GeofenceBreachAvoidance gf_avoidance; + GeofenceBreachAvoidance gf_avoidance(nullptr); struct map_projection_reference_s ref = {}; Vector2d home_global(42.1, 8.2); map_projection_init(&ref, home_global(0), home_global(1)); @@ -86,7 +86,7 @@ TEST_F(GeofenceBreachAvoidanceTest, waypointFromBearingAndDistance) TEST_F(GeofenceBreachAvoidanceTest, generateLoiterPointForFixedWing) { - GeofenceBreachAvoidance gf_avoidance; + GeofenceBreachAvoidance gf_avoidance(nullptr); FakeGeofence geo; struct map_projection_reference_s ref = {}; 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)); - 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_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(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_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) { - GeofenceBreachAvoidance gf_avoidance; + GeofenceBreachAvoidance gf_avoidance(nullptr); FakeGeofence geo; struct map_projection_reference_s ref = {}; Vector2d home_global(42.1, 8.2); @@ -163,7 +163,7 @@ TEST_F(GeofenceBreachAvoidanceTest, generateLoiterPointForMultirotor) gf_avoidance.setHorizontalVelocity(1000.0f); 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); @@ -197,7 +197,7 @@ TEST_F(GeofenceBreachAvoidanceTest, generateLoiterPointForMultirotor) TEST_F(GeofenceBreachAvoidanceTest, generateLoiterAltitudeForFixedWing) { - GeofenceBreachAvoidance gf_avoidance; + GeofenceBreachAvoidance gf_avoidance(nullptr); const float current_alt_amsl = 100.0f; const float vertical_test_point_dist = 10.0f; @@ -219,7 +219,7 @@ TEST_F(GeofenceBreachAvoidanceTest, generateLoiterAltitudeForFixedWing) TEST_F(GeofenceBreachAvoidanceTest, generateLoiterAltitudeForMulticopter) { - GeofenceBreachAvoidance gf_avoidance; + GeofenceBreachAvoidance gf_avoidance(nullptr); const float climbrate = 10.0f; const float current_alt_amsl = 100.0f; geofence_violation_type_u gf_violation; @@ -243,7 +243,7 @@ TEST_F(GeofenceBreachAvoidanceTest, generateLoiterAltitudeForMulticopter) TEST_F(GeofenceBreachAvoidanceTest, maxDistToHomeViolationMulticopter) { - GeofenceBreachAvoidance gf_avoidance; + GeofenceBreachAvoidance gf_avoidance(nullptr); FakeGeofence geo; struct map_projection_reference_s ref = {}; Vector2d home_global(42.1, 8.2); @@ -276,7 +276,7 @@ TEST_F(GeofenceBreachAvoidanceTest, maxDistToHomeViolationMulticopter) TEST_F(GeofenceBreachAvoidanceTest, maxDistToHomeViolationFixedWing) { - GeofenceBreachAvoidance gf_avoidance; + GeofenceBreachAvoidance gf_avoidance(nullptr); FakeGeofence geo; struct map_projection_reference_s ref = {}; Vector2d home_global(42.1, 8.2); diff --git a/src/modules/navigator/GeofenceBreachAvoidance/fake_geofence.hpp b/src/modules/navigator/GeofenceBreachAvoidance/fake_geofence.hpp index d6c8faa044..d276d2d6cd 100644 --- a/src/modules/navigator/GeofenceBreachAvoidance/fake_geofence.hpp +++ b/src/modules/navigator/GeofenceBreachAvoidance/fake_geofence.hpp @@ -51,22 +51,22 @@ public: virtual ~FakeGeofence() {}; - virtual bool isInsidePolygonOrCircle(double lat, double lon, float altitude) + bool isInsidePolygonOrCircle(double lat, double lon, float altitude) override { switch (_probe_function_behavior) { - case ALL_POINTS_OUTSIDE: { + case ProbeFunction::ALL_POINTS_OUTSIDE: { return _allPointsOutside(lat, lon, altitude); } - case LEFT_INSIDE_RIGHT_OUTSIDE: { + case ProbeFunction::LEFT_INSIDE_RIGHT_OUTSIDE: { 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); } - case GF_BOUNDARY_20M_AHEAD: { + case ProbeFunction::GF_BOUNDARY_20M_AHEAD: { 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, LEFT_INSIDE_RIGHT_OUTSIDE, RIGHT_INSIDE_LEFT_OUTSIDE, GF_BOUNDARY_20M_AHEAD }; - void setProbeFunctionBehavior(PROBE_FUNC_ENUM func) {_probe_function_behavior = func;} + void setProbeFunctionBehavior(ProbeFunction func) {_probe_function_behavior = func;} 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) { diff --git a/src/modules/navigator/GeofenceBreachAvoidance/geofence_breach_avoidance.cpp b/src/modules/navigator/GeofenceBreachAvoidance/geofence_breach_avoidance.cpp index 1185e73f7d..691a2f9708 100644 --- a/src/modules/navigator/GeofenceBreachAvoidance/geofence_breach_avoidance.cpp +++ b/src/modules/navigator/GeofenceBreachAvoidance/geofence_breach_avoidance.cpp @@ -38,8 +38,8 @@ using Vector2d = matrix::Vector2; -GeofenceBreachAvoidance::GeofenceBreachAvoidance() : - ModuleParams(nullptr) +GeofenceBreachAvoidance::GeofenceBreachAvoidance(ModuleParams *parent) : + ModuleParams(parent) { _paramHandle.param_mpc_jerk_max = param_find("MPC_JERK_MAX"); _paramHandle.param_mpc_acc_hor = param_find("MPC_ACC_HOR"); diff --git a/src/modules/navigator/GeofenceBreachAvoidance/geofence_breach_avoidance.h b/src/modules/navigator/GeofenceBreachAvoidance/geofence_breach_avoidance.h index 8fdbc6dc18..4cce904562 100644 --- a/src/modules/navigator/GeofenceBreachAvoidance/geofence_breach_avoidance.h +++ b/src/modules/navigator/GeofenceBreachAvoidance/geofence_breach_avoidance.h @@ -51,7 +51,7 @@ union geofence_violation_type_u { class GeofenceBreachAvoidance : public ModuleParams { public: - GeofenceBreachAvoidance(); + GeofenceBreachAvoidance(ModuleParams *parent); ~GeofenceBreachAvoidance() = default; diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 2613589d3d..28a618e6cb 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -78,6 +78,7 @@ Navigator::Navigator() : ModuleParams(nullptr), _loop_perf(perf_alloc(PC_ELAPSED, "navigator")), _geofence(this), + _gf_breach_avoidance(this), _mission(this), _loiter(this), _takeoff(this), @@ -208,8 +209,6 @@ Navigator::run() // update parameters from storage params_update(); - - _gf_breach_avoidance.updateParameters(); } _land_detected_sub.update(&_land_detected);