bool MissionFeasibilityChecker::checkGeofence(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence, float home_alt)
{
/* Check if all mission items are inside the geofence (if we have a valid geofence) */
if (geofence.valid()) {
for (size_t i = 0; i < nMissionItems; i++) {
struct mission_item_s missionitem;
const ssize_t len = sizeof(missionitem);
if (dm_read(dm_current, i, &missionitem, len) != len) {
/* not supposed to happen unless the datamanager can't access the SD card, etc. */
return false;
}
// Geofence function checks against home altitude amsl
missionitem.altitude = missionitem.altitude_is_relative
? missionitem.altitude + home_alt
: missionitem.altitude;
if (MissionBlock::item_contains_position(&missionitem) &&
!geofence.inside(missionitem)) {
mavlink_log_critical(_mavlink_log_pub, "Geofence violation for waypoint %d", i + 1);
return false;
}
}
}
return true;
}
bool MissionFeasibilityChecker::checkGeofence(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence)
{
/* Check if all mission items are inside the geofence (if we have a valid geofence) */
if (geofence.valid()) {
for (size_t i = 0; i < nMissionItems; i++) {
struct mission_item_s missionitem;
const ssize_t len = sizeof(missionitem);
if (dm_read(dm_current, i, &missionitem, len) != len) {
/* not supposed to happen unless the datamanager can't access the SD card, etc. */
return false;
}
if (!geofence.inside_polygon(missionitem.lat, missionitem.lon, missionitem.altitude)) {
mavlink_log_critical(_mavlink_fd, "Geofence violation for waypoint %d", i);
return false;
}
}
}
return true;
}
