public List<MissionItem> createDronie(Coord2D start, Coord2D end) {
final int startAltitude = 4;
final int roiDistance = -8;
Coord2D slowDownPoint = GeoTools.pointAlongTheLine(start, end, 5);
double defaultSpeed = myDrone.getSpeed().getSpeedParameter();
if (defaultSpeed == -1) {
defaultSpeed = 5;
}
List<MissionItem> dronieItems = new ArrayList<MissionItem>();
dronieItems.add(new TakeoffImpl(this, startAltitude));
dronieItems.add(
new RegionOfInterestImpl(
this, new Coord3D(GeoTools.pointAlongTheLine(start, end, roiDistance), (1.0))));
dronieItems.add(
new WaypointImpl(
this, new Coord3D(end, (startAltitude + GeoTools.getDistance(start, end) / 2.0))));
dronieItems.add(
new WaypointImpl(
this,
new Coord3D(
slowDownPoint,
(startAltitude + GeoTools.getDistance(start, slowDownPoint) / 2.0))));
dronieItems.add(new ChangeSpeedImpl(this, 1.0));
dronieItems.add(new WaypointImpl(this, new Coord3D(start, startAltitude)));
dronieItems.add(new ChangeSpeedImpl(this, defaultSpeed));
dronieItems.add(new LandImpl(this, start));
return dronieItems;
}
@Override
public void processNewLocation(Location location) {
Coord2D gcsCoord = new Coord2D(location.getCoord().getLat(), location.getCoord().getLng());
Coord2D goCoord = GeoTools.newCoordFromBearingAndDistance(gcsCoord, circleAngle, radius);
circleAngle = MathUtil.constrainAngle(circleAngle + circleStep);
drone.getGuidedPoint().newGuidedCoord(goCoord);
}
public double getDistanceFromLastWaypoint(SpatialCoordItem waypoint)
throws IllegalArgumentException {
int i = items.indexOf(waypoint);
if (i > 0) {
MissionItem previous = items.get(i - 1);
if (previous instanceof SpatialCoordItem) {
return GeoTools.getDistance(
waypoint.getCoordinate(), ((SpatialCoordItem) previous).getCoordinate());
}
}
throw new IllegalArgumentException("Last waypoint doesn't have a coordinate");
}
/**
* Create and upload a dronie mission to the drone
*
* @return the bearing in degrees the drone trajectory will take.
*/
public double makeAndUploadDronie() {
Coord2D currentPosition = myDrone.getGps().getPosition();
if (currentPosition == null || myDrone.getGps().getSatCount() <= 5) {
myDrone.notifyDroneEvent(DroneEventsType.WARNING_NO_GPS);
return -1;
}
final double bearing = 180 + myDrone.getOrientation().getYaw();
items.clear();
items.addAll(
createDronie(
currentPosition,
GeoTools.newCoordFromBearingAndDistance(currentPosition, bearing, 50.0)));
sendMissionToAPM();
notifyMissionUpdate();
return bearing;
}