Camera footprint

Android/res/values/preferences_keys.xml

@@ -35,6 +35,7 @@ This file is used to store the preferences keys so that it's accessible and modi
     <string name="pref_permanent_notification_key">pref_permanent_notification</string>
     <string name="pref_ui_gps_hdop_key">pref_ui_gps_hdop</string>
     <string name="pref_ui_language_english_key">pref_ui_language_english</string>
+    <string name="pref_ui_realtime_footprints_key">pref_ui_realtime_footprints_key</string>
     <string name="pref_dshare_username_key">dshare_username</string>
     <string name="pref_dshare_password_key">dshare_password</string>
     <string name="pref_dshare_enabled_key">dshare_enabled</string>

Android/res/values/strings.xml

@@ -459,6 +459,8 @@
     <string name="pref_title_osm">Open Street Map Preferences</string>
     <string name="pref_title_mapbox">MapBox Preferences</string>
     <string name="activity_title_map_provider_preferences">Map Provider Preferences</string>
+    <string name="pref_ui_realtime_footprints_title">Realtime camera footprint</string>
+    <string name="pref_ui_realtime_footprints_summary">Display the camera projection on the ground in realtime</string>
 
     <!-- Others (when possible move to a grouping above) -->
     <string name="tune_roll">Manual adjustment of roll and pitch tuning. Rate_P is the primary value to adjust.</string>

Android/res/xml/preferences.xml

@@ -48,6 +48,12 @@
                     android:key="@string/pref_map_provider_settings_key"
                     android:summary="@string/pref_map_provider_settings_summary"
                     android:title="@string/pref_map_provider_settings_title" />
+
+                <CheckBoxPreference
+                    android:defaultValue="false"
+                    android:key="@string/pref_ui_realtime_footprints_key"
+                    android:summary="@string/pref_ui_realtime_footprints_summary"
+                    android:title="@string/pref_ui_realtime_footprints_title" />
             </PreferenceCategory>
             <PreferenceCategory android:title="@string/pref_title_notifications" >
                 <CheckBoxPreference

Android/src/org/droidplanner/android/fragments/DroneMap.java

@@ -17,7 +17,13 @@
 import org.droidplanner.core.drone.DroneInterfaces.DroneEventsType;
 import org.droidplanner.core.drone.DroneInterfaces.OnDroneListener;
 import org.droidplanner.core.helpers.coordinates.Coord2D;
+import org.droidplanner.core.helpers.units.Altitude;
 import org.droidplanner.core.model.Drone;
+import org.droidplanner.core.survey.CameraInfo;
+import org.droidplanner.core.survey.Footprint;
+
+import com.MAVLink.Messages.ardupilotmega.msg_camera_feedback;
+import com.google.android.gms.internal.ln;
 
 import android.app.Activity;
 import android.content.Context;
@@ -95,6 +101,8 @@ public void run() {
 
 	protected Context context;
 
+	private CameraInfo camera = new CameraInfo();
+
 	protected abstract boolean isMissionDraggable();
 
 	@Override
@@ -182,6 +190,15 @@ public void onDroneEvent(DroneEventsType event, Drone drone) {
 			}
 			break;
 
+		case ATTITUDE:
+			if (((DroidPlannerApp) getActivity().getApplication()).getPreferences()
+					.isRealtimeFootprintsEnabled()) {
+				if (drone.getGps().isPositionValid()) {
+					mMapFragment.updateRealTimeFootprint(drone.getCamera().getCurrentFieldOfView());
+				}
+
+			}
+			break;
 		case GUIDEDPOINT:
 			mMapFragment.updateMarker(guided);
 			mMapFragment.updateDroneLeashPath(guided);
@@ -197,7 +214,7 @@ public void onDroneEvent(DroneEventsType event, Drone drone) {
 			mMapFragment.updateMarker(graphicDrone);
 			break;
 		case FOOTPRINT:
-			mMapFragment.addCameraFootprint(drone.getCameraFootprints().getLastFootprint());
+				mMapFragment.addCameraFootprint(drone.getCamera().getLastFootprint());
 			break;
 		default:
 			break;

Android/src/org/droidplanner/android/maps/DPMap.java

@@ -383,5 +383,7 @@ interface OnMarkerDragListener {
      * @param skip if it should skip further events
      */
     public void skipMarkerClickEvents(boolean skip);
+
+	public void updateRealTimeFootprint(Footprint footprint);
 
 }

Android/src/org/droidplanner/android/maps/providers/google_map/GoogleMapFragment.java

@@ -116,6 +116,8 @@ public class GoogleMapFragment extends SupportMapFragment implements DPMap, Loca
 
 	private List<Polygon> polygonsPaths = new ArrayList<Polygon>();
 
+	private Polygon footprintPoly;
+
     @Override
     public View onCreateView(LayoutInflater inflater, ViewGroup viewGroup,
                              Bundle bundle) {
@@ -525,6 +527,26 @@ public void updateMissionPath(PathSource pathSource) {
 
 
     @Override
+	public void updateRealTimeFootprint(Footprint footprint) {
+    	if (footprintPoly == null) {
+    		PolygonOptions pathOptions = new PolygonOptions();
+    		pathOptions.strokeColor(FOOTPRINT_DEFAULT_COLOR).strokeWidth(FOOTPRINT_DEFAULT_WIDTH);
+    		pathOptions.fillColor(FOOTPRINT_FILL_COLOR);
+
+    		for (Coord2D vertex : footprint.getVertexInGlobalFrame()) {
+    			pathOptions.add(DroneHelper.CoordToLatLang(vertex));
+    		}
+    		footprintPoly = getMap().addPolygon(pathOptions);
+    	}else{
+    		List<LatLng> list = new ArrayList<LatLng>();
+    		for (Coord2D vertex : footprint.getVertexInGlobalFrame()) {
+    			list.add(DroneHelper.CoordToLatLang(vertex));
+    		}
+    		footprintPoly.setPoints(list);
+    	}
+	}
+
+	@Override
     public void updatePolygonsPaths(List<List<Coord2D>> paths){
         for (Polygon poly : polygonsPaths) {
 			poly.remove();
@@ -550,7 +572,7 @@ public void addCameraFootprint(Footprint footprintToBeDraw) {
 		pathOptions.strokeColor(FOOTPRINT_DEFAULT_COLOR).strokeWidth(FOOTPRINT_DEFAULT_WIDTH);
 		pathOptions.fillColor(FOOTPRINT_FILL_COLOR);
 
-		for (Coord2D vertex : footprintToBeDraw.getVertex()) {
+		for (Coord2D vertex : footprintToBeDraw.getVertexInGlobalFrame()) {
 			pathOptions.add(DroneHelper.CoordToLatLang(vertex));
 		}
 		getMap().addPolygon(pathOptions);

Android/src/org/droidplanner/android/maps/providers/mapbox/MapBoxFragment.java

@@ -616,4 +616,10 @@ public void addCameraFootprint(Footprint footprintToBeDraw) {
 		// TODO Auto-generated method stub
 
 	}
+
+	@Override
+	public void updateRealTimeFootprint(Footprint footprint) {
+		// TODO Auto-generated method stub
+
+	}
 }

Android/src/org/droidplanner/android/proxy/mission/item/adapters/CamerasAdapter.java

@@ -2,7 +2,7 @@
 
 import org.droidplanner.R;
 import org.droidplanner.android.utils.file.help.CameraInfoLoader;
-import org.droidplanner.core.mission.survey.CameraInfo;
+import org.droidplanner.core.survey.CameraInfo;
 
 import android.content.Context;
 import android.view.View;

Android/src/org/droidplanner/android/proxy/mission/item/fragments/MissionStructureScannerFragment.java

@@ -10,8 +10,8 @@
 import org.droidplanner.android.widgets.spinners.SpinnerSelfSelect;
 import org.droidplanner.core.helpers.units.Altitude;
 import org.droidplanner.core.mission.MissionItemType;
-import org.droidplanner.core.mission.survey.CameraInfo;
 import org.droidplanner.core.mission.waypoints.StructureScanner;
+import org.droidplanner.core.survey.CameraInfo;
 
 import android.content.Context;
 import android.os.Bundle;

Android/src/org/droidplanner/android/proxy/mission/item/fragments/MissionSurveyFragment.java

@@ -12,9 +12,9 @@
 import org.droidplanner.core.drone.DroneInterfaces;
 import org.droidplanner.core.helpers.units.Altitude;
 import org.droidplanner.core.mission.MissionItemType;
-import org.droidplanner.core.mission.survey.CameraInfo;
 import org.droidplanner.core.mission.survey.Survey;
 import org.droidplanner.core.model.Drone;
+import org.droidplanner.core.survey.CameraInfo;
 
 import android.content.Context;
 import android.graphics.Color;

Android/src/org/droidplanner/android/utils/file/IO/CameraInfoReader.java

@@ -3,7 +3,7 @@
 import java.io.IOException;
 import java.io.InputStream;
 
-import org.droidplanner.core.mission.survey.CameraInfo;
+import org.droidplanner.core.survey.CameraInfo;
 import org.xmlpull.v1.XmlPullParser;
 import org.xmlpull.v1.XmlPullParserException;
 

Android/src/org/droidplanner/android/utils/file/help/CameraInfoLoader.java

@@ -12,7 +12,7 @@
 import org.droidplanner.android.utils.file.DirectoryPath;
 import org.droidplanner.android.utils.file.FileList;
 import org.droidplanner.android.utils.file.IO.CameraInfoReader;
-import org.droidplanner.core.mission.survey.CameraInfo;
+import org.droidplanner.core.survey.CameraInfo;
 
 import android.content.Context;
 

Android/src/org/droidplanner/android/utils/prefs/DroidPlannerPrefs.java

@@ -262,6 +262,11 @@ public boolean isEnglishDefaultLanguage() {
 		return prefs.getBoolean(context.getString(R.string.pref_ui_language_english_key),
 				DEFAULT_PREF_UI_LANGUAGE);
 	}
+
+	public boolean isRealtimeFootprintsEnabled() {
+		return prefs.getBoolean(context.getString(R.string.pref_ui_realtime_footprints_key),
+				false);
+	}
 
 	public String getMapProviderName() {
 		return prefs.getString(context.getString(R.string.pref_maps_providers_key), null);

Core/src/org/droidplanner/core/MAVLink/MavLinkMsgHandler.java

@@ -125,7 +125,7 @@ public void receiveData(MAVLinkMessage msg) {
 			}
 			break;
 		case msg_camera_feedback.MAVLINK_MSG_ID_CAMERA_FEEDBACK:
-			drone.getCameraFootprints().newImageLocation((msg_camera_feedback) msg);
+			drone.getCamera().newImageLocation((msg_camera_feedback) msg);
 			break;
 		default:
 			break;

Core/src/org/droidplanner/core/drone/DroneImpl.java

@@ -7,7 +7,7 @@
 import org.droidplanner.core.drone.variables.Altitude;
 import org.droidplanner.core.drone.variables.Battery;
 import org.droidplanner.core.drone.variables.Calibration;
-import org.droidplanner.core.drone.variables.CameraFootprints;
+import org.droidplanner.core.drone.variables.Camera;
 import org.droidplanner.core.drone.variables.GPS;
 import org.droidplanner.core.drone.variables.GuidedPoint;
 import org.droidplanner.core.drone.variables.HeartBeat;
@@ -50,7 +50,7 @@ public class DroneImpl implements Drone {
 	private final Calibration calibrationSetup;
 	private final WaypointManager waypointManager;
 	private final Magnetometer mag;
-	private final CameraFootprints footprints;
+	private final Camera footprints;
 	private final State state;
 	private final HeartBeat heartbeat;
 	private final Parameters parameters;
@@ -85,7 +85,7 @@ public DroneImpl(MAVLinkStreams.MAVLinkOutputStream mavClient, DroneInterfaces.C
         this.calibrationSetup = new Calibration(this);
         this.waypointManager = new WaypointManager(this);
         this.mag = new Magnetometer(this);
-        this.footprints = new CameraFootprints(this);
+        this.footprints = new Camera(this);
 
         loadVehicleProfile();
 	}
@@ -271,7 +271,7 @@ public Magnetometer getMagnetometer() {
 		return mag;
 	}
 
-	public CameraFootprints getCameraFootprints() {
+	public Camera getCamera() {
 		return footprints;
 	}
 }

Core/src/org/droidplanner/core/drone/variables/CameraFootprints.java → Core/src/org/droidplanner/core/drone/variables/Camera.java

@@ -3,20 +3,19 @@
 import java.util.ArrayList;
 import java.util.List;
 
-import org.droidplanner.core.drone.DroneVariable;
 import org.droidplanner.core.drone.DroneInterfaces.DroneEventsType;
-import org.droidplanner.core.helpers.coordinates.Coord2D;
-import org.droidplanner.core.mission.survey.CameraInfo;
+import org.droidplanner.core.drone.DroneVariable;
 import org.droidplanner.core.model.Drone;
+import org.droidplanner.core.survey.CameraInfo;
 import org.droidplanner.core.survey.Footprint;
 
 import com.MAVLink.Messages.ardupilotmega.msg_camera_feedback;
 
-public class CameraFootprints extends DroneVariable {
+public class Camera extends DroneVariable {
 	private CameraInfo camera = new CameraInfo();
 	private List<Footprint> footprints = new ArrayList<Footprint>();
 
-	public CameraFootprints(Drone myDrone) {
+	public Camera(Drone myDrone) {
 		super(myDrone);
 	}
 
@@ -28,5 +27,16 @@ public void newImageLocation(msg_camera_feedback msg) {
 	public Footprint getLastFootprint() {
 		return footprints.get(footprints.size()-1);
 	}
+
+	public CameraInfo getCamera(){
+		return camera;
+	}
+
+	public Footprint getCurrentFieldOfView() {
+		return new Footprint(camera, myDrone .getGps()
+				.getPosition(), myDrone.getAltitude().getAltitude(), myDrone.getOrientation()
+				.getPitch(), myDrone.getOrientation().getRoll(), myDrone.getOrientation()
+				.getYaw());
+	}
 
 }

Core/src/org/droidplanner/core/helpers/coordinates/Coord2D.java

@@ -1,5 +1,7 @@
 package org.droidplanner.core.helpers.coordinates;
 
+import org.droidplanner.core.helpers.geoTools.GeoTools;
+
 public class Coord2D {
 	private double latitude; // aka x
 	private double longitude; // aka y
@@ -71,4 +73,17 @@ public static Coord2D sum(Coord2D... toBeAdded) {
 		}
 		return new Coord2D(latitude, longitude);
 	}
+
+	@Override
+	public boolean equals(Object obj) {
+		if (obj instanceof Coord2D) {
+			return equals((Coord2D) obj);
+		}else{
+			return super.equals(obj);
+		}
+	}
+
+	public boolean equals(Coord2D obj) {
+		return GeoTools.getDistance(this, obj).valueInMeters()<1e-6;
+	}
 }

Core/src/org/droidplanner/core/helpers/geoTools/GeoTools.java

@@ -34,7 +34,7 @@ public static Double getAproximatedDistance(Coord2D p1, Coord2D p2) {
 		return (Math.hypot((p1.getX() - p2.getX()), (p1.getY() - p2.getY())));
 	}
 
-	public static Double metersTolat(double meters) {
+	private static Double metersTolat(double meters) {
 		return Math.toDegrees(meters / RADIUS_OF_EARTH);
 	}
 
@@ -72,7 +72,25 @@ public static Coord2D newCoordFromBearingAndDistance(Coord2D origin, double bear
 
 		return (new Coord2D(Math.toDegrees(lat2), Math.toDegrees(lon2)));
 	}
-
+
+	/**
+	 * Offset a coordinate by a local distance
+	 * @param original location in WGS84
+	 * @param xMeters Offset distance in the east direction
+	 * @param yMeters Offset distance in the north direction
+	 * @return new coordinate with the offset
+	 */
+	public static Coord2D moveCoordinate(Coord2D origin, double xMeters, double yMeters) {
+		double lon = origin.getLng();
+		double lat = origin.getLat();
+		double lon1 = Math.toRadians(lon);
+		double lat1 = Math.toRadians(lat);
+
+		double lon2 = lon1+ Math.toRadians(metersTolat(xMeters));
+		double lat2 = lat1+ Math.toRadians(metersTolat(yMeters));		
+		return (new Coord2D(Math.toDegrees(lat2), Math.toDegrees(lon2)));
+	}	
+
 	/**
 	 * Calculates the arc between two points
 	 * http://en.wikipedia.org/wiki/Haversine_formula

Core/src/org/droidplanner/core/helpers/math/MathUtil.java

@@ -37,4 +37,32 @@ public static double bisectAngle(double a, double b, double alpha) {
 	public static Length hypot(Length altDelta, Length distDelta) {
 		return new Length(Math.hypot(altDelta.valueInMeters(), distDelta.valueInMeters()));
 	}
+
+	/** 
+	 * Create a rotation matrix given some euler angles
+	 * this is based on http://gentlenav.googlecode.com/files/EulerAngles.pdf 
+	 */
+	public static double [][] dcmFromEuler(double roll, double pitch, double yaw)
+	{
+		double dcm[][] = new double[3][3];
+
+	    double cp = Math.cos(pitch);
+	    double sp = Math.sin(pitch);
+	    double sr = Math.sin(roll);
+	    double cr = Math.cos(roll);
+	    double sy = Math.sin(yaw);
+	    double cy = Math.cos(yaw);
+
+	    dcm[0][0] = cp * cy;
+	    dcm[1][0] = (sr * sp * cy) - (cr * sy);
+	    dcm[2][0] = (cr * sp * cy) + (sr * sy);
+	    dcm[0][1] = cp * sy;
+	    dcm[1][1] = (sr * sp * sy) + (cr * cy);
+	    dcm[2][1] = (cr * sp * sy) - (sr * cy);
+	    dcm[0][2] = -sp;
+	    dcm[1][2] = sr * cp;
+	    dcm[2][2] = cr * cp;
+
+	    return dcm;
+	}
 }

Core/src/org/droidplanner/core/mission/survey/Survey.java

@@ -13,6 +13,7 @@
 import org.droidplanner.core.mission.survey.grid.Grid;
 import org.droidplanner.core.mission.survey.grid.GridBuilder;
 import org.droidplanner.core.polygon.Polygon;
+import org.droidplanner.core.survey.CameraInfo;
 
 import com.MAVLink.Messages.ardupilotmega.msg_mission_item;
 import com.MAVLink.Messages.enums.MAV_CMD;

Core/src/org/droidplanner/core/mission/survey/SurveyData.java

@@ -5,6 +5,7 @@
 import org.droidplanner.core.helpers.units.Altitude;
 import org.droidplanner.core.helpers.units.Area;
 import org.droidplanner.core.helpers.units.Length;
+import org.droidplanner.core.survey.CameraInfo;
 
 public class SurveyData {
 	private Altitude altitude = new Altitude(50.0);