Hivemapper features

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 = "[email protected]", role = c("aut", "cre")),
   person("Danilo Roberti Alves de", "Almeida", email = "[email protected]", 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

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,31 +235,51 @@ 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")
   }
 
 
   # Transform to WGS84 latitude and longitude
   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 = [email protected]
+  dfLitchi$photo_distinterval = [email protected] * flightSpeedMs * photos
+  dfLitchi$photo_timeinterval = [email protected] * 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,18 +328,113 @@ 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
       output2 = paste0(first, i, second)
       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([email protected], appendLF = FALSE)
   message(" s")
+  message("Photo distance:    ", appendLF = FALSE)
+  message([email protected] * [email protected] / 3.6, appendLF = FALSE)
+  message(" m")
   message("Flight speed:      ", appendLF = FALSE)
   message(round([email protected], 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")

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 <https://github.com/ramnathv>
 #'
 #' @param xy A matrix of xy values from which to calculate the minimum oriented