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]) # 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]) lon= float(loc_array[i]) alt= float(loc_array[i]) last_id= int(loc_array[-1]) 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]), last_id) break break time.sleep(5)