def nav_goto_gps_xy_body_coordinates(self, x, y, alt = 0):
if(not self.isAirborne() or not self.isReady() or self._IsFlying):
print("Not airbourne")
self._IsFlying = True
# set airspeed
self.vehicle.airspeed = self.AIR_SPEED
# if altitude is not especified then get current altitude
current_altitude = self.vehicle.location.global_relative_frame.alt if alt == 0 else alt
# create gps location point
target_location = ArduTools.get_location_metres_local(self.vehicle.location.global_frame, self.vehicle.heading, x, y, alt)
target_distance = ArduTools.get_distance_metres(self.vehicle.location.global_frame, target_location)
self.nav_goto_heading(ArduTools.get_bearing(self.vehicle.location.global_frame, target_location), overide = True)
self.vehicle.simple_goto(target_location)
while self.isActive("GUIDED"):
remainingDistance = ArduTools.get_distance_metres(self.vehicle.location.global_frame, target_location)
print("Distance to target: ", remainingDistance)
if ArduTools.hasReachedDestination(self.vehicle.location.global_frame, target_location):
print("Reached target")
break
time.sleep(1)
self._IsFlying = False
def returnToHome(self):
self._IsFlying = False
time.sleep(1)
if(self.isAirborne() and self.isReady() and not self._IsFlying):
self._IsFlying = True
# set airspeed
self.vehicle.airspeed = self.AIR_SPEED
# set TRL mode to return
self.vehicle.mode = VehicleMode("RTL")
time.sleep(2)
# create gps location point
target_location = self.vehicle.home_location
print(target_location)
while self.isActive("RTL"):
if target_location == None or self.vehicle.location.global_frame == None:
print("global_frame", self.vehicle.location.global_frame)
time.sleep(1)
continue
# target_distance = ArduTools.get_distance_metres(self.vehicle.location.global_frame, target_location)
# print("target_distance", target_distance)
remainingDistance = ArduTools.get_distance_metres(self.vehicle.location.global_frame, target_location)
print("Distance to target: ", remainingDistance)
if ArduTools.hasReachedDestination(self.vehicle.location.global_frame, target_location):
print("Reached target")
break
time.sleep(1)
while self.isActive("RTL"):
print(" Altitude: ", self.vehicle.location.global_relative_frame.alt)
if self.vehicle.location.global_relative_frame.alt <= 0.05: #Trigger just below target alt.
print("Landed")
break
time.sleep(1)
self._IsFlying = False
else:
print("Not on the air")