diff --git a/DESCRIPTION b/DESCRIPTION index db01147..07fcf6c 100644 --- a/DESCRIPTION +++ b/DESCRIPTION @@ -1,7 +1,7 @@ Package: flightplanning Type: Package -Title: UAV Flight Planning -Version: 0.8.4 +Title: Hivemapper/UAV Flight Planning +Version: 0.8.14 Authors@R: c( person("Caio", "Hamamura", email = "caiohamamura@gmail.com", role = c("aut", "cre")), person("Danilo Roberti Alves de", "Almeida", email = "daniloflorestas@gmail.com", role = c("aut")), @@ -22,5 +22,3 @@ License: MIT + file LICENSE Encoding: UTF-8 LazyData: true RoxygenNote: 7.1.0 -URL: https://github.com/caiohamamura/flightplanning-R -BugReports: https://github.com/caiohamamura/flightplanning-R/issues diff --git a/R/main.R b/R/main.R index a3d2d25..32134b0 100644 --- a/R/main.R +++ b/R/main.R @@ -1,6 +1,23 @@ MIN_PHOTO_INTERVAL = 2 DIAG_35MM = sqrt(36^2 + 24^2) # Classical 35mm film diagonal - +MAX_WAYPOINTS = 99 + +# Calculate distance in meters between two points +earth.dist <- function (long1, lat1, long2, lat2) +{ + rad <- pi/180 + a1 <- lat1 * rad + a2 <- long1 * rad + b1 <- lat2 * rad + b2 <- long2 * rad + dlon <- b2 - a2 + dlat <- b1 - a1 + a <- (sin(dlat/2))^2 + cos(a1) * cos(b1) * (sin(dlon/2))^2 + c <- 2 * atan2(sqrt(a), sqrt(1 - a)) + R <- 6378.145 + d <- R * c + return(d * 1000) +} #' Function to generate Litchi csv flight plan #' @@ -61,7 +78,7 @@ DIAG_35MM = sqrt(36^2 + 24^2) # Classical 35mm film diagonal litchi.plan = function(roi, output, flight.params, gimbal.pitch.angle = -90, flight.lines.angle = -1, max.waypoints.distance = 2000, - max.flight.time = 15, starting.point = 1) { + max.flight.time = 15, starting.point = 1, launch = list(0, 0)) { # Check parameters if (class(roi)[1] != "SpatialPolygonsDataFrame") stop("ROI is not a valid polygon layer") @@ -96,6 +113,8 @@ litchi.plan = function(roi, output, # based on angle and width/height offsets # width offsets (between flightlines) nLines = ceiling(width / flightLineDistance) + 1 + # Then need to update flightLineDistance to avoid offset/quantization errors + flightLineDistance = width / (nLines - 1) xWidths = (-nLines/2):(nLines/2) * flightLineDistance xWidths = rep(xWidths, each=2) @@ -111,6 +130,14 @@ litchi.plan = function(roi, output, # Switch position of the first point + if (starting.point == 0) { + # In this case we will automatically pick the best starting point + # TODO check if launch is valid and not (0,0) + # TODO figure out closest corner in shape to launch point and then set starting.point to 1-4 + # But until then, just use and set default value + starting.point == 1 + } + if (starting.point == 2) { yHeights = c(heightPHalf, heightMHalf) } else if (starting.point == 3) { @@ -208,24 +235,43 @@ litchi.plan = function(roi, output, } waypoints = wptsMatrix + # Break if distance greater than the maxWaypointDistance - waypointsXY = waypoints[, c("x", "y")] - distances = sqrt(diff(waypoints$x)**2 + diff(waypoints$y)**2) - breakIdx = distances > max.waypoints.distance - - newSize = nrow(waypoints) + sum(breakIdx) - if (newSize != nrow(waypoints)) { - midpoints = (waypointsXY[breakIdx,] + waypointsXY[-1,][breakIdx,])/2 - waypoints2 = data.frame(x = numeric(newSize), - y = numeric(newSize), - isCurve = FALSE, - takePhoto = TRUE) - - pos = seq_along(breakIdx)[breakIdx] - idx = pos + order(pos) - waypoints2[idx,1:2] = midpoints - waypoints2[-idx,] = waypoints - waypoints = waypoints2 + # A single pass only adds one intermediate waypoint even if a leg is longer than max, but more than one intermediate point may be needed. + # We can iterate this process as a temp fix but we may be adding more intermediate waypoints than strictly necessary--e.g. when 2 intermediate points will suffice we will get 3. + retest = TRUE + while (retest) { + waypointsXY = waypoints[, c("x", "y")] + distances = sqrt(diff(waypoints$x)**2 + diff(waypoints$y)**2) + breakIdx = distances > max.waypoints.distance + + newSize = nrow(waypoints) + sum(breakIdx) + if (newSize != nrow(waypoints)) { + midpoints = (waypointsXY[breakIdx,] + waypointsXY[-1,][breakIdx,])/2 + waypoints2 = data.frame(x = numeric(newSize), + y = numeric(newSize), + isCurve = FALSE, + takePhoto = TRUE) + + pos = seq_along(breakIdx)[breakIdx] + idx = pos + order(pos) + waypoints2[idx,1:2] = midpoints + waypoints2[-idx,] = waypoints + waypoints = waypoints2 + } + else { + retest = FALSE + } + } + + + # Check if launch point has been specified before inserting it as way-point 1 + hasCustomLaunch = (launch[1] != 0) || (launch[2] != 0) + if (hasCustomLaunch) { + message("Launch point specified: ", launch[1], ',', launch[2]) + MAX_WAYPOINTS = MAX_WAYPOINTS - 1 + } else { + message("No launch point specified") } @@ -233,6 +279,7 @@ litchi.plan = function(roi, output, transform = rgdal::rawTransform(roi@proj4string@projargs, wgs84, n=nrow(waypoints), x=waypoints[,1], y=waypoints[,2]) lats = transform[[2]] lngs = transform[[1]] + photos = waypoints[,4] graphics::plot(waypoints[,1:2]) graphics::polygon(roi@polygons[[1]]@Polygons[[1]]@coords) @@ -251,13 +298,16 @@ litchi.plan = function(roi, output, dfLitchi$latitude = lats dfLitchi$longitude = lngs dfLitchi$altitude.m. = flight.params@height + dfLitchi$altitudemode = 1 dfLitchi$speed.m.s. = flightSpeedMs dfLitchi$heading.deg. = c(finalHeading, 90) dfLitchi$curvesize.m. = 0 dfLitchi$curvesize.m.[waypoints$isCurve==1] = flightLineDistance*0.5 - dfLitchi$photo_distinterval = flight.params@ground.height + dfLitchi$photo_distinterval = flight.params@photo.interval * flightSpeedMs * photos + dfLitchi$photo_timeinterval = flight.params@photo.interval * photos dfLitchi$gimbalpitchangle = gimbal.pitch.angle - + dfLitchi$actiontype1 = 5 + dfLitchi$actionparam1 = gimbal.pitch.angle # Split the flight if is too long dists = sqrt(diff(waypoints[,1])**2+diff(waypoints[,2])**2) @@ -266,9 +316,9 @@ litchi.plan = function(roi, output, finalSize = nrow(dfLitchi) totalFlightTime = flightTime[finalSize] dfLitchi$split = 1 - if (totalFlightTime > max.flight.time) { + if ((totalFlightTime > max.flight.time) || (nrow(waypoints) > MAX_WAYPOINTS)) { indexes = seq_len(finalSize) - nBreaks = ceiling(totalFlightTime/max.flight.time) + nBreaks = max(ceiling(totalFlightTime/max.flight.time), ceiling(nrow(waypoints)/MAX_WAYPOINTS)) breaks = seq(0, flightTime[finalSize], length.out = nBreaks+1)[c(-1, -nBreaks-1)] endWaypointsIndex = indexes[waypoints$isCurve & (seq_len(finalSize) %% 2 == 0)] endWaypoints = flightTime[waypoints$isCurve & (seq_len(finalSize) %% 2 == 0)] @@ -278,10 +328,105 @@ litchi.plan = function(roi, output, dfLitchi$split = rep(1:nBreaks, diff(c(0, waypointsBreak, finalSize))) splits = split.data.frame(dfLitchi, f = dfLitchi$split) - message("Your flight was splitted in ", length(splits), " splits, -because the total time would be ", round(totalFlightTime, 2), " minutes.") - message("They were saved as:") - first = substr(output, 1, nchar(output)-4) + + if (hasCustomLaunch) { + p0x = launch[[1]][1] + p0y = launch[[2]][1] + + message("adding custom launch point to submissions") + + launch84 = rgdal::rawTransform(roi@proj4string@projargs, wgs84, as.integer(1), launch[[1]], launch[[2]]) + + overage = NULL + + for (i in 1:length(splits)) { + message("starting ", i) + if (!is.null(overage)) { + message("setting ", i, " to ", "overage (", nrow(overage), ") and ", nrow(splits[[i]])) + splits[[i]] = rbind(overage, splits[[i]]) + } + + mercator = rgdal::rawTransform(wgs84, roi@proj4string@projargs, nrow(splits[[i]]), splits[[i]]$longitude, splits[[i]]$latitude) + p1x = mercator[[1]][1] + p1y = mercator[[2]][1] + dx = p1x - p0x + dy = p1y - p0y + distance = earth.dist(launch84[[1]][1], launch84[[2]][1], splits[[i]]$longitude[1], splits[[i]]$latitude[1]) + + interpPtsToAdd = floor(distance / max.waypoints.distance) + nPtsToAdd = 1 + interpPtsToAdd + + message("adding ", nPtsToAdd, " points") + + ptsToAdd = rbind(splits[[1]][1:nPtsToAdd,]) + ptsToAdd$split <- i + ptsToAdd$curvesize.m. <- 0 + ptsToAdd$photo_distinterval <- 0 + ptsToAdd$photo_timeinterval <- 0 + + toConvert = data.frame( + lat = numeric(nPtsToAdd), + lon = numeric(nPtsToAdd) + ) + + toConvert[1,] = c(p0x, p0y) + if (nPtsToAdd > 1) { + for (j in 2:nPtsToAdd) { + toConvert[j,] <- c(p0x + ((j - 1) / nPtsToAdd) * dx, p0y + ((j - 1) / nPtsToAdd) * dy) + } + } + + wgs84D = rgdal::rawTransform(roi@proj4string@projargs, wgs84, nrow(toConvert), toConvert$lat, toConvert$lon) + + ptsToAdd$latitude = wgs84D[[2]] + ptsToAdd$longitude = wgs84D[[1]] + + splitSize = nrow(splits[[i]]) + totalSize = splitSize + nPtsToAdd + rem = 0 + if (totalSize > MAX_WAYPOINTS + 1) { + rem = totalSize - (MAX_WAYPOINTS + 1) + } + + if (rem > 0) { + message("setting overage to ", splitSize + 1 - rem, " : ", splitSize) + message(class(splits[[i]])) + message(splits[[i]][splitSize + 1 - rem:splitSize,]) + message(colnames(splits[[i]])) + message(splits[[i]]) + message(colnames(splits[[i]][splitSize + 1 - rem:splitSize,])) + message(rownames(splits[[i]][splitSize + 1 - rem:splitSize,])) + overage = rbind(splits[[i]][splitSize + 1 - rem:splitSize,]) + message("overage has ", nrow(overage)) + message(overage) + message(overage[splitSize + 1 - rem: splitSize,]) + } else { + message("setting overage to NULL") + overage = NULL + } + + message("setting ", i, " to ptsToAdd (", nrow(ptsToAdd), ") + splits 1 : ", splitSize - rem) + splits[[i]] = rbind(ptsToAdd, splits[[i]][1:splitSize - rem,]) + } + + if (!is.null(overage)) { + newIdx = length(splits) + 1 + splits[[newIdx]] = rbind(overage) + splits[[newIdx]]$split = newIdx + } + } + + if (nrow(waypoints) > MAX_WAYPOINTS) { + message("Your flight was split into ", length(splits), " sub-flights, +because the number of waypoints ", nrow(waypoints), " exceeds the maximum of ", MAX_WAYPOINTS, ".") + } + else { + # XXX flight time doesn't include custom launch point stuff + message("Your flight was split into ", length(splits), " sub-flights, +because the total flight time of ", round(totalFlightTime, 2), " minutes exceeds the max of ", max.flight.time, " minutes.") + } + message("The flights were saved as:") + first = paste0(substr(output, 1, nchar(output)-4), "_") second = substr(output, nchar(output)-3, nchar(output)) for (dataSplit in splits) { i = dataSplit[1, ]$split @@ -289,7 +434,7 @@ because the total time would be ", round(totalFlightTime, 2), " minutes.") write.csv(dataSplit[,-ncol(dataSplit)], output2, row.names = FALSE) message(output2) } - output2 = paste0(first, "_entire", second) + output2 = paste0(first, "entire", second) write.csv(dfLitchi, output2, row.names = FALSE) message("The entire flight plan was saved as:") message(output2) @@ -313,9 +458,14 @@ because the total time would be ", round(totalFlightTime, 2), " minutes.") message("Photo interval: ", appendLF = FALSE) message(flight.params@photo.interval, appendLF = FALSE) message(" s") + message("Photo distance: ", appendLF = FALSE) + message(flight.params@photo.interval * flight.params@flight.speed.kmh / 3.6, appendLF = FALSE) + message(" m") message("Flight speed: ", appendLF = FALSE) message(round(flight.params@flight.speed.kmh, 4), appendLF = FALSE) message(" km/h") + message("Total number of waypoints", appendLF = FALSE) + message(nrow(waypoints)) message("Flight lines angle: ", appendLF = FALSE) message(round(alpha, 4)) message('Total flight time: ', appendLF = FALSE) @@ -359,7 +509,8 @@ flight.parameters = function( image.height.px = 3000, side.overlap = 0.8, front.overlap = 0.8, - flight.speed.kmh = 54) { + flight.speed.kmh = 54, + max.gsd = 0) { if (is.na(gsd) == is.na(height)) { stop("You must specify either gsd or height!") @@ -371,6 +522,16 @@ flight.parameters = function( mult.factor = (height / focal.length35) diag.ground = DIAG_35MM * mult.factor gsd = diag.ground / image.diag.px * 100 + if ((max.gsd != 0) && (gsd > max.gsd)) { + height = height * max.gsd / gsd + warning(paste0("GSD of ", gsd, " is above target of ", max.gsd, " so adjusting height down to ", height)) + # Repeat as a Warning message because warnings are not always getting through + message("WARNING: GSD of ", gsd, " is above target of ", max.gsd, " so adjusting height down to ", height) + mult.factor = (height / focal.length35) + diag.ground = DIAG_35MM * mult.factor + gsd = diag.ground / image.diag.px * 100 + message("Final GSD is ", gsd) + } groundWidth = image.width.px * gsd / 100 } else { groundWidth = image.width.px * gsd / 100 @@ -395,16 +556,22 @@ flight.parameters = function( groundAllowedOffset = groundHeight - groundHeightOverlap photoInterval = groundAllowedOffset / flightSpeedMs if (photoInterval < MIN_PHOTO_INTERVAL) { - photoInterval = 2 + photoInterval = MIN_PHOTO_INTERVAL flightSpeedMs = groundAllowedOffset / photoInterval - flight.speed.kmh = flightSpeedMs*3.6 + flight.speed.kmh = flightSpeedMs * 3.6 warning(paste0("Speed had to be lowered because frequency of photos would be too high - New speed: ", flight.speed.kmh, "km/h")) - } else if ((photoInterval %% 1) > 1e-4) { - photoInterval = ceiling(photoInterval) + New speed: ", flight.speed.kmh, " km/h")) + # Repeat as a Warning message because warnings are not always getting through + message("WARNING: Speed had to be lowered because frequency of photos would be too high + New speed: ", flight.speed.kmh, " km/h") + } else if ((photoInterval %% .1) > 1e-4) { + # Allow 0.1s resolution because integer seconds blocks useful drone speeds + photoInterval = ceiling(photoInterval * 10) / 10 flightSpeedMs = groundAllowedOffset / photoInterval flight.speed.kmh = flightSpeedMs*3.6 - warning(paste0("Speed lowered to ", flight.speed.kmh, "km/h to round up photo interval time\n")) + warning(paste0("Speed lowered to ", flight.speed.kmh, " km/h to round up photo interval time to ", photoInterval, " seconds")) + # Repeat as a Warning message because warnings are not always getting through + message("WARNING: Speed lowered to ", flight.speed.kmh, " km/h to round up photo interval time to ", photoInterval, " seconds") } params = methods::new("Flight Parameters") diff --git a/R/utils.R b/R/utils.R index 864257f..ba17bb8 100644 --- a/R/utils.R +++ b/R/utils.R @@ -2,7 +2,7 @@ #' #' @description #' Calculates the minimum oriented bounding box using the -#' rotating claipers algorithm. +#' rotating calipers algorithm. #' Credits go to Daniel Wollschlaeger #' #' @param xy A matrix of xy values from which to calculate the minimum oriented