I am trying to make a script using python paymavlink and mavproxy to make drone go to multiple waypoints and land using aruco markers. script works perfectly for first waypoint but when the script passes altitude for second waypoint it takesoff and stops 3 meter before reaching the altitude specified.
for ex. if i entered 24 m in second waypoint it will stop at 21 m height.
def arm_and_takeoff(loc_array):
#print("1st wave point is:" , param1, " ", param2, "on altitude of" , param3)
print ("Basic pre-arm checks")
# Don't let the user try to arm until autopilot is ready
while not vehicle.is_armable:
print (" Waiting for vehicle to initialise...")
time.sleep(1)
print(loc_array)
for i in range(len(loc_array)):
print ("Arming motors")
aTargetAltitude = float(loc_array[i][2])
# Copter should arm in GUIDED mode
vehicle.mode = VehicleMode("GUIDED")
vehicle.armed = True
while not vehicle.armed:
print (" Waiting for arming...")
time.sleep(1)
print ("Taking off!")
vehicle.simple_takeoff(aTargetAltitude) # Take off to target altitude
time.sleep(2)
# Check that vehicle has reached takeoff altitude
while True:
print(aTargetAltitude)
print (" Altitude: ", vehicle.location.global_relative_frame.alt )
time.sleep(1)
#Break and return from function just below target altitude.
if vehicle.location.global_relative_frame.alt>=aTargetAltitude*0.95:
print ("Reached target altitude")
print("going to waypoint")
lat= float(loc_array[i][0])
lon= float(loc_array[i][1])
alt= float(loc_array[i][2])
last_id= int(loc_array[-1][3])
print(lat, lon, alt)
point1 = LocationGlobalRelative(lat, lon, alt)
print(point1)
vehicle.simple_goto(point1)
time.sleep(3)
while True:
print("latitude: ", vehicle.location.global_relative_frame.lat )
time.sleep(2)
cam_on(int(loc_array[i][3]), last_id)
break
break
time.sleep(5)