Support custom launch in sub missions

Author: evanmoss

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

R/main.R

@@ -2,6 +2,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
 #'
 #'
@@ -249,13 +266,12 @@ litchi.plan = function(roi, output,
 
 
   # Check if launch point has been specified before inserting it as way-point 1
-  if ((launch[1] == 0) && (launch[2] == 0)) {
-    message("No launch point specified")
+  hasCustomLaunch = (launch[1] != 0) || (launch[2] != 0)
+  if (hasCustomLaunch) {
+    message("Launch point specified: ", launch[1], ',', launch[2])
+    MAX_WAYPOINTS = MAX_WAYPOINTS - 1
   } else {
-    launchdf = data.frame(launch[1], launch[2], FALSE, FALSE)
-    names(launchdf) = c("x", "y", "isCurve", "takePhoto")
-    tempdf = rbind(launchdf, waypoints)
-    waypoints = tempdf
+    message("No launch point specified")
   }
 
 
@@ -293,7 +309,6 @@ litchi.plan = function(roi, output,
   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)
   distAcum = c(0,cumsum(dists))
@@ -313,11 +328,112 @@ litchi.plan = function(roi, output,
 
     dfLitchi$split = rep(1:nBreaks, diff(c(0, waypointsBreak, finalSize)))
     splits = split.data.frame(dfLitchi, f = dfLitchi$split)
+
+    if (hasCustomLaunch) {
+      p0x = launch[[1]][1]
+      p0y = launch[[2]][1]
+      message(p0x)
+      message(p0y)
+
+      launch84 = rgdal::rawTransform(roi@proj4string@projargs, wgs84, as.integer(1), launch[[1]], launch[[2]])
+      message(roi@proj4string@projargs)
+      message(wgs84)
+
+      overage = NULL
+
+      for (i in 1:length(splits)) {
+        message(i)
+        message(splits[[i]])
+
+        if (!is.null(overage)) {
+          message('handling overage')
+          message(overage)
+          splits[[i]] = rbind(overage, splits[[i]])
+        } else {
+          message('no overage')
+        }
+
+        message('mercator...')
+        # mercator = rgdal::rawTransform(wgs84, roi@proj4string@projargs, length(splits[[i]]), splits[[i]]$latitude, splits[[i]]$longitude)
+        mercator = rgdal::rawTransform("+init=epsg:4326", "+init=epsg:3857", length(splits[[i]]), splits[[i]]$latitude, splits[[i]]$longitude)
+        message("MERCATOR")
+        # message(mercator)
+        # p1x = splits[[i]]$latitude[1]
+        # p1y = splits[[i]]$longitude[1]
+        message(colnames(mercator))
+        p1x = mercator[[1]][1]
+        p1y = mercator[[2]][1]
+        message(p1x)
+        message(p1y)
+        dx = p1x - p0x
+        dy = p1y - p0y
+        distance = earth.dist(launch84[[1]][1], launch84[[2]][1], splits[[i]]$longitude[1], splits[[i]]$latitude[1])
+        message(launch84[[2]][1], launch84[[1]][1], splits[[i]]$longitude[1], splits[[i]]$latitude[1])
+        message(distance)
+        message(max.waypoints.distance)
+
+
+        interpPtsToAdd = floor(distance / max.waypoints.distance)
+        nPtsToAdd = 1 + interpPtsToAdd
+
+        message('ptsToAdd...')
+        ptsToAdd = rbind(splits[[i]][1:min(nrow(splits[[i]]), nPtsToAdd),])
+        ptsToAdd$curvesize.m. <- 0
+        ptsToAdd$photo_distinterval <- 0
+        ptsToAdd$photo_timeinterval <- 0
+
+        message(nPtsToAdd)
+
+        toConvert = data.frame(
+          lat = numeric(nPtsToAdd),
+          lon = numeric(nPtsToAdd)
+        )
+
+        toConvert[1,] = c(p0x, p0y)
+        if (nPtsToAdd > 1) {
+          for (j in 2:nPtsToAdd) {
+            message('interpolating ', nPtsToAdd, ': ', c(p0x + ((j - 1) / nPtsToAdd) * dx, p0y + ((j - 1) / nPtsToAdd) * dy))
+            toConvert[j,] <- c(p0x + ((j - 1) / nPtsToAdd) * dx, p0y + ((j - 1) / nPtsToAdd) * dy)
+          }
+        }
+
+        wgs84 = rgdal::rawTransform("+init=epsg:3857", "+init=epsg:4326", length(toConvert), toConvert$lat, toConvert$lon)
+        message("WGS84")
+        message(wgs84)
+
+        ptsToAdd$latitude = wgs84[[2]]
+        ptsToAdd$longitude = wgs84[[1]]
+
+        dataSize = length(splits[[i]])
+        lim = min(MAX_WAYPOINTS + 1, dataSize)
+        rem = 0
+        if (dataSize + nPtsToAdd > MAX_WAYPOINTS + 1) {
+          rem = dataSize + nPtsToAdd - (MAX_WAYPOINTS + 1)          
+        }
+
+        splits[[i]] = rbind(ptsToAdd, splits[[i]][1:lim,])
+
+        if (rem > 0) {
+          overage = splits[[i]][lim + 1:length(splits[[i]]),]
+        } else {
+          overage = NULL
+        }
+
+        message("FINAL ", i)
+        message(splits[[i]])
+      }
+
+      if (!is.null(overage)) {
+        splits[length(splits) + 1] = rbind(overage)
+      }
+    }
+
     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.")
     }

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