diff options
| author | Gaspard Coulet <gaspard.coulet@mines-ales.org> | 2021-04-28 23:08:21 +0200 |
|---|---|---|
| committer | Gaspard Coulet <gaspard.coulet@mines-ales.org> | 2021-04-28 23:08:21 +0200 |
| commit | 20f3a92f1c5ddecb565634237e33b321e846ad5d (patch) | |
| tree | 1de4cea19d6bf2768a212c1e3c030bdb05a39544 /Realisation_vol/App/app/src/main/java/com | |
Initial commit
Diffstat (limited to 'Realisation_vol/App/app/src/main/java/com')
11 files changed, 1796 insertions, 0 deletions
diff --git a/Realisation_vol/App/app/src/main/java/com/proj/drone_routing/AlgoPlannificator.java b/Realisation_vol/App/app/src/main/java/com/proj/drone_routing/AlgoPlannificator.java new file mode 100644 index 0000000..b1f4298 --- /dev/null +++ b/Realisation_vol/App/app/src/main/java/com/proj/drone_routing/AlgoPlannificator.java @@ -0,0 +1,78 @@ +package com.proj.drone_routing; + +import android.util.Log; + +import java.util.ArrayList; + +class AlgoPlannificator { + //Nb de waypoints entre 2 et 99 + //distance waypoint entre 2 et 1000 m + static ArrayList<DroneWaypoint> algo(float coordInitX, + float coordInitY, + float longueurBat, + float largeurBat, + float altBat, + float kHelice, + int nbWaypoint, + float angleBat) { + if ( nbWaypoint > 99 || nbWaypoint < 2){ + return null; + } + + ArrayList<DroneWaypoint> waypoints = new ArrayList<>(); + + float z = 3; + float theta = 0; + float gimbal = 0; + float buffer = 0; + int nbwp2 = nbWaypoint/2; + int nbwp1 = nbWaypoint-nbwp2; + angleBat=(float)(Math.toRadians(angleBat)+Math.PI/2); + float delta = (float)Math.toRadians((kHelice*360)/(nbwp1-1)); + double zForEachWaypoint = altBat / (nbwp1-1); + + for(int i = 0 ; i < nbwp1;i++) { + + float x = xToLat(coordInitX ,buffer+(float)(longueurBat * Math.cos(theta)* Math.cos(angleBat) - largeurBat* Math.sin(theta)* Math.sin(angleBat))); + float y = yToLon(coordInitY, buffer+(float)(longueurBat* Math.cos(theta)* Math.sin(angleBat) + largeurBat* Math.sin(theta)* Math.cos(angleBat))); + + waypoints.add(new DroneWaypoint(x, y, z, 0, gimbal, true)); + Log.e("Gaspard","Waypoint cree up : "+Double.toString(x)+":"+Double.toString(y)+":"+Double.toString(z)); + z += zForEachWaypoint; + theta+=delta; + + } + + z = altBat; + theta=(float)Math.PI/2; + delta = (float)Math.toRadians((float)(kHelice*360)/(nbwp2-1)); + zForEachWaypoint = altBat / (nbwp2-1); + for(int i = 0 ; i < nbwp2;i++) { + + if(z<0){ + z=0; + } + float x = xToLat(coordInitX ,buffer+(float)(longueurBat * Math.cos(theta)* Math.cos(angleBat) - largeurBat* Math.sin(theta)* Math.sin(angleBat))); + float y = yToLon(coordInitY, buffer+(float)(longueurBat* Math.cos(theta)* Math.sin(angleBat) + largeurBat* Math.sin(theta)* Math.cos(angleBat))); + waypoints.add(new DroneWaypoint(x, y, z, 0, gimbal, true)); + Log.e("Gaspard","Waypoint cree down : "+Double.toString(x)+":"+Double.toString(y)+":"+Double.toString(z)); + z -= zForEachWaypoint; + theta+=delta; + + } + + return waypoints; + + } + + private static double radTerre = 6378.137; + private static double m = (1 / ((2 * Math.PI / 360) * radTerre)) / 1000; + private static float xToLat(float latInit, float x) { + return latInit + (x * (float)m); + } + + private static float yToLon(float lonInit, float y) { + return lonInit + (float)((y * m) / Math.cos(lonInit * (Math.PI / 180))); + } + +}
\ No newline at end of file diff --git a/Realisation_vol/App/app/src/main/java/com/proj/drone_routing/DroneMission.java b/Realisation_vol/App/app/src/main/java/com/proj/drone_routing/DroneMission.java new file mode 100644 index 0000000..1bf34df --- /dev/null +++ b/Realisation_vol/App/app/src/main/java/com/proj/drone_routing/DroneMission.java @@ -0,0 +1,281 @@ +package com.proj.drone_routing; + +import android.support.annotation.NonNull; +import android.support.annotation.Nullable; +import android.util.Log; + +import java.util.ArrayList; + +import dji.common.error.DJIError; +import dji.common.error.DJIMissionError; +import dji.common.mission.waypoint.Waypoint; +import dji.common.mission.waypoint.WaypointMission; +import dji.common.mission.waypoint.WaypointMissionDownloadEvent; +import dji.common.mission.waypoint.WaypointMissionExecutionEvent; +import dji.common.mission.waypoint.WaypointMissionFinishedAction; +import dji.common.mission.waypoint.WaypointMissionFlightPathMode; +import dji.common.mission.waypoint.WaypointMissionGotoWaypointMode; +import dji.common.mission.waypoint.WaypointMissionHeadingMode; +import dji.common.mission.waypoint.WaypointMissionState; +import dji.common.mission.waypoint.WaypointMissionUploadEvent; +import dji.common.model.LocationCoordinate2D; +import dji.common.util.CommonCallbacks; +import dji.sdk.base.BaseProduct; +import dji.sdk.flightcontroller.FlightController; +import dji.sdk.mission.waypoint.WaypointMissionOperator; +import dji.sdk.mission.waypoint.WaypointMissionOperatorListener; +import dji.sdk.products.Aircraft; + + +class DroneMission { + + + private WaypointMission m_wpMission; + private WaypointMissionOperator m_wpMissionOp; + private FlightController mFlightController; + public ArrayList<Waypoint> waypoints; + private UIUpdater uiUpdater = null; + private int oldWp = 0; + + ////////////////////////////////////////////////////// + boolean generate(float lat, float lon, + float largeurBat, float longueurBat, + float alt, float angleBat, int nbWaypoints) { + initFlightController(); + m_wpMissionOp = new WaypointMissionOperator(); + waypoints = new ArrayList<>(); + oldWp = 0; + // Gets the waypoints. + float kHelice = 1f; + float mSpeed = 6.0f; + WaypointsCreator wpCreator = new WaypointsCreator(); + wpCreator.createWaypoints(waypoints, lat, lon, largeurBat, longueurBat, alt, angleBat, + kHelice, nbWaypoints); + + Log.e("Gaspard","Number of Waypoint : " + Integer.toString(waypoints.size())); + // Builds the mission. + WaypointMission.Builder builder = new WaypointMission.Builder() + .finishedAction(WaypointMissionFinishedAction.NO_ACTION) + .headingMode(WaypointMissionHeadingMode.TOWARD_POINT_OF_INTEREST) + .autoFlightSpeed(mSpeed) + .maxFlightSpeed(mSpeed) + .flightPathMode(WaypointMissionFlightPathMode.NORMAL) + .waypointList(waypoints) + .waypointCount(waypoints.size()) + .gotoFirstWaypointMode(WaypointMissionGotoWaypointMode.POINT_TO_POINT) + .setPointOfInterest(new LocationCoordinate2D(lat,lon)); + DJIError error = builder.checkParameters(); + + if (error ==null){ + m_wpMission = builder.build(); + return (m_wpMission != null); + } + else { + String err; + if (error == DJIMissionError.WAYPOINT_COUNT_NOT_VALID || DJIMissionError.WAYPOINT_DISTANCE_TOO_CLOSE==error){ + err = "too much waypoints!"; + } + else { + err = ""; + } + sendTextToUI("Error while trying to generate the path : "+err); + Log.e("Gaspard",error.getDescription()); + return false; + } + } + + /** + * + * @param _uiupdater UIUpdater that should be used to display info + */ + void setUiUpdater(UIUpdater _uiupdater){ + uiUpdater = _uiupdater; + } + + /** + * + * @param text Text to be displayed by the UIUpdater + */ + private void sendTextToUI(String text){ + if ( uiUpdater != null){ + uiUpdater.updateInfoText(text); + } + } + ////////////////////////////////////////////////////// + /** + * The WaypointMissionOperatorListener is listening to the updates on the mission progress, it is used to get information about the running mission. + */ + private WaypointMissionOperatorListener eventNotificationListener = new WaypointMissionOperatorListener() { + @Override + public void onDownloadUpdate(@NonNull WaypointMissionDownloadEvent downloadEvent) { + } + + @Override + public void onUploadUpdate(@NonNull WaypointMissionUploadEvent uploadEvent) { + String progress; + if ( uploadEvent.getProgress()!= null){ + progress = " Waypoint "+Integer.toString(uploadEvent.getProgress().uploadedWaypointIndex + 1)+"/"+Integer.toString(waypoints.size()); + sendTextToUI("Uploading waypoint "+progress+" to the aircraft"); + } + } + + /** + * + * @param executionEvent + * That method is used in order to get a feedback about the execution, but is also used in order to remove already visited Waypoints from the map view. + */ + @Override + public void onExecutionUpdate(@NonNull WaypointMissionExecutionEvent executionEvent) { + if ( oldWp != executionEvent.getProgress().targetWaypointIndex){ + uiUpdater.removeMarker(oldWp); + oldWp ++; + uiUpdater.setMarkerRed(oldWp); + } + String texte; + switch (executionEvent.getProgress().executeState) { + case INITIALIZING: + texte = "Mission Started, flying to 1st waypoint"; + break; + case MOVING: + texte = "Flying to waypoint "+Integer.toString(oldWp+1)+"/"+waypoints.size(); + break; + case BEGIN_ACTION: + texte ="Preparing to shoot from waypoint "+Integer.toString(oldWp+1)+"/"+waypoints.size(); + break; + case DOING_ACTION: + texte = "Shooting the subject, waypoint "+Integer.toString(oldWp+1)+"/"+waypoints.size(); + break; + case FINISHED_ACTION: + texte = "Shooting done, waypoint "+Integer.toString(oldWp+1)+"/"+waypoints.size(); + break; + case RETURN_TO_FIRST_WAYPOINT: + texte = "Mission done, flying to first waypoint"; + break; + default: + texte = "Working"; + } + sendTextToUI(texte); + } + + @Override + public void onExecutionStart() { + sendTextToUI("Execution Started"); + } + + @Override + public void onExecutionFinish(@Nullable final DJIError error) { + sendTextToUI("Execution complete"); + uiUpdater.removeMarker(oldWp); + } + }; + ////////////////////////////////////////////////////// + + /** + * Load m_wpMission as a DJI Mission in the application, then uploads it to the aircraft + * @return Error generated by loadMission, null if none + */ + private DJIError load() { + DJIError error= m_wpMissionOp.loadMission(m_wpMission); + if (error == null) { + Log.e("Gaspard","loadWaypoint succeeded"); + } else { + Log.e("Gaspard","loadWaypoint failed " + error.getDescription()); + return error; + } + m_wpMissionOp.addListener(eventNotificationListener); + m_wpMissionOp.uploadMission(null); + return null; + } + + /** + * Checks if the aircraft is flying, if it is not, take off, then load the mission, uploads it, awaits until it is done, then start it up and set the speed to 5 m/s + */ + void start() { + if (!mFlightController.getState().isFlying()) { + sendTextToUI("Taking off!"); + mFlightController.startTakeoff(new CommonCallbacks.CompletionCallback() { + public void onResult(@NonNull DJIError djiError) { + sendTextToUI("Took off!"); + } + }); + //Attente de 5 secondes le temps du decollage + synchronized (this) { + try { + wait(5000); + } catch (InterruptedException e) { + e.printStackTrace(); + } + } + } + //chargement de la mission et upload vers l'appareil + DJIError error = load(); + while(m_wpMissionOp.getCurrentState()!= WaypointMissionState.READY_TO_EXECUTE){ + synchronized (this) { + try { + wait(1000); + } catch (InterruptedException e) { + e.printStackTrace(); + } + } + } + if (error==null){ + sendTextToUI("Mission is ready, starting..."); + //demarrage de la mission + m_wpMissionOp.startMission(new CommonCallbacks.CompletionCallback() { + @Override + public void onResult(DJIError djiError) { + if (djiError != null){ + sendTextToUI("Error while starting the mission"); + } + else { + sendTextToUI("Starting complete"); + m_wpMissionOp.setAutoFlightSpeed(5.0f, new CommonCallbacks.CompletionCallback() { + @Override + public void onResult(DJIError djiError) { + if ( djiError != null){ + sendTextToUI("Error while adjusting the speed"); + } + } + }); + } + } + }); + } + else { + sendTextToUI("Chargement de la mission impossible"); + } + } + + /** + * Pause the mission + */ + void pause(){ + m_wpMissionOp.pauseMission(null); + sendTextToUI("Mission paused"); + } + + /** + * Resume the mission + */ + void resume(){m_wpMissionOp.resumeMission(null); + sendTextToUI("Mission resumed");} + + /** + * Stops the mission + */ + void stop(){m_wpMissionOp.stopMission(null); + sendTextToUI("Mission stopped");} + + /** + * gets the flightcontroller and stores it + */ + private void initFlightController() { + BaseProduct product = MainActivity.getProductInstance(); + if (product != null && product.isConnected()) { + if (product instanceof Aircraft) { + mFlightController = ((Aircraft) product).getFlightController(); + } + } + } + +}
\ No newline at end of file diff --git a/Realisation_vol/App/app/src/main/java/com/proj/drone_routing/DroneWaypoint.java b/Realisation_vol/App/app/src/main/java/com/proj/drone_routing/DroneWaypoint.java new file mode 100644 index 0000000..f8e0fdf --- /dev/null +++ b/Realisation_vol/App/app/src/main/java/com/proj/drone_routing/DroneWaypoint.java @@ -0,0 +1,55 @@ +package com.proj.drone_routing;
+
+import android.support.annotation.NonNull;
+
+public class DroneWaypoint {
+
+ private float lat;
+ private float lon;
+ private float alt;
+ private int direction;
+ private float gimbal;
+ private boolean photo;
+
+ DroneWaypoint(float lat, float lon, float alt, int direction, float gimbal, boolean photo) {
+ super();
+ this.lat = lat;
+ this.lon = lon;
+ this.alt = alt;
+ this.direction = direction;
+ this.gimbal = gimbal;
+ this.photo = photo;
+ }
+
+ float getLat() {
+ return lat;
+ }
+
+ float getLon() {
+ return lon;
+ }
+
+ float getAlt() {
+ return alt;
+ }
+
+ int getDirection() {
+ return direction;
+ }
+
+ float getGimbal() {
+ return gimbal;
+ }
+
+
+
+ boolean isPhoto() {
+ return photo;
+ }
+ @NonNull
+ public String toString(){
+ return Float.toString(lat)+":"+ Float.toString(lon)+":"+Float.toString(alt)+":"+ Integer.toString(direction)+":"+Float.toString(gimbal);
+ }
+
+
+}
diff --git a/Realisation_vol/App/app/src/main/java/com/proj/drone_routing/InputActivity.java b/Realisation_vol/App/app/src/main/java/com/proj/drone_routing/InputActivity.java new file mode 100644 index 0000000..86dce0e --- /dev/null +++ b/Realisation_vol/App/app/src/main/java/com/proj/drone_routing/InputActivity.java @@ -0,0 +1,148 @@ +
+package com.proj.drone_routing;
+/*
+ * Revoir graphisme disposition champs texte
+ * separer la partie Algo en lui faisant une classe a elle
+ * supprimer les fonction inutiles
+ * bouton valider, et c'est d'ici qu'on chargera les waypoints donc renvoyer une mission DJI ( tester si on peut creer une mission DJI sans etre connecte a un appareil
+ * changer le current coord en placage de point sur une map
+ * implementer une OSM ici ( on pourra reutiliser la classe MapView qu'on aura deja fait)*/
+
+import android.content.Context;
+import android.content.Intent;
+import android.os.Environment;
+import android.preference.PreferenceManager;
+import android.support.v7.app.AppCompatActivity;
+import android.os.Bundle;
+import android.support.v7.widget.PopupMenu;
+import android.util.Log;
+import android.view.Gravity;
+import android.view.MenuItem;
+import android.view.View;
+import android.widget.Button;
+import android.widget.EditText;
+
+import org.osmdroid.config.Configuration;
+import org.osmdroid.events.MapEventsReceiver;
+import org.osmdroid.util.GeoPoint;
+import org.osmdroid.views.MapView;
+import org.osmdroid.views.overlay.MapEventsOverlay;
+import org.osmdroid.views.overlay.Marker;
+
+/**
+ * InputActivity is used for the users to enter the specification of the subject, that will be used be Working Activity to generate the mission.
+ */
+
+public class InputActivity extends AppCompatActivity {
+ public MMap mmap;
+ public Boolean chooseOnMap = true;
+ public EditText w, l, h, lat, lon,ang,nb;
+ public Button gen;
+ public Intent intentact;
+ public Marker addP = null;
+
+ @Override
+ protected void onCreate(Bundle savedInstanceState) {
+ super.onCreate(savedInstanceState);
+ setContentView(R.layout.activity_input);
+ final Context ctx = getApplicationContext();
+ Configuration.getInstance().load(ctx, PreferenceManager.getDefaultSharedPreferences(ctx));
+ MapView map = findViewById(R.id.input_mapView);
+ mmap = new MMap(map, ctx);
+ w = findViewById(R.id.tb_width);
+ l = findViewById(R.id.tb_length);
+ h = findViewById(R.id.tb_height);
+ lat = findViewById(R.id.tb_lat);
+ lon = findViewById(R.id.tb_lon);
+ ang = findViewById(R.id.tb_angle);
+ nb = findViewById(R.id.tb_nb);
+ gen = findViewById(R.id.button);
+
+ intentact = new Intent();
+ MapEventsReceiver mReceive = new MapEventsReceiver() {
+ @Override
+ public boolean singleTapConfirmedHelper(GeoPoint p) {
+
+ // write your code here
+ Log.d("loc", p.toString());
+
+ return false;
+ }
+
+
+ public boolean longPressHelper(final GeoPoint p) {
+ // write your code here
+ if (chooseOnMap) {
+ Log.d("longPress", "LongPressed " + p.toString());
+ addP = new Marker(mmap.getMv());
+ addP.setPosition(p);
+ addP.setAnchor(Marker.ANCHOR_CENTER, Marker.ANCHOR_BOTTOM);
+ mmap.getMv().getOverlays().add(0, addP);
+ chooseOnMap = false;
+ lat.setText(Double.toString(p.getLatitude()));
+ lon.setText(Double.toString(p.getLongitude()));
+// PopupMenu popup = new PopupMenu(InputActivity.this, mmap.getMv(), Gravity.NO_GRAVITY, R.attr.actionOverflowMenuStyle, 0);
+// //PopupWindow popup=new PopupWindow(layout,LayoutParams.MATCH_PARENT, LayoutParams.MATCH_PARENT,true);
+// //Inflating the Popup using xml file
+// popup.getMenuInflater().inflate(R.menu.popup_menu, popup.getMenu());
+//
+// //registering popup with OnMenuItemClickListener
+// popup.setOnMenuItemClickListener(new PopupMenu.OnMenuItemClickListener() {
+// public boolean onMenuItemClick(MenuItem item) {
+// switch (item.getItemId()) {
+// case R.id.addP:
+// addP = new Marker(mmap.getMv());
+// addP.setPosition(p);
+// addP.setAnchor(Marker.ANCHOR_CENTER, Marker.ANCHOR_BOTTOM);
+// mmap.getMv().getOverlays().add(0, addP);
+// chooseOnMap = false;
+// lat.setText(Double.toString(p.getLatitude()));
+// lon.setText(Double.toString(p.getLongitude()));
+// return true;
+// case R.id.cancel:
+// return true;
+// default:
+// return false;
+// }
+// }
+// });
+
+// popup.show();//showing popup menu
+ }
+ return false;
+ }
+ };
+ MapEventsOverlay OverlayEvents = new MapEventsOverlay(getBaseContext(), mReceive);
+ mmap.getMv().getOverlays().add(OverlayEvents);
+ }
+
+ /**
+ * Stores the parameters of the flight plan and exit the activity
+ * @param view unused param
+ */
+ public void generate(View view) {
+
+ Intent intent = new Intent();
+
+ intent.putExtra("param",lat.getText()+":"+lon.getText()+":"+l.getText()+":"+w.getText()+":"+h.getText()+":"+ang.getText()+":"+nb.getText()+":"+"0");
+ setResult(RESULT_OK,intent);
+ finish();
+ }
+
+ public void getCurr(View view) {
+ lat.setText(Double.toString(mmap.getLoc().getLatitude()));
+ lon.setText(Double.toString(mmap.getLoc().getLongitude()));
+ }
+
+
+ public void remove(View v) {
+ Log.d("remove", "remove Pressed");
+ if (!chooseOnMap && !mmap.getMv().getOverlays().isEmpty()) {
+ chooseOnMap=true;
+ mmap.getMv().getOverlays().remove(0);
+ mmap.getMv().invalidate();
+ lat.setText("");
+ lon.setText("");
+ }
+ }
+}
diff --git a/Realisation_vol/App/app/src/main/java/com/proj/drone_routing/MApplication.java b/Realisation_vol/App/app/src/main/java/com/proj/drone_routing/MApplication.java new file mode 100644 index 0000000..0151ad3 --- /dev/null +++ b/Realisation_vol/App/app/src/main/java/com/proj/drone_routing/MApplication.java @@ -0,0 +1,16 @@ +package com.proj.drone_routing;
+/*Ici, rien a faire*/
+import android.app.Application;
+import android.content.Context;
+
+import com.secneo.sdk.Helper;
+
+public class MApplication extends Application {
+
+ @Override
+ protected void attachBaseContext(Context paramContext) {
+ super.attachBaseContext(paramContext);
+ Helper.install(MApplication.this);
+ }
+
+}
\ No newline at end of file diff --git a/Realisation_vol/App/app/src/main/java/com/proj/drone_routing/MMap.java b/Realisation_vol/App/app/src/main/java/com/proj/drone_routing/MMap.java new file mode 100644 index 0000000..c8dd230 --- /dev/null +++ b/Realisation_vol/App/app/src/main/java/com/proj/drone_routing/MMap.java @@ -0,0 +1,238 @@ +package com.proj.drone_routing; + +import android.Manifest; +import android.content.Context; +import android.content.Intent; +import android.content.pm.PackageManager; +import android.location.Location; +import android.location.LocationListener; +import android.location.LocationManager; +import android.os.Bundle; +import android.provider.Settings; +import android.support.v4.content.ContextCompat; + +import org.osmdroid.api.IMapController; +import org.osmdroid.tileprovider.tilesource.TileSourceFactory; +import org.osmdroid.util.GeoPoint; +import org.osmdroid.views.MapView; +import org.osmdroid.views.overlay.Marker; + +import java.util.ArrayList; +import java.util.Collections; +import java.util.List; + +import dji.common.mission.waypoint.Waypoint; + +import static android.content.Context.LOCATION_SERVICE; + +/** + * That class is managing the mapView it is associated with, it is used to display the aircraft, user and waypoint position + */ +public class MMap { + + + + private MapView mv; + + + private Marker DroneLoc; + private Marker user; + private Marker subject; + private Location loc; + private Context ctx; + private LocationManager mLocationManager; + private GeoPoint currentLocation; + private IMapController mapController; + private ArrayList<Marker> listeWaypoint = new ArrayList<>(); + + /** + * + * @return the MapView associated with the MMap + */ + MapView getMv() { + return mv; + } + + /** + * + * @return the updated location of the application + */ + Location getLoc() { + setCurrLoc(); + return loc; + } + + MMap(MapView mv_,Context ctx_){ + this.mv=mv_; + this.ctx=ctx_; + init(); + setCurrLoc(); + } + + /** + * Initialises the Open Street Map View + */ + private void init(){ + mv.setTileSource(TileSourceFactory.MAPNIK); + mv.setMultiTouchControls(true); + mapController = mv.getController(); + mapController.setZoom(19.0); + } + + /** + * Get the current localisation of the smartphone and updates its position on the map + */ + void setCurrLoc(){ + + int off = 0; + try { + off = Settings.Secure.getInt(ctx.getContentResolver(), Settings.Secure.LOCATION_MODE); + } catch (Settings.SettingNotFoundException e) { + e.printStackTrace(); + } + if(off==0){ + Intent onGPS = new Intent(Settings.ACTION_LOCATION_SOURCE_SETTINGS); + ctx.startActivity(onGPS); + } + + loc = getLastKnownLocation(); + + if( loc != null ) { + if ( user == null) user = new Marker(mv); + currentLocation = new GeoPoint(loc.getLatitude(), loc.getLongitude()); + user.setPosition(currentLocation); + user.setAnchor(Marker.ANCHOR_CENTER, Marker.ANCHOR_BOTTOM); + user.setIcon(ctx.getResources().getDrawable(R.drawable.user)); + mv.getOverlays().add(user); + mapController.setCenter(currentLocation); + } + + } + + /** + * If GPS localisation is not available that method is called, which returns a less precise localisation + * @return + */ + private Location getLastKnownLocation() { + mLocationManager = (LocationManager)ctx.getSystemService(LOCATION_SERVICE); + List<String> providers = mLocationManager.getProviders(true); + Location bestLocation; + bestLocation = null; + Location l=null; + for (String provider : providers) { + if(ContextCompat.checkSelfPermission( ctx, Manifest.permission.ACCESS_FINE_LOCATION ) == PackageManager.PERMISSION_GRANTED) { + l = mLocationManager.getLastKnownLocation(provider); + } + if (l == null) { + continue; + } + if (bestLocation == null || l.getAccuracy() < bestLocation.getAccuracy()) { + bestLocation = l; + } + } + return bestLocation; + } + + + public class MyLocationListener implements LocationListener { + + public void onLocationChanged(Location location) { + currentLocation = new GeoPoint(location); + showCurrentLoc(); + } + + + public void onProviderDisabled(String provider) { + } + + public void onProviderEnabled(String provider) { + } + + public void onStatusChanged(String provider, int status, Bundle extras) { + } + } + + /** + * Updates the position of the user's smartPhone on the map + */ + private void showCurrentLoc(){ + if (user == null){ + user= new Marker(mv); + } + user.setPosition(currentLocation); + user.setAnchor(Marker.ANCHOR_CENTER, Marker.ANCHOR_BOTTOM); + user.setIcon(ctx.getResources().getDrawable(R.drawable.user)); + mv.getOverlays().add(user); + } + + /** + * Displays on the map the list of Waypoints that composes the DroneMission. + * First Waypoint is red. + * @param dm DroneMission + */ + void addWaypoint(DroneMission dm){ + for (int i = dm.waypoints.size()-1; i >=0; i --){ + Waypoint w = dm.waypoints.get(i); + GeoPoint point = new GeoPoint(w.coordinate.getLatitude(),w.coordinate.getLongitude()); + Marker passage = new Marker(mv); + listeWaypoint.add(passage); + passage.setPosition(point); + passage.setAnchor(Marker.ANCHOR_CENTER,Marker.ANCHOR_CENTER); + if (w == dm.waypoints.get(0)){ + passage.setIcon(ctx.getResources().getDrawable(R.drawable.first_wp)); + } + else { + passage.setIcon(ctx.getResources().getDrawable(R.drawable.wp)); + } + mv.getOverlays().add(passage); + } + Collections.reverse(listeWaypoint); + } + + /** + * + * @param index Index of the waypoint that should be made red + */ + + void makeWaypointRed(int index){ + listeWaypoint.get(index).setIcon(ctx.getResources().getDrawable(R.drawable.first_wp)); + } + + /** + * + * @param index Index of the waypoints that should be removed from the map + */ + void removeWaypoint(int index){ + listeWaypoint.get(index).remove(mv); + } + + /** + * Removes all the waypoint from the map. + */ + void clearWaypoint(){ + if (listeWaypoint.size()!=0){ + for ( Marker m : listeWaypoint){ + m.remove(mv); + } + listeWaypoint.clear(); + } + } + + /** + * Set the aircraft location on the map, set the heading direction of the aircraft too. + * @param x latitude + * @param y longitude + * @param rotation rotation ( 0 is north ) + */ + void setDrone(double x, double y, float rotation){ + if (DroneLoc == null){ + DroneLoc = new Marker(mv); + } + GeoPoint point = new GeoPoint(x,y); + DroneLoc.setPosition(point); + DroneLoc.setIcon(ctx.getResources().getDrawable(R.drawable.drone)); + DroneLoc.setAnchor(Marker.ANCHOR_CENTER,Marker.ANCHOR_CENTER); + DroneLoc.setRotation(rotation); + mv.getOverlays().add(DroneLoc); + } +} diff --git a/Realisation_vol/App/app/src/main/java/com/proj/drone_routing/MainActivity.java b/Realisation_vol/App/app/src/main/java/com/proj/drone_routing/MainActivity.java new file mode 100644 index 0000000..a881ae6 --- /dev/null +++ b/Realisation_vol/App/app/src/main/java/com/proj/drone_routing/MainActivity.java @@ -0,0 +1,345 @@ +package com.proj.drone_routing;
+
+import android.Manifest;
+import android.app.Activity;
+import android.content.Intent;
+import android.content.pm.PackageManager;
+import android.os.AsyncTask;
+import android.os.Build;
+import android.os.Bundle;
+import android.os.Handler;
+import android.os.Looper;
+import android.support.annotation.NonNull;
+import android.support.constraint.ConstraintLayout;
+import android.support.v4.app.ActivityCompat;
+import android.support.v4.content.ContextCompat;
+import android.support.v7.app.AppCompatActivity;
+import android.util.Log;
+import android.view.View;
+import android.widget.Button;
+import android.widget.TextView;
+import android.widget.Toast;
+
+import java.util.ArrayList;
+import java.util.List;
+import java.util.concurrent.atomic.AtomicBoolean;
+
+import dji.common.error.DJIError;
+import dji.common.error.DJISDKError;
+import dji.sdk.base.BaseComponent;
+import dji.sdk.base.BaseProduct;
+import dji.sdk.products.Aircraft;
+import dji.sdk.sdkmanager.DJISDKManager;
+
+/**
+ * Starting Activity in which the SDK is loaded and the app is registered, it is possible to enter InputActivity from here with no device connected, but WorkingActivity can only be accessed if an aircraft is connected.
+ */
+public class MainActivity extends AppCompatActivity {
+
+ private static final String TAG = MainActivity.class.getName();
+ public static final String FLAG_CONNECTION_CHANGE = "dji_sdk_connection_change";
+ private static BaseProduct mProduct;
+ private static BaseProduct product;
+ private Handler mHandler;
+ private boolean isConnected =false;
+ private Thread screenUpdater = null;
+ private String param;
+
+ private static final String[] REQUIRED_PERMISSION_LIST = new String[]{
+// Manifest.permission.VIBRATE,
+ Manifest.permission.INTERNET,
+ Manifest.permission.ACCESS_WIFI_STATE,
+ Manifest.permission.WAKE_LOCK,
+ Manifest.permission.ACCESS_COARSE_LOCATION,
+ Manifest.permission.ACCESS_NETWORK_STATE,
+ Manifest.permission.ACCESS_FINE_LOCATION,
+ Manifest.permission.CHANGE_WIFI_STATE,
+ Manifest.permission.WRITE_EXTERNAL_STORAGE,
+// Manifest.permission.BLUETOOTH,
+// Manifest.permission.BLUETOOTH_ADMIN,
+ Manifest.permission.READ_EXTERNAL_STORAGE,
+ Manifest.permission.READ_PHONE_STATE,
+ };
+ private List<String> missingPermission = new ArrayList<>();
+ private AtomicBoolean isRegistrationInProgress = new AtomicBoolean(false);
+ private static final int REQUEST_PERMISSION_CODE = 12345;
+ private BaseComponent.ComponentListener mDJIComponentListener = new BaseComponent.ComponentListener() {
+
+ @Override
+ public void onConnectivityChange(boolean isConnected) {
+ notifyStatusChange();
+ }
+ };
+ public Button BtnStart=null;
+ public Button BtnLoad;
+ public TextView TextePrcp;
+ private ArrayList<String> waypointList = null;
+ public boolean isloaded = false;
+
+ /**
+ * Check et request permissions, initialise the displayed information and buttons
+ * @param savedInstanceState
+ */
+ @Override
+ protected void onCreate(Bundle savedInstanceState) {
+ super.onCreate(savedInstanceState);
+
+ // When the compile and target version is higher than 22, please request the following permission at runtime to ensure the SDK works well.
+ if (Build.VERSION.SDK_INT >= Build.VERSION_CODES.M) {
+ checkAndRequestPermissions();
+ }
+
+ setContentView(R.layout.activity_main);
+
+ mHandler = new Handler(Looper.getMainLooper());
+ BtnStart = (Button) findViewById(R.id.button_Main_start);
+ BtnLoad = (Button) findViewById(R.id.button_Main_Load);
+ TextePrcp = findViewById(R.id.welcome_text);
+ initUpdateThread(this);
+
+ }
+
+ /**
+ * Only displayed when an aircraft is connected, launch the working activity, with parameters if existing.
+ * @param view
+ */
+ public void btnMainStart (View view){
+ stopUpdatethread();
+ Intent WorkingAct = new Intent(MainActivity.this, WorkingActivity.class);
+ WorkingAct.putExtra("isloaded",isloaded);
+ if (isloaded){
+ WorkingAct.putExtra("param",param);
+ }
+ startActivity(WorkingAct);
+ }
+
+ /**
+ * Start the InputActivity and awaits for its result
+ * @param view
+ */
+ public void btnMainLoad (View view){
+ Intent InputAct = new Intent(MainActivity.this, InputActivity.class);
+ startActivityForResult(InputAct,1);
+ }
+
+ /**
+ * Stores the parameters returned by the InputActivity
+ * @param requestCode
+ * @param resultCode
+ * @param dataIntent
+ */
+ @Override
+ public void onActivityResult(int requestCode, int resultCode, Intent dataIntent){
+ if (requestCode == 1 && resultCode==RESULT_OK){
+ param = dataIntent.getStringExtra("param");
+ isloaded = true;
+ }
+ }
+ /**
+ * Checks if there is any missing permissions, and
+ * requests runtime permission if needed.
+ */
+ private void checkAndRequestPermissions() {
+ // Check for permissions
+ for (String eachPermission : REQUIRED_PERMISSION_LIST) {
+ if (ContextCompat.checkSelfPermission(this, eachPermission) != PackageManager.PERMISSION_GRANTED) {
+ missingPermission.add(eachPermission);
+ }
+ }
+ // Request for missing permissions
+ if (missingPermission.isEmpty()) {
+ startSDKRegistration();
+ } else if (Build.VERSION.SDK_INT >= Build.VERSION_CODES.M) {
+ showToast("Need to grant the permissions!");
+ ActivityCompat.requestPermissions(this,
+ missingPermission.toArray(new String[missingPermission.size()]),
+ REQUEST_PERMISSION_CODE);
+ }
+
+ }
+
+ /**
+ * Result of runtime permission request
+ */
+ @Override
+ public void onRequestPermissionsResult(int requestCode,
+ @NonNull String[] permissions,
+ @NonNull int[] grantResults) {
+ super.onRequestPermissionsResult(requestCode, permissions, grantResults);
+ // Check for granted permission and remove from missing list
+ if (requestCode == REQUEST_PERMISSION_CODE) {
+ for (int i = grantResults.length - 1; i >= 0; i--) {
+ if (grantResults[i] == PackageManager.PERMISSION_GRANTED) {
+ missingPermission.remove(permissions[i]);
+ }
+ }
+ }
+ // If there is enough permission, we will start the registration
+ if (missingPermission.isEmpty()) {
+ startSDKRegistration();
+ } else {
+ showToast("Missing permissions!!!");
+ }
+ }
+
+ /**
+ * Start the registration of the SDK, overrides onProductConnect and onProductDisconnect to check if an aircraft is connected
+ */
+ private void startSDKRegistration() {
+ if (isRegistrationInProgress.compareAndSet(false, true)) {
+ AsyncTask.execute(new Runnable() {
+ @Override
+ public void run() {
+ DJISDKManager.getInstance().registerApp(MainActivity.this.getApplicationContext(), new DJISDKManager.SDKManagerCallback() {
+ @Override
+ public void onRegister(DJIError djiError) {
+ if (djiError == DJISDKError.REGISTRATION_SUCCESS) {
+ showToast("Register Success");
+ DJISDKManager.getInstance().startConnectionToProduct();
+ } else {
+ showToast("Register sdk fails");
+ }
+ Log.v(TAG, djiError.getDescription());
+ }
+
+ @Override
+ public void onProductDisconnect() {
+ notifyStatusChange();
+ isConnected =false;
+ }
+ @Override
+ public void onProductConnect(BaseProduct baseProduct) {
+ notifyStatusChange();
+ if (baseProduct instanceof Aircraft)
+ isConnected=true;
+ }
+ @Override
+ public void onComponentChange(BaseProduct.ComponentKey componentKey, BaseComponent oldComponent,
+ BaseComponent newComponent) {
+
+ if (newComponent != null) {
+ newComponent.setComponentListener(new BaseComponent.ComponentListener() {
+
+ @Override
+ public void onConnectivityChange(boolean isConnected) {
+ notifyStatusChange();
+ }
+ });
+ }
+ }
+ });
+ }
+ });
+ }
+ }
+
+ private void notifyStatusChange() {
+ mHandler.removeCallbacks(updateRunnable);
+ mHandler.postDelayed(updateRunnable, 500);
+ }
+
+ /**
+ * Creates and enable the start button
+ */
+ public void enableStartButton(){
+ if(BtnStart==null) {
+ BtnStart = new Button(this);
+ BtnStart.setId(R.id.button_Main_start);
+ BtnStart.setBackgroundResource(R.drawable.rounded_button_blue);
+ BtnStart.setText(R.string.button_start);
+ BtnStart.setOnClickListener(new Button.OnClickListener() {
+ public void onClick(View view) {
+ btnMainStart(view);
+ }
+ });
+ ConstraintLayout ll = (ConstraintLayout) findViewById(R.id.layout_main);
+ ConstraintLayout.LayoutParams lp = new ConstraintLayout.LayoutParams(ConstraintLayout.LayoutParams.WRAP_CONTENT,
+ ConstraintLayout.LayoutParams.WRAP_CONTENT);
+ lp.topToTop = (R.id.layout_main);
+ lp.leftToLeft = (R.id.layout_main);
+ lp.rightToRight = (R.id.layout_main);
+ lp.bottomToBottom = (R.id.layout_main);
+ lp.horizontalBias = (float) 0.6;
+ lp.verticalBias = (float) 0.7;
+ ll.addView(BtnStart, lp);
+ }
+ }
+
+ /**
+ * Deletes the start button
+ */
+ public void disableStartButton(){
+ if (BtnStart !=null) {
+ ConstraintLayout ll = (ConstraintLayout) findViewById(R.id.layout_main);
+ if (null != ll) {
+ ll.removeView(BtnStart);
+ BtnStart = null;
+ }
+ }
+ }
+ public void stopUpdatethread(){
+ if (screenUpdater!=null){
+ screenUpdater.interrupt();
+ }
+
+ }
+
+ /**
+ * Initialises an UI thread that updates the start Button when it should be displayed or not ( if an aircraft is connected )
+ * @param context
+ */
+ public void initUpdateThread(final Activity context){
+
+ screenUpdater = new Thread(){
+ @Override
+ public void run(){
+ try {
+ while (!isInterrupted()) {
+ Thread.sleep(1000);
+ context.runOnUiThread(new Runnable() {
+ @Override
+ public void run() {
+ if (isConnected){
+ TextePrcp.setText(R.string.text_connected);
+ enableStartButton();
+ }
+ else {
+ TextePrcp.setText(R.string.start_text);
+ disableStartButton();
+ }
+
+ }
+ });
+ }
+ } catch (InterruptedException e) {
+ }
+ }
+ };
+ screenUpdater.start();
+ }
+
+ private Runnable updateRunnable = new Runnable() {
+
+ @Override
+ public void run() {
+ Intent intent = new Intent(FLAG_CONNECTION_CHANGE);
+ sendBroadcast(intent);
+ }
+ };
+
+ private void showToast(final String toastMsg) {
+
+ Handler handler = new Handler(Looper.getMainLooper());
+ handler.post(new Runnable() {
+ @Override
+ public void run() {
+ Toast.makeText(getApplicationContext(), toastMsg, Toast.LENGTH_LONG).show();
+ }
+ });
+
+ }
+ public static synchronized BaseProduct getProductInstance() {
+ product = DJISDKManager.getInstance().getProduct();
+ return product;
+ }
+}
diff --git a/Realisation_vol/App/app/src/main/java/com/proj/drone_routing/UIUpdater.java b/Realisation_vol/App/app/src/main/java/com/proj/drone_routing/UIUpdater.java new file mode 100644 index 0000000..43ff425 --- /dev/null +++ b/Realisation_vol/App/app/src/main/java/com/proj/drone_routing/UIUpdater.java @@ -0,0 +1,233 @@ +package com.proj.drone_routing;
+
+import android.app.Activity;
+import android.graphics.Color;
+import android.support.annotation.NonNull;
+import android.util.Log;
+import android.widget.TextView;
+
+import java.util.Locale;
+
+import dji.common.battery.BatteryState;
+import dji.common.flightcontroller.FlightControllerState;
+import dji.sdk.base.BaseProduct;
+import dji.sdk.flightcontroller.FlightController;
+import dji.sdk.products.Aircraft;
+import dji.sdk.sdkmanager.DJISDKManager;
+
+import static java.lang.Math.pow;
+import static java.lang.Math.sqrt;
+
+/**
+ * That class is managing the text information displayed in WorkingActivity, it displays text and manages information about the aircraft, such as speed and localisation
+ */
+class UIUpdater {
+ private FlightController mFlightController = null;
+ private String batteryLevel;
+ private TextView batteryText;
+ private Activity context;
+ private double lat = 0;
+ private double lon = 0;
+ private float speedx;
+ private float speedy;
+ private float speedz;
+ private TextView speedText;
+ private String CurrentDisplayedSpeed;
+ private TextView infoText;
+ private String CurrentDisplayedInfo;
+ private MMap map;
+ private float rota=0;
+ //update battery level text
+
+ /**
+ *
+ * @param _context Context of the previous acitvity
+ * @param batterie TextView in which the battery level should be displayed
+ * @param mapp Initialised MMap for further interaction
+ * @param speed TextView in which the speed info should be written
+ * @param info_texte TextView in which the common info will be written
+ */
+ UIUpdater(final Activity _context, TextView batterie, MMap mapp, TextView speed,TextView info_texte) {
+ batteryText = batterie;
+ context = _context;
+ speedText=speed;
+ initBattery();
+ initFlightController();
+ map = mapp;
+ infoText = info_texte;
+ }
+
+ /**
+ * Update the battery text field, using the batteryLevel field which is updated by the SDK
+ */
+ private void updateBatteryText() {
+ int color;
+ String toThis = batteryLevel;
+ if (Integer.parseInt(toThis) > 50) {
+ color = Color.GREEN;
+ } else if (Integer.parseInt(toThis) > 25) {
+ color = Color.YELLOW;
+ } else {
+ color = Color.RED;
+ }
+ batteryText.setText(toThis.concat("%"));
+ batteryText.setTextColor(color);
+ }
+
+ /**
+ * Initialisation of the battery level SDK callback
+ */
+ private void initBattery() {
+ //Battery status
+ DJISDKManager.getInstance().getProduct().getBattery().setStateCallback(new BatteryState.Callback() {
+ @Override
+ public void onUpdate(BatteryState djiBatteryState) {
+ batteryLevel = Integer.toString(djiBatteryState.getChargeRemainingInPercent());
+ }
+ });
+ }
+
+ /**
+ * Refresh the position of the aircraft on the Mmap
+ */
+ private void refreshMap() {
+ if (map != null) {
+ map.setDrone(lat, lon, rota);
+ Log.e("Gaspard", "drone rota = "+Double.toString(rota));
+ //Log.e("Gaspard", "Mise a jour de la pos du drone sur carte coord : "+Double.toString(lat)+":"+Double.toString(lon));
+ }
+ }
+
+ /**
+ * Refresh the speed text field with latest speed info
+ */
+ private void updateSpeedText(){
+ String texte = " Speed : "+String.format("%.1f",sqrt(pow(speedx,2)+pow(speedy,2)))+" m/s Ascending : "+ Float.toString(speedz)+" m/s";
+ if (CurrentDisplayedSpeed == null){
+ CurrentDisplayedSpeed = texte;
+ speedText.setText(texte);
+ speedText.setTextColor(Color.WHITE);
+ }
+ else if ( texte.compareTo(CurrentDisplayedSpeed)!=0) {
+ speedText.setText(texte);
+ speedText.setTextColor(Color.WHITE);
+ CurrentDisplayedSpeed = texte;
+ }
+ }
+
+ /**
+ *
+ * @param _newText New text that should be displayed
+ * Check if new Text is the same as the previous one, if it is not, update the displayedd info text
+ */
+ void updateInfoText(final String _newText){
+ if (CurrentDisplayedInfo == null) {
+ context.runOnUiThread(new Runnable() {
+ @Override
+ public void run() {
+ infoText.setText(_newText);
+ infoText.setTextColor(Color.WHITE);
+ }
+ });
+ }
+ else if ( _newText.compareTo(CurrentDisplayedInfo)!=0) {
+ context.runOnUiThread(new Runnable() {
+ @Override
+ public void run() {
+ infoText.setText(_newText);
+ infoText.setTextColor(Color.WHITE);
+ }
+ });
+ }
+ CurrentDisplayedInfo = _newText;
+ Log.e("Gaspard", "New text ="+CurrentDisplayedInfo);
+ }
+
+ /**
+ *
+ * @param index index of the waypoint which marker should be red
+ */
+ void setMarkerRed(int index){
+ map.makeWaypointRed(index);
+ }
+
+ /**
+ *
+ * @param index index of the waypoint which marker should be deleted
+ */
+ void removeMarker(int index){
+ map.removeWaypoint(index);
+ }
+
+ /**
+ *
+ * @param x latitude
+ * @param y longitude
+ * @param _rota compass angle
+ * update the fields for the localisation of the aircraft
+ */
+ private void setCoord(double x, double y, float _rota) {
+ lat = x;
+ lon = y;
+ rota = _rota;
+ }
+
+ /**
+ *
+ * @param n new speed on x axis
+ */
+ private void setSpeedx(float n) {
+ speedx = n;
+ }
+
+ /**
+ *
+ * @param n new speed on y axis
+ */
+ private void setSpeedy(float n) {
+ speedy = n;
+ }
+
+ /**
+ *
+ * @param n new speed on z axis
+ */
+
+ private void setSpeedz(float n) {
+ speedz = n;
+ }
+
+ /**
+ * Initialisation of the FlightController, which monitors the state of the aircraft, our implemented callback gets the speed and the localisation of the aircraft, then update the displayed informations.
+ */
+ private void initFlightController() {
+ BaseProduct product = MainActivity.getProductInstance();
+ if (product != null && product.isConnected()) {
+ if (product instanceof Aircraft) {
+ mFlightController = ((Aircraft) product).getFlightController();
+ }
+ }
+ if (mFlightController != null) {
+ mFlightController.setStateCallback(
+ new FlightControllerState.Callback() {
+ @Override
+ public void onUpdate(@NonNull FlightControllerState djiFlightControllerCurrentState) {
+ //Log.e("Gaspard","Callback flight controller position : " + Double.toString(lat)+":"+Double.toString(lon) +" speed x :"+Double.toString(speedx));
+ setCoord(djiFlightControllerCurrentState.getAircraftLocation().getLatitude(), djiFlightControllerCurrentState.getAircraftLocation().getLongitude(), djiFlightControllerCurrentState.getAircraftHeadDirection());
+ setSpeedx(djiFlightControllerCurrentState.getVelocityX());
+ setSpeedy(djiFlightControllerCurrentState.getVelocityY());
+ setSpeedz(djiFlightControllerCurrentState.getVelocityZ());
+ context.runOnUiThread(new Runnable() {
+ @Override
+ public void run() {
+ refreshMap();
+ updateBatteryText();
+ updateSpeedText();
+ }
+ });
+ }
+ }
+ );
+ }
+ }
+}
diff --git a/Realisation_vol/App/app/src/main/java/com/proj/drone_routing/VideoFeed.java b/Realisation_vol/App/app/src/main/java/com/proj/drone_routing/VideoFeed.java new file mode 100644 index 0000000..ff39ccb --- /dev/null +++ b/Realisation_vol/App/app/src/main/java/com/proj/drone_routing/VideoFeed.java @@ -0,0 +1,72 @@ +package com.proj.drone_routing;
+
+import android.graphics.SurfaceTexture;
+import android.view.TextureView;
+
+import dji.sdk.camera.VideoFeeder;
+import dji.sdk.codec.DJICodecManager;
+
+import static dji.midware.data.manager.P3.ServiceManager.getContext;
+
+/**
+ * That class is used to initialise and to update the video feed received from the aircraft main camera.
+ */
+public class VideoFeed implements TextureView.SurfaceTextureListener {
+ private VideoFeeder.VideoDataListener videoDataListener = null;
+ private DJICodecManager codecManager = null;
+ VideoFeed(TextureView target){
+ init(target);
+ }
+
+ /**
+ * Initialise the video feed view
+ * @param target TextureView on which the video feed should be displayed
+ */
+ private void init(TextureView target) {
+ if (null != target) {
+ target.setSurfaceTextureListener(this);
+
+ // This callback is for
+
+ videoDataListener = new VideoFeeder.VideoDataListener() {
+ @Override
+ public void onReceive(byte[] bytes, int size) {
+ if (null != codecManager) {
+ codecManager.sendDataToDecoder(bytes, size);
+ }
+ }
+ };
+ }
+
+ initSDKCallback();
+ }
+
+ private void initSDKCallback() {
+ try {
+ VideoFeeder.getInstance().getPrimaryVideoFeed().addVideoDataListener(videoDataListener);
+ } catch (Exception ignored) {
+ }
+ }
+
+ public void onSurfaceTextureAvailable(SurfaceTexture surface, int width, int height) {
+ if (codecManager == null) {
+ codecManager = new DJICodecManager(getContext(), surface, width, height);
+ }
+ }
+
+ public void onSurfaceTextureSizeChanged(SurfaceTexture surface, int width, int height) {
+
+ }
+
+ public boolean onSurfaceTextureDestroyed(SurfaceTexture surface) {
+ if (codecManager != null) {
+ codecManager.cleanSurface();
+ codecManager = null;
+ }
+ return false;
+ }
+
+ public void onSurfaceTextureUpdated(SurfaceTexture surface) {
+
+ }
+}
diff --git a/Realisation_vol/App/app/src/main/java/com/proj/drone_routing/WaypointsCreator.java b/Realisation_vol/App/app/src/main/java/com/proj/drone_routing/WaypointsCreator.java new file mode 100644 index 0000000..ff7f0f7 --- /dev/null +++ b/Realisation_vol/App/app/src/main/java/com/proj/drone_routing/WaypointsCreator.java @@ -0,0 +1,57 @@ +package com.proj.drone_routing; + +import android.util.Log; + +import java.util.ArrayList; +import dji.common.mission.waypoint.Waypoint; +import dji.common.mission.waypoint.WaypointAction; +import dji.common.mission.waypoint.WaypointActionType; + +class WaypointsCreator { + + ////////////////////////////////////////////////////// + int createWaypoints(ArrayList<Waypoint> waypointsDji, + float coordInitX, + float coordInitY, + float longueurBat, + float largeurBat, + float altBat, + float angleBat, + float kHelice, + int nbWaypoint) { + + ArrayList<DroneWaypoint> droneWaypoints = AlgoPlannificator.algo( + coordInitX, coordInitY, + longueurBat, largeurBat, + altBat, + kHelice, + nbWaypoint, + angleBat); + + int res = 0; + if ( droneWaypoints!=null) { + for (DroneWaypoint wp : droneWaypoints) { + Log.e("Gaspard Waypoints ", wp.toString()); + Waypoint wpDji = new Waypoint(wp.getLat(), wp.getLon(), wp.getAlt()); + wpDji.heading = wp.getDirection(); + wpDji.gimbalPitch = wp.getGimbal(); + + if (wp.isPhoto()) { + WaypointAction wpAction = new WaypointAction(WaypointActionType.START_TAKE_PHOTO, 6); + wpDji.addAction(wpAction); + } + + waypointsDji.add(wpDji); + + res++; + + } + + return res; + } + else{ + return 0; + } + } + +} diff --git a/Realisation_vol/App/app/src/main/java/com/proj/drone_routing/WorkingActivity.java b/Realisation_vol/App/app/src/main/java/com/proj/drone_routing/WorkingActivity.java new file mode 100644 index 0000000..91f1d1b --- /dev/null +++ b/Realisation_vol/App/app/src/main/java/com/proj/drone_routing/WorkingActivity.java @@ -0,0 +1,273 @@ +package com.proj.drone_routing;
+/*
+* Separer d'un coté le code relatif a la camera
+* on a les deux bouton load et strat, du coup renommer correctement les fonction associées aux boutons, appeller les nouvelles calsses
+* et une fois le plan de vol charger, remplacer les deux boutons par des boutons de controle ( play pause, stop, return ? )
+* donc ces boutons implementent le SDK pour controler le planned vol,
+* du coup il faut supprimer les boutons precedent, et les rappeller quand ils reservent
+* suppriemr fonction inutiles*/
+import android.content.Intent;
+import android.preference.PreferenceManager;
+import android.support.constraint.ConstraintLayout;
+import android.support.v7.app.AppCompatActivity;
+import android.os.Bundle;
+import android.content.Context;
+import android.view.TextureView;
+import android.view.View;
+import android.widget.Button;
+import android.widget.TextView;
+
+//import com.google.android.gms.maps.MapView;
+
+import org.osmdroid.config.Configuration;
+import org.osmdroid.views.MapView;
+
+
+/**
+ * WorkingActivity is the running mission activity, from wich users can set up and start a mission, it allows user to keep control over the mission, displaying information and control buttons.
+ */
+public class WorkingActivity extends AppCompatActivity {
+ private VideoFeed videoView =null;
+ public Button load, start, pause, stop;
+ private boolean isloaded = false;
+ private UIUpdater uiUpdater = null;
+ public MMap mmap = null;
+ private DroneMission dronemission=null;
+ private boolean ispause = false;
+
+ /**
+ * Generates the droneMission with the given parameters and enable the start button
+ * @param param lat:lon:width:length:height:orientation
+ */
+ public void LoadMission (String param){
+ isloaded = true;
+ mmap.clearWaypoint();
+ dronemission = new DroneMission();
+ dronemission.setUiUpdater(uiUpdater);
+ String[] tab = param.split(":");
+ float lat = Float.parseFloat(tab[0]);
+ float lon = Float.parseFloat(tab[1]);
+ float lar = Float.parseFloat(tab[2]);
+ float longu = Float.parseFloat(tab[3]);
+ float alt = Float.parseFloat(tab[4]);
+ float angle = Float.parseFloat(tab[5]);
+ int nb = Integer.parseInt(tab[6]);
+ if (dronemission.generate(lat, lon, lar, longu, alt, angle, nb)) {
+ mmap.addWaypoint(dronemission);
+ enableStartButton();
+ }
+ }
+ @Override
+ protected void onCreate(Bundle savedInstanceState) {
+ super.onCreate(savedInstanceState);
+ isloaded = getIntent().getBooleanExtra("isloaded",false);
+ initUI();
+ if (isloaded){
+ LoadMission(getIntent().getStringExtra("param"));
+ }
+
+ }
+
+ /**
+ * Puts the buttons back to default ( load, and start if the parameters are loaded )
+ */
+ public void Reset(){
+ removeButton(stop);
+ removeButton(pause);
+ enableLoadButton();
+ if (isloaded) enableStartButton();
+ }
+ public void onResume(){
+ super.onResume();
+ mmap.getMv().onResume();
+
+ }
+
+ public void onPause(){
+ super.onPause();
+ mmap.getMv().onPause();
+ mmap.setCurrLoc();
+ }
+
+ /**
+ * Initialises the MMap with the MapView
+ */
+ public void initMap(){
+ final Context ctx = getApplicationContext();
+ Configuration.getInstance().load(ctx, PreferenceManager.getDefaultSharedPreferences(ctx));
+ mmap = new MMap((MapView) findViewById(R.id.mapView),ctx);
+ }
+
+ /**
+ * Initialises the UiUpdater with the mapView and the text fieds
+ */
+ private void initUIUpdater(){
+ uiUpdater = new UIUpdater(this, (TextView) findViewById(R.id.battery_level), mmap,(TextView) findViewById(R.id.info_speed), (TextView) findViewById(R.id.info_text));
+ }
+
+ /**
+ * Initialises the layout of that activity with all of its components
+ */
+ private void initUI() {
+ setContentView(R.layout.activity_woking);
+ load = null;
+ start = null;
+ videoView = new VideoFeed((TextureView) findViewById(R.id.retour_video));
+ initMap();
+ initUIUpdater();
+ enableLoadButton();
+ }
+
+ /**
+ * Behavior when InputActivity returns
+ * @param requestCode
+ * @param resultCode
+ * @param dataIntent
+ */
+ @Override
+ public void onActivityResult(int requestCode, int resultCode, Intent dataIntent){
+ if (requestCode == 1 && resultCode==RESULT_OK){
+ LoadMission(dataIntent.getStringExtra("param"));
+ }
+ }
+
+ /**
+ * Enable the Start button
+ */
+ public void enableStartButton(){
+ start = new Button(this);
+ start.setId(R.id.button_work_start);
+ start.setBackgroundResource(R.drawable.rounded_button_blue);
+ start.setText(R.string.button_start);
+ start.setOnClickListener(new Button.OnClickListener(){
+ public void onClick(View view){
+ onClickStart(view);
+ }
+ });
+ ConstraintLayout ll = findViewById(R.id.layout_working);
+
+ ConstraintLayout.LayoutParams lp = new ConstraintLayout.LayoutParams(ConstraintLayout.LayoutParams.WRAP_CONTENT,
+ ConstraintLayout.LayoutParams.WRAP_CONTENT);
+ lp.endToEnd=(R.id.layout_working);
+ lp.bottomToBottom=(R.id.layout_working);
+ ll.addView(start,lp);
+ }
+
+ /**
+ * Enable the Load Button
+ */
+ public void enableLoadButton(){
+ load = new Button(this);
+ load.setId(R.id.button_work_load);
+ load.setBackgroundResource(R.drawable.rounded_button_blue);
+ load.setText(R.string.button_load);
+ load.setOnClickListener(new Button.OnClickListener(){
+ public void onClick(View view){
+ onClickload(view);
+ }
+ });
+ ConstraintLayout ll = findViewById(R.id.layout_working);
+
+ ConstraintLayout.LayoutParams lp = new ConstraintLayout.LayoutParams(ConstraintLayout.LayoutParams.WRAP_CONTENT,
+ ConstraintLayout.LayoutParams.WRAP_CONTENT);
+ lp.startToEnd=(R.id.retour_video);
+ lp.bottomToBottom=(R.id.layout_working);
+ ll.addView(load,lp);
+ }
+
+ /**
+ *
+ * @param button Button to be removedd from the current layout
+ */
+ public void removeButton(View button){
+ ConstraintLayout ll = findViewById(R.id.layout_working);
+ if ( null!= ll){
+ ll.removeView(button);
+ }
+ }
+
+ /**
+ * Remove the Start and Load buttons and replaces them with Pause and Stop
+ */
+ public void refreshButtons(){
+ removeButton(start);
+ removeButton(load);
+ stop = new Button(this);
+ pause = new Button (this);
+ stop.setId(R.id.button_stop);
+ pause.setId(R.id.button_pause);
+ stop.setBackgroundResource(R.drawable.rounded_button_blue);
+ pause.setBackgroundResource(R.drawable.rounded_button_blue);
+ pause.setText(R.string.pause);
+ stop.setText(R.string.stop);
+ stop.setOnClickListener(new Button.OnClickListener(){
+ public void onClick(View view){
+ stop();
+ }
+ });
+ pause.setOnClickListener(new Button.OnClickListener(){
+ public void onClick(View view){
+ pause();
+ }
+ });
+ ConstraintLayout ll = findViewById(R.id.layout_working);
+
+ ConstraintLayout.LayoutParams lp = new ConstraintLayout.LayoutParams(ConstraintLayout.LayoutParams.WRAP_CONTENT,
+ ConstraintLayout.LayoutParams.WRAP_CONTENT);
+ lp.startToEnd=(R.id.retour_video);
+ lp.bottomToBottom=(R.id.layout_working);
+ ConstraintLayout.LayoutParams lp2 = new ConstraintLayout.LayoutParams(ConstraintLayout.LayoutParams.WRAP_CONTENT,
+ ConstraintLayout.LayoutParams.WRAP_CONTENT);
+ lp2.startToEnd=R.id.button_stop;
+ lp2.bottomToBottom=(R.id.layout_working);
+ ll.addView(stop, lp);
+ ll.addView(pause,lp2);
+ }
+
+ /**
+ * Starts the generated mission
+ * @param view
+ */
+ public void onClickStart (View view) {
+ if(isloaded) {
+ uiUpdater.updateInfoText("Uploading the path to the aircraft");
+
+ dronemission.start();
+ refreshButtons();
+ isloaded = false;
+ }
+ }
+
+ /**
+ * Open InputActivity and awaits for its result
+ * @param view
+ */
+ public void onClickload (View view){
+ Intent intent = new Intent(this, InputActivity.class);
+ startActivityForResult(intent,1);
+ }
+
+ /**
+ * Pauses the mission or resume it if it was paused
+ */
+ public void pause (){
+ if ( ispause) {
+ dronemission.resume();
+ pause.setText(R.string.pause);
+ ispause = false;
+ }
+ else {
+ dronemission.pause();
+ pause.setText(R.string.resume);
+ ispause = true;
+ }
+ }
+
+ /**
+ * Stops the mission
+ */
+ public void stop (){
+ dronemission.stop();
+ Reset();
+ }
+}
|
