add safe hardcoded WPs for mapping B100 at RFS

Author: orrblue

Assets/Scripts/Utility Scripts/M210_Flight_TestManager.cs

@@ -185,11 +185,15 @@ void Update()
             rosDroneConnection.UploadWaypointsTask(test_Task);
         }
 
-        // 3D Mapping hardcoded WPs here
+        // This is for flying around B100 for mapping it
+        // If yaw_turn_mode = True Untested: so ENSURE drone follows YAW angles correctly
+        // If yaw_turn_mode = False, drone flies a regular mission, always pointing in the direction of flight. Make sure camera is mounted to the drone
+        // using the mount that makes the camera pointed to the left.
         if (Input.GetKeyUp(uploadRFS_B100MappingMission))
         {
             uint[] command_list = new uint[16];
             uint[] command_params = new uint[16];
+            bool yaw_turn_mode = false;
             for (int i = 0; i < 16; i++)
             {
                 command_list[i] = 0;
@@ -200,42 +204,98 @@ void Update()
             // If so, 0 deg = North, 90 deg = East
             //         N 0
             //     315     45
-            // W 270         E 90
+            // W 270  *B100*  E 90
             //     225     135
             //        S 180
             // 
 
-            // flight path clockwise around B100 defined below:
-            // Start out in the field
-            MissionWaypointMsg mappingWP1 = new MissionWaypointMsg(37.915373f, -122.337945f, 20.0f, 3.0f, 0, 0, MissionWaypointMsg.TurnMode.CLOCKWISE, 0, 30, new MissionWaypointActionMsg(0, command_list, command_params));
+            MissionWaypointTaskMsg mapB100Task = null;
 
-            //NE Corner of B100 
-            MissionWaypointMsg mappingWP2 = new MissionWaypointMsg(37.915279f, -122.337941f, 20.0f, 3.0f, 225, 0, MissionWaypointMsg.TurnMode.CLOCKWISE, 0, 30, new MissionWaypointActionMsg(0, command_list, command_params));
+            // UNTESTED WPs. Not considered safe until tested.
+            //3D Mapping hardcoded yaw - adjusted WPs for mapping B100, for a drone outfitted with a forward - facing camera
+            // Make sure drone's camera is pointing forward as drone is supposed to turn (yaw changing) to always face the building.
 
-            //SE Corner of B100 
-            MissionWaypointMsg mappingWP3 = new MissionWaypointMsg(37.915152f, -122.337943f, 20.0f, 3.0f, 315, 0, MissionWaypointMsg.TurnMode.CLOCKWISE, 0, 30, new MissionWaypointActionMsg(0, command_list, command_params));
+                    if (yaw_turn_mode)
+            {
+                // flight path clockwise around B100 defined below:
+                // Start out in the field
+                MissionWaypointMsg mappingWP1 = new MissionWaypointMsg(37.915373f, -122.337945f, 20.0f, 3.0f, 0, 0, MissionWaypointMsg.TurnMode.CLOCKWISE, 0, 30, new MissionWaypointActionMsg(0, command_list, command_params));
+
+                //NE Corner of B100 
+                MissionWaypointMsg mappingWP2 = new MissionWaypointMsg(37.915279f, -122.337941f, 20.0f, 3.0f, 225, 0, MissionWaypointMsg.TurnMode.CLOCKWISE, 0, 30, new MissionWaypointActionMsg(0, command_list, command_params));
+
+                //SE Corner of B100 
+                MissionWaypointMsg mappingWP3 = new MissionWaypointMsg(37.915152f, -122.337943f, 20.0f, 3.0f, 315, 0, MissionWaypointMsg.TurnMode.CLOCKWISE, 0, 30, new MissionWaypointActionMsg(0, command_list, command_params));
 
-            //SW Corner of B100
-            MissionWaypointMsg mappingWP4 = new MissionWaypointMsg(37.915153f, -122.338110f, 20.0f, 3.0f, 45, 0, MissionWaypointMsg.TurnMode.CLOCKWISE, 0, 30, new MissionWaypointActionMsg(0, command_list, command_params));
+                //SW Corner of B100
+                MissionWaypointMsg mappingWP4 = new MissionWaypointMsg(37.915153f, -122.338110f, 20.0f, 3.0f, 45, 0, MissionWaypointMsg.TurnMode.CLOCKWISE, 0, 30, new MissionWaypointActionMsg(0, command_list, command_params));
 
-            //NW Corner of B100
-            MissionWaypointMsg mappingWP5 = new MissionWaypointMsg(37.915291f, -122.338112f, 20.0f, 3.0f, 135, 0, MissionWaypointMsg.TurnMode.CLOCKWISE, 0, 30, new MissionWaypointActionMsg(0, command_list, command_params));
+                //NW Corner of B100
+                MissionWaypointMsg mappingWP5 = new MissionWaypointMsg(37.915291f, -122.338112f, 20.0f, 3.0f, 135, 0, MissionWaypointMsg.TurnMode.CLOCKWISE, 0, 30, new MissionWaypointActionMsg(0, command_list, command_params));
 
-            //NE Corner of B100 
-            MissionWaypointMsg mappingWP6 = new MissionWaypointMsg(37.915279f, -122.337941f, 20.0f, 3.0f, 225, 0, MissionWaypointMsg.TurnMode.CLOCKWISE, 0, 30, new MissionWaypointActionMsg(0, command_list, command_params));
+                //NE Corner of B100 
+                MissionWaypointMsg mappingWP6 = new MissionWaypointMsg(37.915279f, -122.337941f, 20.0f, 3.0f, 225, 0, MissionWaypointMsg.TurnMode.CLOCKWISE, 0, 30, new MissionWaypointActionMsg(0, command_list, command_params));
 
-            // Back to out in the field
-            MissionWaypointMsg mappingWP7 = new MissionWaypointMsg(37.915373f, -122.337945f, 20.0f, 3.0f, 0, 0, MissionWaypointMsg.TurnMode.CLOCKWISE, 0, 30, new MissionWaypointActionMsg(0, command_list, command_params));
+                // Back to out in the field
+                MissionWaypointMsg mappingWP7 = new MissionWaypointMsg(37.915373f, -122.337945f, 20.0f, 3.0f, 0, 0, MissionWaypointMsg.TurnMode.CLOCKWISE, 0, 30, new MissionWaypointActionMsg(0, command_list, command_params));
 
-            MissionWaypointMsg[] mappingWPs = new MissionWaypointMsg[] { mappingWP1, mappingWP2, mappingWP3, mappingWP4, mappingWP5, mappingWP6, mappingWP7};
-            MissionWaypointTaskMsg mapB100Task = new MissionWaypointTaskMsg(8.0f, 4.0f, MissionWaypointTaskMsg.ActionOnFinish.NO_ACTION, 1, MissionWaypointTaskMsg.YawMode.WAYPOINT, MissionWaypointTaskMsg.TraceMode.COORDINATED, MissionWaypointTaskMsg.ActionOnRCLost.FREE, MissionWaypointTaskMsg.GimbalPitchMode.FREE, mappingWPs);
-            ViewHardcodedWaypoints(mapB100Task);
-            // Replace with below line for point-to-point transition between WPs instead of smooth (idk what it does)
-            // MissionWaypointTaskMsg test_Task = new MissionWaypointTaskMsg(8.0f, 4.0f, MissionWaypointTaskMsg.ActionOnFinish.NO_ACTION, 1, MissionWaypointTaskMsg.YawMode.WAYPOINT, MissionWaypointTaskMsg.TraceMode.POINT, MissionWaypointTaskMsg.ActionOnRCLost.FREE, MissionWaypointTaskMsg.GimbalPitchMode.FREE, mappingWPs);
+                MissionWaypointMsg[] mappingWPs = new MissionWaypointMsg[] { mappingWP1, mappingWP2, mappingWP3, mappingWP4, mappingWP5, mappingWP6, mappingWP7 };
+                mapB100Task = new MissionWaypointTaskMsg(8.0f, 4.0f, MissionWaypointTaskMsg.ActionOnFinish.NO_ACTION, 1, MissionWaypointTaskMsg.YawMode.WAYPOINT, MissionWaypointTaskMsg.TraceMode.COORDINATED, MissionWaypointTaskMsg.ActionOnRCLost.FREE, MissionWaypointTaskMsg.GimbalPitchMode.FREE, mappingWPs);
+                ViewHardcodedWaypoints(mapB100Task);
+                // Replace with below line for point-to-point transition between WPs instead of smooth (idk what it does)
+                // MissionWaypointTaskMsg test_Task = new MissionWaypointTaskMsg(8.0f, 4.0f, MissionWaypointTaskMsg.ActionOnFinish.NO_ACTION, 1, MissionWaypointTaskMsg.YawMode.WAYPOINT, MissionWaypointTaskMsg.TraceMode.POINT, MissionWaypointTaskMsg.ActionOnRCLost.FREE, MissionWaypointTaskMsg.GimbalPitchMode.FREE, mappingWPs);
+
+            }
+
+            // Flies Counter clockwise (CCW) around B100 without any fancy yaw -- drone always points in the direction of flight, as is usual
+            // These WPs lat/long are safe. Fly at high altitude to verify they're good. They also show up in Unity so you can see where they
+            // are in relation to the building. I've flown with these lat/long through iPAD DJI GS Pro app, and then compied the lat/long values
+            // to here so they are supposed to be safe if you use these. Just set the altitude to your preference. 
+            else if (yaw_turn_mode == false)
+            {
+                // flight path ***COUNTER-CLOCKWISE*** around B100 defined below:
+                // Start out next to building. Have drone hovering close to this WP when starting mapping routine on jetson computer.
+                //NW Corner of B100
+                MissionWaypointMsg mappingWP1a = new MissionWaypointMsg(37.915306300f, -122.338041512f, 10.0f, 3.0f, 0, 0, MissionWaypointMsg.TurnMode.CLOCKWISE, 0, 30, new MissionWaypointActionMsg(0, command_list, command_params));
+                MissionWaypointMsg mappingWP1b = new MissionWaypointMsg(37.915266382f, -122.338096355f, 10.0f, 3.0f, 0, 0, MissionWaypointMsg.TurnMode.CLOCKWISE, 0, 30, new MissionWaypointActionMsg(0, command_list, command_params));
+
+                //SW Corner of B100 
+                MissionWaypointMsg mappingWP2a = new MissionWaypointMsg(37.915208625f, -122.338096832f, 10.0f, 3.0f, 0, 0, MissionWaypointMsg.TurnMode.CLOCKWISE, 0, 30, new MissionWaypointActionMsg(0, command_list, command_params));
+                MissionWaypointMsg mappingWP2b = new MissionWaypointMsg(37.915168440f, -122.338051218f, 10.0f, 3.0f, 0, 0, MissionWaypointMsg.TurnMode.CLOCKWISE, 0, 30, new MissionWaypointActionMsg(0, command_list, command_params));
+
+                //SE Corner of B100
+                MissionWaypointMsg mappingWP3a = new MissionWaypointMsg(37.915165315f, -122.337991285f, 10.0f, 3.0f, 0, 0, MissionWaypointMsg.TurnMode.CLOCKWISE, 0, 30, new MissionWaypointActionMsg(0, command_list, command_params));
+                MissionWaypointMsg mappingWP3b = new MissionWaypointMsg(37.915202140f, -122.337917794f, 10.0f, 3.0f, 0, 0, MissionWaypointMsg.TurnMode.CLOCKWISE, 0, 30, new MissionWaypointActionMsg(0, command_list, command_params));
+
+                //NE Corner of B100
+                MissionWaypointMsg mappingWP4a = new MissionWaypointMsg(37.915273840f, -122.337920489f, 10.0f, 3.0f, 0, 0, MissionWaypointMsg.TurnMode.CLOCKWISE, 0, 30, new MissionWaypointActionMsg(0, command_list, command_params));
+                MissionWaypointMsg mappingWP4b = new MissionWaypointMsg(37.915327647f, -122.337965254f, 10.0f, 3.0f, 0, 0, MissionWaypointMsg.TurnMode.CLOCKWISE, 0, 30, new MissionWaypointActionMsg(0, command_list, command_params));
+
+                //NW Corner of B100 
+                MissionWaypointMsg mappingWP5 = new MissionWaypointMsg(37.915326300f, -122.338047125f, 10.0f, 3.0f, 0, 0, MissionWaypointMsg.TurnMode.CLOCKWISE, 0, 30, new MissionWaypointActionMsg(0, command_list, command_params));
+
+                // Back to out in the field
+                MissionWaypointMsg mappingWP6 = new MissionWaypointMsg(37.915387568f, -122.337874484f, 10.0f, 3.0f, 0, 0, MissionWaypointMsg.TurnMode.CLOCKWISE, 0, 30, new MissionWaypointActionMsg(0, command_list, command_params));
+
+                MissionWaypointMsg[] mappingWPs = new MissionWaypointMsg[] {
+                    mappingWP1a,
+                    mappingWP1b,
+                    mappingWP2a,
+                    mappingWP2b,
+                    mappingWP3a,
+                    mappingWP3b,
+                    mappingWP4a,
+                    mappingWP4b,
+                    mappingWP5,
+                    mappingWP6};
+                mapB100Task = new MissionWaypointTaskMsg(0.5f, 0.5f, MissionWaypointTaskMsg.ActionOnFinish.NO_ACTION, 1, MissionWaypointTaskMsg.YawMode.AUTO, MissionWaypointTaskMsg.TraceMode.COORDINATED, MissionWaypointTaskMsg.ActionOnRCLost.FREE, MissionWaypointTaskMsg.GimbalPitchMode.FREE, mappingWPs);
+                ViewHardcodedWaypoints(mapB100Task);
+            }
 
             rosDroneConnection.UploadWaypointsTask(mapB100Task);
         }
 
+
         if (Input.GetKeyUp(missionInfo))
         {
             rosDroneConnection.FetchMissionStatus();