diff --git a/src/modules/navigator/mission_feasibility_checker.cpp b/src/modules/navigator/mission_feasibility_checker.cpp index 6933667b37db..cbf204c16bdd 100644 --- a/src/modules/navigator/mission_feasibility_checker.cpp +++ b/src/modules/navigator/mission_feasibility_checker.cpp @@ -150,6 +150,64 @@ MissionFeasibilityChecker::checkMissionAgainstGeofence(const mission_s &mission, i + 1); return false; } + + if (!checkLoiterPerimeterAgainstGeofence(missionitem)) { + mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Geofence violation for loiter radius of waypoint %zu\t", + i + 1); + // clang-tidy misparses the explicit "" template argument + // together with the call's parentheses as a chained relational + // comparison ("send < int16_t > (...)"); this is not one. + // NOLINTNEXTLINE(bugprone-chained-comparison) + events::send(events::ID("navigator_mis_geofence_violation_loiter"), {events::Log::Error, events::LogInternal::Info}, + "Geofence violation for loiter radius of waypoint {1}", + i + 1); + return false; + } + } + } + + return true; +} + +bool +MissionFeasibilityChecker::checkLoiterPerimeterAgainstGeofence(const mission_item_s &item) +{ + switch (item.nav_cmd) { + case NAV_CMD_LOITER_UNLIMITED: + case NAV_CMD_LOITER_TIME_LIMIT: + case NAV_CMD_LOITER_TO_ALT: + break; + + default: + return true; + } + + // negative radius means counter-clockwise; the geometry is the same either way + const float radius = fabsf(item.loiter_radius); + + if (!PX4_ISFINITE(radius) || radius < FLT_EPSILON) { + return true; + } + + // Sample the perimeter finely enough that the chord between two samples cannot + // step over a small geofence, but keep the count bounded so that upload time + // stays predictable for large radii. + static constexpr float kMaxArcStep = 10.f; // metres between samples + static constexpr int kMinSamples = 8; + static constexpr int kMaxSamples = 64; + + const int num_samples = math::constrain(static_cast(ceilf(2.f * M_PI_F * radius / kMaxArcStep)), + kMinSamples, kMaxSamples); + + for (int s = 0; s < num_samples; s++) { + const float bearing = (2.f * M_PI_F * static_cast(s)) / static_cast(num_samples); + double lat = 0.0; + double lon = 0.0; + + waypoint_from_heading_and_distance(item.lat, item.lon, bearing, radius, &lat, &lon); + + if (!_navigator->get_geofence().checkPointAgainstAllGeofences(lat, lon, item.altitude)) { + return false; } } diff --git a/src/modules/navigator/mission_feasibility_checker.h b/src/modules/navigator/mission_feasibility_checker.h index 88b36b6f9b58..6ed305f2aa47 100644 --- a/src/modules/navigator/mission_feasibility_checker.h +++ b/src/modules/navigator/mission_feasibility_checker.h @@ -59,6 +59,15 @@ class MissionFeasibilityChecker: public ModuleParams bool checkMissionAgainstGeofence(const mission_s &mission, float home_alt, bool home_valid); void logDatamanReadFailure(const size_t mission_item, const uint8_t dataman_id); + /** + * Check the perimeter of a loiter pattern against all geofences. + * The centre of a loiter item is already covered by the per-item point check; this + * samples the circle the vehicle actually flies, which the centre point does not + * represent when the radius is large. + * Returns true for items that are not loiter items or that have no usable radius. + */ + bool checkLoiterPerimeterAgainstGeofence(const mission_item_s &item); + public: MissionFeasibilityChecker(Navigator *navigator, DatamanClient &dataman_client) : ModuleParams(nullptr),