def isFacingObstacle(minDist=150, maxMeasures=1):
obstacleDist = bb.getDist("front")
result = not obstacleDist or obstacleDist < minDist
actualMeasure = 0
while result and actualMeasure < maxMeasures:
print("Facing obstacle at dist(" + str(maxMeasures) + "): ", obstacleDist)
time.sleep(0.1)
obstacleDist = bb.getDist("front")
result = not obstacleDist or obstacleDist < minDist
actualMeasure = actualMeasure + 1
if result:
print("Facing obstacle at dist(" + str(maxMeasures) + "): ", obstacleDist)
print("AUsweichen?", result)
return result
import bb
import time
time.sleep(1)
print("dist. ", bb.getDist(0))
def landOnObstacle():
obstacleDist = bb.getDist("front")
if not obstacleDist or obstacleDist < 200:
print("Aborting mission, obstacle found at: ", obstacleDist, " cm")
landAndDisarmAndClose()
bb.cleanAndExit()
bb.setupPins()
altitude = 1
startLocation = vehicle.location.global_relative_frame
startLocation.alt = altitude
point1 = get_location_metres(startLocation, 5, 0)
point2 = get_location_metres(point1, 0, -5)
point3 = get_location_metres(point2, -5, 0)
arm_and_takeoff(altitude)
speed = 1
print("Setting speed to ", speed)
setSpeed(speed)
for p in [point1, point2, point3, startLocation]:
dist = get_distance_metres(vehicle.location.global_relative_frame, p)
print("distance to ", p, ": ", dist)
obstacleDist = bb.getDist("front")
landOnObstacle()
vehicle.simple_goto(p, groundspeed=speed)
while dist > 1:
print("flying to waypoint, dist: ", dist)
landOnObstacle()
time.sleep(0.1)
dist = get_distance_metres(vehicle.location.global_relative_frame, p)
print("reached waypoint!")
time.sleep(3)
print("Landing...")
landAndDisarmAndClose()