New Navigation Method and FlightTest FIle

Created send_ned_velocity method in the navigator and a method to get vector components of the distance between the waypoints.

These methods were then utilized for the flight test so that the drone makes a turn around the waypoint

src/flight_tests/task1_mock_alt.py

@@ -0,0 +1,121 @@
+import time
+
+from dronekit import connect, VehicleMode, LocationGlobal
+
+from src.modules.autopilot import navigator
+from src.modules.autopilot import lander
+
+CONN_STR = "udp:127.0.0.1:14551"
+MESSENGER_PORT = 14552
+
+drone = connect(CONN_STR, wait_ready=False)
+
+nav = navigator.Navigator(drone, MESSENGER_PORT)
+lander = lander.Lander()
+
+nav.send_status_message("Shepard is online")
+
+while not (drone.armed and drone.mode == VehicleMode("GUIDED")):
+    pass
+
+nav.send_status_message("Executing mission")
+time.sleep(2)
+
+nav.takeoff(10)
+#drone.groundspeed = 2  # m/s
+start_coords = drone.location.global_relative_frame
+time.sleep(2)
+
+MAX_GROUND_SPEED = 20
+TIME = 180  #3 minutes
+ALTITUDE = 15
+WAY_POINT1 = [53.496500, -113.551900]
+WAY_POINT2 = [53.497400, -113.551900]
+#WAY_POINT3 = [53.496588, -113.548689]
+
+#Hard coded position which is 5m away from the waypoint
+
+MOVE_MARK1 = [53.496450, -113.551850]
+MOVE_MARK2 = [53.497450, -113.551850]
+
+waypoint1_location_global = LocationGlobal(WAY_POINT1[0], WAY_POINT1[1], ALTITUDE)
+waypoint2_location_global = LocationGlobal(WAY_POINT2[0], WAY_POINT2[1], ALTITUDE)
+#waypoint3_location_global = LocationGlobal(WAY_POINT3[0], WAY_POINT3[1], ALTITUDE)
+
+
+movemark1_location_global = LocationGlobal(MOVE_MARK1[0], MOVE_MARK1[1], ALTITUDE)
+movemark2_location_global = LocationGlobal(MOVE_MARK2[0], MOVE_MARK2[1], ALTITUDE)
+
+distance_marks = nav.__get_distance_metres(movemark1_location_global, movemark2_location_global)
+
+
+speed = 10
+
+#checking to ensure ground speed is safe
+assert speed > 0
+assert speed < MAX_GROUND_SPEED
+
+#drone.groundspeed = speed
+#nav.send_status_message(f"Ground speed set to {speed} m/s")
+
+# workaround to get the speed to set properly for the actual waypoints
+nav.set_position_relative(0, 0)
+
+time.sleep(1)
+nav.set_speed(speed)
+time.sleep(1)
+
+
+# https://mavlink.io/en/messages/common.html#SET_POSITION_TARGET_LOCAL_NED
+
+
+# Predecided number of laps for the drone to complete (This will later be adjusted based on battery consumption)
+
+laps = 2
+
+
+
+nav.send_status_message("Moving Around Waypoint 1")
+start_vector_distance = nav.__get_vector_distance(start_coords, movemark1_location_global)
+
+start_x_distance = start_vector_distance[0]
+start_y_distance = start_vector_distance[1]
+
+nav.send_ned_velocity(-start_x_distance, start_y_distance)
+nav.send_ned_velocity(-5, 5)
+nav.send_ned_velocity(5, 5)
+
+for i in range(laps):
+
+    nav.send_status_message("Moving Around Waypoint 2")
+
+    current_coords2 = drone.location.global_relative_frame
+
+    point2_vector_distance = nav.__get_vector_distance(current_coords1, movemark2_location_global)
+    point2_x_distance = point2_vector_distance[0]
+    point2_y_disance = point2_vector_distance[1]
+
+    nav.send_ned_velocity(point2_x_distance, -point2_y_disance)
+    nav.send_ned_velocity(5, -5)
+    nav.send_ned_velocity(-5, -5)
+
+    current_coords1 = drone.location.global_relative_frame
+
+    point1_vector_distance = nav.__get_vector_distance(current_coords1, movemark1_location_global)
+    point1_x_distance = point1_vector_distance[0]
+    point1_y_disance = point1_vector_distance[1]
+
+    nav.send_ned_velocity(-point1_x_distance, point1_y_disance)
+    nav.send_ned_velocity(-5, 5)
+    nav.send_ned_velocity(5, 5)
+
+    nav.send_status_message(f"Completed Lap {i} out of {laps} laps")
+
+nav.return_to_launch()
+
+drone.close()
+
+nav.send_status_message("Flight test script execution terminated")
+
+
+

src/modules/autopilot/navigator.py

@@ -233,6 +233,41 @@ def set_altitude_position_relative(self, d_north, d_east, alt):
                 break
             time.sleep(2)
 
+    def send_ned_velocity(self, x, y):
+        """
+        Controlling Vehicle movement via velocity control. The distances are relative to the vehicle
+        NOTE: All lengths are in meters
+
+        :param x: Distance to be moved North
+        :param y: Distance to be moved East
+        """
+
+        #https://mavlink.io/en/messages/common.html#SET_POSITION_TARGET_LOCAL_NED
+
+        distance = (x**2 + y**2 + z**2)**1/2
+
+        self.__message(f"Moving drone {distance}m ")
+        msg = self.vehicle.message_factory.set_position_target_local_ned_encode(
+                0, 0, 0,  #  timestamp, target system, target component
+                7,  # MAV_FRAME_LOCAL_OFFSET_NED frame (Sets coordinates relative to the drone)
+                0b0000111111111100 ,  # typemask to state that only x and y positions are necessary and the rest are ignored
+                x,  # x position
+                y,  # y position
+                0,  # z position
+                0,  # x velocity  (ignored)
+                0,  # y velocity  (ignored)
+                0,  # z velocity  (ignored)
+                0,  # x acceleration (ignored for now)
+                0,  # y acceleration (ignored for now)
+                0,  # z acceleration (ignored for now)
+                0,  #yaw (ignored)
+                0   #yaw rate (ignored)
+                )
+
+        self.vehicle.send_mavlink(msg)
+        self.vehicle.flush()
+
+
     def land(self):
         """
         Lands the vehicle.
@@ -321,6 +356,28 @@ def __get_distance_metres(self, location_1, location_2):
 
         return math.sqrt((d_lat * d_lat) + (d_lon * d_lon)) * 1.113195e5
 
+    def __get_vector_distance(self, location_1, location_2, altitude):
+        """
+        Returns the x and y components of the distance between two `LocationGlobal` objects.'
+        
+        :param location_1: The first `LocationGlobal` or `LocationGlobalRelative` object.
+        :param location_2: The second `LocationGlobal` or `LocationGlobalRelative` object.
+        :param altiutude: The current altitude
+        :return: The x and y components of the distance between the two points. 
+        """
+
+        # Draws a rectangle on the earth's surface and finds the base and the height of this rectangle as the vector components
+
+        p1 = LocationGlobal(location_1.lat, location_2.lon, altitude)
+        p2 = LocationGlobal(location_2.lat, location_1.lon, altitude)
+
+        x = self.__get_distance_metres(location_1, p1)
+        y = self.__get_distance_metres(location_2, p2)
+
+        return (x, y)
+
+
+
     def __condition_yaw(self, heading, relative=False):
         if relative:
             is_relative = 1  # yaw relative to direction of travel
@@ -364,3 +421,6 @@ def optimum_speed(self, time_left, waypoints):
                 f"Speed required to travel {total_distance} m in {time_left} s is {speed_required} m/s")
 
         return speed_required
+
+
+