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 nav_goto_heading(heading, cw = True):
direction = 1 if cw else -1
# create the CONDITION_YAW command using command_long_encode()
msg = vehicle.message_factory.command_long_encode(
0, 0, # target system, target component
mavutil.mavlink.MAV_CMD_CONDITION_YAW, #command
0, #confirmation
heading, # param 1, yaw in degrees
0, # param 2, yaw speed deg/s
direction, # param 3, direction -1 ccw, 1 cw
0, # param 4, relative offset 1, absolute angle 0
0, 0, 0) # param 5 ~ 7 not used
# send command to vehicle
vehicle.send_mavlink(msg)
print "Target heading ", heading
while not ArduTools.hasReachedHeading(vehicle.heading, heading):
print " Heading: ", vehicle.heading
# if vehicle.heading >= target-2 and vehicle.heading <= target+2: #Trigger just below target alt.
# print "Reached target heading"
# break
time.sleep(1)
print "Reached heading ", vehicle.heading
def nav_change_heading(angle):
direction = 1 if angle > 0 else -1
heading = math.fabs(angle)
target = vehicle.heading + angle
target = target if target < 360 else target - 360
target = target if target > 0 else target + 360
# create the CONDITION_YAW command using command_long_encode()
msg = vehicle.message_factory.command_long_encode(
0, 0, # target system, target component
mavutil.mavlink.MAV_CMD_CONDITION_YAW, #command
0, #confirmation
heading, # param 1, yaw in degrees
0, # param 2, yaw speed deg/s
direction, # param 3, direction -1 ccw, 1 cw
1, # param 4, relative offset 1, absolute angle 0
0, 0, 0) # param 5 ~ 7 not used
# send command to vehicle
vehicle.send_mavlink(msg)
# Wait for takeoff to finish
print "Target heading ", target
#while True:
while not ArduTools.hasReachedHeading(vehicle.heading, target):
print " Heading: ", vehicle.heading
#if vehicle.heading >= target-2 and vehicle.heading <= target+2: #Trigger just below target alt.
# print "Reached target heading"
# break
time.sleep(1)
print "Reached heading ", vehicle.heading
def setAttitude(self, roll_angle = 0.0, pitch_angle = 0.0, yaw_rate = 0.0, thrust = 0.5, duration = 0):
"""
Note that from AC3.3 the message should be re-sent every second (after about 3 seconds
with no message the velocity will drop back to zero). In AC3.2.1 and earlier the specified
velocity persists until it is canceled. The code below should work on either version
(sending the message multiple times does not cause problems).
"""
"""
The roll and pitch rate cannot be controllbed with rate in radian in AC3.4.4 or earlier,
so you must use quaternion to control the pitch and roll for those vehicles.
"""
# Thrust > 0.5: Ascend
# Thrust == 0.5: Hold the altitude
# Thrust < 0.5: Descend
msg = self.vehicle.message_factory.set_attitude_target_encode(
0, # time_boot_ms
1, # Target system
1, # Target component
0b00000000, # Type mask: bit 1 is LSB
ArduTools.to_quaternion(roll_angle, pitch_angle), # Quaternion
0, # Body roll rate in radian
0, # Body pitch rate in radian
math.radians(yaw_rate), # Body yaw rate in radian
thrust # Thrust
)
self.vehicle.send_mavlink(msg)
start = time.time()
while time.time() - start < duration:
self.vehicle.send_mavlink(msg)
time.sleep(0.1)
def nav_goto_heading(self, heading, cw = True, overide = False):
if(not self.isAirborne() or not self.isReady() or (self._IsFlying and not overide)):
print("Not airbourne")
if not overide:
self._IsFlying = True
direction = 1 if cw else -1
# create the CONDITION_YAW command using command_long_encode()
msg = self.vehicle.message_factory.command_long_encode(
0, 0, # target system, target component
mavutil.mavlink.MAV_CMD_CONDITION_YAW, #command
0, #confirmation
heading, # param 1, yaw in degrees
0, # param 2, yaw speed deg/s
direction, # param 3, direction -1 ccw, 1 cw
0, # param 4, relative offset 1, absolute angle 0
0, 0, 0) # param 5 ~ 7 not used
# send command to vehicle
self.vehicle.send_mavlink(msg)
print("Target heading ", heading)
while self.isActive("GUIDED"):
print(" Heading: ", self.vehicle.heading)
if ArduTools.hasReachedHeading(self.vehicle.heading, heading):
print("Reached heading ", self.vehicle.heading)
break
time.sleep(1)
if not overide:
self._IsFlying = False
def land(self):
if not self.isAirborne():
print("Not on the air")
return
print("Begin landing!")
#Release any current task
self._IsFlying = False
time.wait(2)
#Begin task
self._IsFlying = True
self.vehicle.mode = VehicleMode("LAND")
aTargetAltitude = 0
while self.isActive("LAND"):
print(" Altitude: ", self.vehicle.location.global_relative_frame.alt)
if ArduTools.hasReachedAltitude(self.vehicle.location.global_relative_frame.alt, aTargetAltitude):
#if self.vehicle.location.global_relative_frame.alt <= 0.05: #Trigger just below target alt.
print("Landed")
break
time.sleep(1)
#Wair for automatic disaming
time.sleep(3)
#Release current task
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")
def takeoff(self, aTargetAltitude = 2):
if not self._IsConnected:
print("Not connected")
return
if self._IsFlying:
print("Busy cant takeoff now")
return
if self.isAirborne():
print("Already on the air")
return
while not self.vehicle.is_armable:
print(" Waiting for vehicle to initialise...")
time.sleep(1)
self.vehicle.mode = VehicleMode("GUIDED")
self.vehicle.armed = True
while not self.vehicle.armed:
print(" Waiting for arming...")
time.sleep(1)
print("Taking off!")
self._IsFlying = True
self.vehicle.simple_takeoff(aTargetAltitude)
time.sleep(2)
# Wait for takeoff to finish
while self.isActive("GUIDED"):
print(" Altitude: ", self.vehicle.location.global_relative_frame.alt)
if ArduTools.hasReachedAltitude(self.vehicle.location.global_relative_frame.alt, aTargetAltitude):
#if self.vehicle.location.global_relative_frame.alt >= aTargetAltitude*0.95: #Trigger just below target alt.
print("Reached target altitude")
break
time.sleep(1)
self._IsFlying = False
def nav_change_heading(self, angle, overide = False):
if(not self.isAirborne() or not self.isReady() or (self._IsFlying and not overide)):
print("Not airbourne")
if not overide:
self._IsFlying = True
direction = 1 if angle > 0 else -1
heading = math.fabs(angle)
target = self.vehicle.heading + angle
target = target if target < 360 else target - 360
target = target if target > 0 else target + 360
# create the CONDITION_YAW command using command_long_encode()
msg = self.vehicle.message_factory.command_long_encode(
0, 0, # target system, target component
mavutil.mavlink.MAV_CMD_CONDITION_YAW, #command
0, #confirmation
heading, # param 1, yaw in degrees
0, # param 2, yaw speed deg/s
direction, # param 3, direction -1 ccw, 1 cw
1, # param 4, relative offset 1, absolute angle 0
0, 0, 0) # param 5 ~ 7 not used
# send command to vehicle
self.vehicle.send_mavlink(msg)
# Wait for takeoff to finish
print("Target heading ", target)
while self.isActive("GUIDED"):
print(" Heading: ", self.vehicle.heading)
if ArduTools.hasReachedHeading(self.vehicle.heading, target):
print("Reached heading ", self.vehicle.heading)
break
time.sleep(1)
if not overide:
self._IsFlying = False
def nav_goto_altitute(self, aTargetAltitude):
if(not self.isAirborne() or not self.isReady() or self._IsFlying):
print("Not airbourne")
self._IsFlying = True
current_loc = self.vehicle.location.global_relative_frame #get current location
target_loc = LocationGlobalRelative(current_loc.lat, current_loc.lon, aTargetAltitude)
self.vehicle.simple_goto(target_loc)
while self.isActive("GUIDED"):
print(" Altitude: ", self.vehicle.location.global_relative_frame.alt)
if ArduTools.hasReachedAltitude(self.vehicle.location.global_relative_frame.alt, aTargetAltitude):
print("Reached target altitude ", self.vehicle.location.global_relative_frame.alt)
break
time.sleep(1)
self._IsFlying = False
from Tools import ArduTools
target = 3
if ArduTools.hasReachedHeading(356, target):
print "Reached case 356 of ", target
if ArduTools.hasReachedHeading(357, target):
print "Reached case 357 of ", target
if ArduTools.hasReachedHeading(358, target):
print "Reached case 358 of ", target
if ArduTools.hasReachedHeading(359, target):
print "Reached case 359 of ", target
if ArduTools.hasReachedHeading(0, target):
print "Reached case 0 of ", target
if ArduTools.hasReachedHeading(1, target):
print "Reached case 1 of ", target
if ArduTools.hasReachedHeading(2, target):
print "Reached case 2 of ", target
if ArduTools.hasReachedHeading(3, target):
print "Reached case 3 of ", target
if ArduTools.hasReachedHeading(4, target):
print "Reached case 4 of ", target
if ArduTools.hasReachedHeading(5, target):
print "Reached case 5 of ", target
print("")
print("#################################################################")
print("# Test area coverage")
#target = ArduTools.get_location_metres(UAV.vehicle.location.global_frame, 50, 50)
newlat = -35.363004
newlon = 149.166698
target = LocationGlobal(newlat, newlon, UAV.vehicle.location.global_frame.alt)
print (target)
points = ArduTools.calculate_boustrophedon_points(50, 10)
waypoints = []
for p in points:
destination = ArduTools.get_location_metres(target, p[0], p[1])
waypoints.append(destination)
print (destination)
UAV.nav_goto_gps_point(destination)
print(points)
print(waypoints)
time.sleep(5)
print("")
print("#################################################################")
