def _thread_handler(cls) -> None: while cls._status: LOG_INFO = "gy_neo6mv2.py - _thread_handler()" try: gps_data = cls._serial.read_until( GY_NEO6MV2.LINE_END).decode("ascii") data_split = gps_data.split(",") if "$GPGGA" != data_split[0]: continue cls._last_data = { "timestamp": Time.get_current_timestamp("ms"), "latitude": float(data_split[2]) / 100, # Degree, north "longitude": float(data_split[4]) / 100, # Degree, east "fix": int(data_split[6]), # Fix quality "sat_count": int(data_split[7]), # Number of satellites being tracked "altitude": float(data_split[9]) # Meters, above mean sea level } except ValueError: Logger.PrintLog( LOG_INFO, "Couldn't parse gps data. Perhaps module is still initializing?" ) except Exception as ex: Logger.PrintException(LOG_INFO, ex) continue
def impl(): COMMAND = eb_mavutil.Enum.get_int_reference("MAV_CMD_DO_SET_MODE") self._drone.mav().command_long_send( self._drone._get_target_system(), 0, COMMAND, 0, # params eb_mavutil.Enum.get_int_reference( "MAV_MODE_FLAG_CUSTOM_MODE_ENABLED"), flight_mode_list[flight_mode], 0, 0, 0, 0, 0) res = self._drone.wait_cmd_ack(COMMAND, timeout) if res == 0: Logger.PrintLog( self.LOG_INFO, "set_flight_mode() - Flight mode successfully set to {}.". format(flight_mode)) return True else: Logger.PrintLog( self.LOG_INFO, "set_flight_mode() - An error occured while setting flight mode to {}. Result: {}." .format(flight_mode, str(res))) return False
def move_based_local_ned(self, vel_x, vel_y, vel_z, duration_ms=500): Logger.PrintLog( self.LOG_INFO, "move_based_local_ned() - Moving based on NED. X:{} m/s, Y:{} m/s, Z:{} m/s." .format(str(vel_x), str(vel_y), str(vel_z))) if self._drone.telemetry().get_flight_mode() != "GUIDED": Logger.PrintLog( self.LOG_INFO, "move_based_local_ned() - Cannot move based on NED. Drone is not in GUIDED mode." ) return False start_timestamp = Time.get_current_timestamp("ms") while 1: eb_mavutil.Enum.get_method_reference( self._drone.mav(), "SET_POSITION_TARGET_LOCAL_NED")( 0, self._drone._get_target_system(), self._drone._get_target_component(), eb_mavutil.Enum.get_int_reference("MAV_FRAME_LOCAL_NED"), 0b0000111111000111, 0, 0, 0, vel_x, vel_y, vel_z, 0, 0, 0, 0, 0) if duration_ms <= 0 \ or Time.get_current_timestamp("ms") - start_timestamp >= duration_ms: break return True
def is_reached_to_local_position(self, dest_x, dest_y, dest_z, threshold=0.1): curr_local_pos = self._drone.telemetry().get_local_position() if dest_z - threshold <= curr_local_pos["z"] <= dest_z + threshold: dist_to_dest = EB_Math.TwoDimensional.distance_between_two_points( (curr_local_pos["x"], curr_local_pos["y"]), (dest_x, dest_y)) Logger.PrintLog( self.LOG_INFO, "is_reached_to_local_position() - Current X: {}, Current Y: {}, Current Z: {}, Destination X: {}, Destination Y: {}, Destination Z: {}, Threshold: {}." .format(str(curr_local_pos["x"]), str(curr_local_pos["y"]), str(curr_local_pos["z"]), str(dest_x), str(dest_y), str(dest_z), str(threshold))) if dist_to_dest <= threshold: Logger.PrintLog( self.LOG_INFO, "is_reached_to_local_position() - Returning True.") return True else: Logger.PrintLog( self.LOG_INFO, "is_reached_to_local_position() - Distance to destination: {} m." .format(str(dist_to_dest))) Logger.PrintLog(self.LOG_INFO, "is_reached_to_local_position() - Returning False.") return False
def _thread_handler(cls) -> None: LOG_INFO = cls.LOG_INFO + "_thread_handler()" while cls.get_status(): try: cls._socket.settimeout(cls.get_read_timeout()) recv_data, addr = cls._socket.recvfrom(config.Connection.BUFFER_SIZE) addr_str = UDP_Client.get_addr_str(addr) if recv_data == b"": Logger.PrintLog(LOG_INFO, "Connection with server has been lost.") raise ConnectionAbortedError if addr_str not in cls._data: cls._data[addr_str] = {} if addr_str not in cls._storage: cls._storage[addr] = { "last_packet_type": telemetry.Packet.Type.UNKNOWN, "failure_tracker" : {} } packet_type = telemetry.Packet.get_type(recv_data, just_start=True) is_data_append = False if not cls.get_is_packet_type_exist(addr_str, packet_type) \ and packet_type != telemetry.Packet.Type.UNKNOWN: cls.set_packet_data(addr_str, packet_type, b"") if packet_type == telemetry.Packet.Type.MESSAGE \ and packet_type == telemetry.Packet.Message.Response.CLEAR: del cls._data cls.set_last_packet_type(addr_str, telemetry.Packet.Type.MESSAGE) Logger.PrintLog(LOG_INFO, "Recieved clear request from server. Clearing storage.") continue # UNKNOWN PACKET if packet_type == telemetry.Packet.Type.UNKNOWN \ and cls.get_last_packet_type(addr_str) == telemetry.Packet.Type.UNKNOWN: cls.set_last_packet_type(addr_str, telemetry.Packet.Type.UNKNOWN) Logger.PrintLog(LOG_INFO, "Got unknown packet. ({})".format(str(recv_data))) continue else: # Continuation of previous packet(s) if packet_type == telemetry.Packet.Type.UNKNOWN: packet_type = cls.get_last_packet_type(addr_str) is_data_append = True cls.set_packet_data(addr_str, packet_type, recv_data, is_data_append) cls.set_last_packet_type(addr_str, packet_type) if callable(cls._handler): cls._handler(addr, cls) except Exception as ex: cls.exception_handler(__file__, ex)
def stop(self): Logger.PrintLog(self.LOG_INFO, "stop() - Stopping mission(s).") if self.get_status() != 1: Logger.PrintLog( self.LOG_INFO, "stop() - Cannot stop the mission(s). Current status is {}.". format(str(self._status))) return False self._drone.action().set_flight_mode("LOITER") self._status = 2 return True
def impl(cls, hold): Logger.PrintLog( cls.LOG_INFO, "hold_global_position() - Handler has started. Lat:{}, Lon:{}, Alt:{}." .format(str(hold["current_pos"]["lat"]), str(hold["current_pos"]["lon"]), str(hold["current_pos"]["relative_alt"]))) start_timestamp = Time.get_current_timestamp("ms") update_counter = -1 while hold["hold"] \ and self._drone.telemetry().get_flight_mode() == "GUIDED": if hold.get("skip", False): continue if hold.get("pos_override", None) is not None: Logger.PrintLog( cls.LOG_INFO, "hold_global_position() - impl() - Position override detected. Current lat: {}, lon:{}, relative alt:{}. Overridden lat: {}, lon: {}, relative alt: {}." .format(str(hold["current_pos"]["lat"]), str(hold["current_pos"]["lon"]), str(hold["current_pos"]["relative_alt"]), str(hold["pos_override"]["lat"]), str(hold["pos_override"]["lon"]), str(hold["pos_override"]["relative_alt"]))) hold["current_pos"] = hold["pos_override"] hold["pos_override"] = None if floor((Time.get_current_timestamp("ms") - start_timestamp) / update_interval) > update_counter: COMMAND = eb_mavutil.Enum.get_int_reference( "MAV_CMD_NAV_WAYPOINT") eb_mavutil.Enum.get_method_reference( self._drone.mav(), "MISSION_ITEM")(self._drone._get_target_system(), self._drone._get_target_component(), 0, eb_mavutil.Enum.get_int_reference( "MAV_FRAME_GLOBAL_RELATIVE_ALT"), COMMAND, 2, 0, 0, 0, 0, 0, hold["current_pos"]["lat"], hold["current_pos"]["lon"], hold["current_pos"]["relative_alt"]) update_counter += 1 hold["ended"] = True Logger.PrintLog(self.LOG_INFO, "hold_global_position() - Handler has ended.")
def set_flight_mode(self, flight_mode, retries=4, timeout=1500): Logger.PrintLog( self.LOG_INFO, "set_flight_mode() - Setting flight mode to {}.".format( flight_mode.upper())) flight_mode = flight_mode.upper() flight_mode_list = eb_mavutil.get_flight_modes(self._drone.mavlink()) if flight_mode not in flight_mode_list: Logger.PrintLog( self.LOG_INFO, "set_flight_mode() - Flight mode {} not in flight mode list.". format(flight_mode)) return False def impl(): COMMAND = eb_mavutil.Enum.get_int_reference("MAV_CMD_DO_SET_MODE") self._drone.mav().command_long_send( self._drone._get_target_system(), 0, COMMAND, 0, # params eb_mavutil.Enum.get_int_reference( "MAV_MODE_FLAG_CUSTOM_MODE_ENABLED"), flight_mode_list[flight_mode], 0, 0, 0, 0, 0) res = self._drone.wait_cmd_ack(COMMAND, timeout) if res == 0: Logger.PrintLog( self.LOG_INFO, "set_flight_mode() - Flight mode successfully set to {}.". format(flight_mode)) return True else: Logger.PrintLog( self.LOG_INFO, "set_flight_mode() - An error occured while setting flight mode to {}. Result: {}." .format(flight_mode, str(res))) return False return Method.Repeat.until_value(impl, (), retries, ret_val=True)[0]
def impl(cls, hold): Logger.PrintLog( cls.LOG_INFO, "hold_local_position() - Handler has started. X:{}, Y:{}, Z:{}." .format(str(hold["current_pos"]["x"]), str(hold["current_pos"]["y"]), str(hold["current_pos"]["z"]))) start_timestamp = Time.get_current_timestamp("ms") update_counter = -1 while hold["hold"]: if hold.get("skip", False): continue if hold.get("pos_override", None) is not None: Logger.PrintLog( cls.LOG_INFO, "hold_local_position() - impl() - Position override detected. Current X: {}, Y:{}, Z:{}. Overridden X: {}, Y: {}, Z: {}." .format(str(hold["current_pos"]["x"]), str(hold["current_pos"]["y"]), str(hold["current_pos"]["z"]), str(hold["pos_override"]["x"]), str(hold["pos_override"]["y"]), str(hold["pos_override"]["z"]))) hold["current_pos"] = hold["pos_override"] hold["pos_override"] = None if floor((Time.get_current_timestamp("ms") - start_timestamp) / update_interval) > update_counter: COMMAND = eb_mavutil.Enum.get_int_reference( "MAV_CMD_NAV_WAYPOINT") eb_mavutil.Enum.get_method_reference( self._drone.mav(), "MISSION_ITEM")(self._drone._get_target_system(), self._drone._get_target_component(), 0, eb_mavutil.Enum.get_int_reference( "MAV_FRAME_LOCAL_NED"), COMMAND, 2, 0, 0, 0, 0, 0, hold["current_pos"]["x"], hold["current_pos"]["y"], hold["current_pos"]["z"]) update_counter += 1 hold["ended"] = True Logger.PrintLog(self.LOG_INFO, "hold_local_position() - Handler has ended.")
def abort(self): Logger.PrintLog(self.LOG_INFO, "abort() - Aborting the mission(s).") if self.get_status() != 1 \ and self.get_status() != 2: Logger.PrintLog( self.LOG_INFO, "abort() - Cannot abort the mission(s). Current status is {}.". format(str(self._status))) return False self.stop() self.clear_list() return True
def clear_list(self): Logger.PrintLog(self.LOG_INFO, "clear_list() - Clearing mission list.") if self.get_status() == 1: Logger.PrintLog( self.LOG_INFO, "clear_list() - Cannot clear mission list. Current status is {}." .format(str(self._status))) return False self._mission_list = [] self._current_mission_index = -1 self._current_running_script = "" return True
def impl(): COMMAND = eb_mavutil.Enum.get_int_reference("MAV_CMD_NAV_TAKEOFF") self._drone.mav().command_long_send( self._drone._get_target_system(), self._drone._get_target_component(), COMMAND, 0, # params 0, 0, 0, 0, 0, 0, rel_alt) res = self._drone.wait_cmd_ack(COMMAND, timeout) Logger.PrintLog(self.LOG_INFO, "takeoff() - Result: {}.".format(str(res))) if res == 0: return True else: return False
def impl(): COMMAND = eb_mavutil.Enum.get_int_reference( "MAV_CMD_DO_CHANGE_SPEED") self._drone.mav().command_long_send( self._drone._get_target_system(), self._drone._get_target_component(), COMMAND, 0, # params 3, spd, -1, 0, 0, 0, 0) res = self._drone.wait_cmd_ack(COMMAND, timeout) Logger.PrintLog( self.LOG_INFO, "set_descent_speed() - Result: {}.".format(str(res))) if res == 0: return True else: return False
def _execute_mission(self, mission, retries=5, timeout=1000): Logger.PrintLog( self.LOG_INFO, "_execute_mission() - Executing mission. Mission type: {}.".format( Mission.Type.get_str(mission.get("type")))) # https://mavlink.io/en/messages/common.html#MAV_CMD_NAV_TAKEOFF if mission.get("type") == Mission.Type.TAKEOFF: return self._drone.action().takeoff(mission.get("alt"), retries=retries, timeout=timeout) # https://mavlink.io/en/messages/common.html#MAV_CMD_NAV_LAND elif mission.get("type") == Mission.Type.LAND: return self._drone.action().land(retries=retries, timeout=timeout) # https://mavlink.io/en/messages/common.html#SET_POSITION_TARGET_GLOBAL_INT # https://mavlink.io/en/messages/common.html#MAV_CMD_NAV_WAYPOINT # https://github.com/ArduPilot/MAVProxy/blob/b2452edc85c6d8c83cc258e3e6219ac9b1268675/MAVProxy/modules/mavproxy_mode.py#L73 # https://github.com/dronekit/dronekit-python/blob/master/dronekit/__init__.py#L2187 elif mission.get("type") == Mission.Type.WAYPOINT: return self._drone.action().go_to_global_position( mission.get("lat"), mission.get("lon"), mission.get("alt")) elif mission.get("type") == Mission.Type.SPLINE_WAYPOINT: raise NotImplementedError # https://mavlink.io/en/messages/common.html#MAV_CMD_DO_SET_HOME elif mission.get("type") == Mission.Type.HOME_POINT: return self._drone.action().set_home_position(retries=retries, timeout=timeout) else: raise TypeError("Invalid mission type.")
def impl(): COMMAND = eb_mavutil.Enum.get_int_reference("MAV_CMD_DO_SET_RELAY") self._drone.mav().command_long_send( self._drone._get_target_system(), self._drone._get_target_component(), COMMAND, 0, # params channel, status, 0, 0, 0, 0, 0) res = self._drone.wait_cmd_ack(COMMAND, timeout) Logger.PrintLog(self.LOG_INFO, "set_relay() - Result: {}.".format(str(res))) if res == 0: return True else: return False
def set_servo_pwm(self, channel, pwm, retries=12, timeout=500): Logger.PrintLog( self.LOG_INFO, "set_servo_pwm() - Setting servo's pwm at channel {} to {}.". format(str(channel), str(pwm))) def impl(): COMMAND = eb_mavutil.Enum.get_int_reference("MAV_CMD_DO_SET_SERVO") self._drone.mav().command_long_send( self._drone._get_target_system(), self._drone._get_target_component(), COMMAND, 0, # params channel, pwm, 0, 0, 0, 0, 0) res = self._drone.wait_cmd_ack(COMMAND, timeout) Logger.PrintLog(self.LOG_INFO, "set_servo_pwm() - Result: {}.".format(str(res))) if res == 0: return True else: return False return Method.Repeat.until_value(impl, (), retries, ret_val=True)[0]
def arm(self, retries=4, timeout=1500): Logger.PrintLog(self.LOG_INFO, "arm() - Arming.") def impl(): COMMAND = eb_mavutil.Enum.get_int_reference( "MAV_CMD_COMPONENT_ARM_DISARM") self._drone.mav().command_long_send( self._drone._get_target_system(), self._drone._get_target_component(), COMMAND, 0, 1, 0, 0, 0, 0, 0, 0) res = self._drone.wait_cmd_ack(COMMAND, timeout) Logger.PrintLog(self.LOG_INFO, "arm() - Result: {}.".format(str(res))) if res == 0: return True else: return False if self._drone._first_arm: first_arm_res = Method.Repeat.until_value(impl, (), retries, ret_val=True)[0] if not first_arm_res: return first_arm_res else: self._drone._first_arm = False self.arm(retries + 2, timeout) return Method.Repeat.until_value(impl, (), retries, ret_val=True)[0]
def reset(self): Logger.PrintLog(self.LOG_INFO, "reset() - Resetting missions to their first states.") if self.get_status() == 1: Logger.PrintLog( self.LOG_INFO, "reset() - Cannot reset mission(s). Status must be 0 or 2.") return False for mission in self._mission_list: if mission.get("type") != Mission.Type.HOME_POINT: mission.set("status", 0) self._current_mission_index = -1 self._current_running_script = ""
def connect(self) -> None: LOG_INFO = "tcp_client.py - connect()" # if not callable(self._handler): # Logger.PrintLog(LOG_INFO, "self.handler is not callable.") # return if self._type == TCP_Client.Type.UNSPECIFIED: self.set_type(TCP_Client.Type.RECV_ONLY) self._socket.connect((self._ip, self._port)) self._status = 1 if self._type != TCP_Client.Type.RECV_ONLY \ and self._type != TCP_Client.Type.SEND_AND_RECV: Logger.PrintLog( LOG_INFO, "Client type is not eligible for receiving data. Handler thread is not going to started." ) return t = threading.Thread(target=self._thread_handler, args=(self, )) t.daemon = False t.start()
def is_reached_to_relative_alt(self, dest_rel_alt, threshold=0.5): curr_rel_alt = self._drone.telemetry().get_global_position( )["relative_alt"] Logger.PrintLog( self.LOG_INFO, "is_reached_to_relative_alt() - Current relative altitude: {}, Destination relative altitude: {}, Threshold: {}." .format(str(curr_rel_alt), str(dest_rel_alt), str(threshold))) if dest_rel_alt - threshold <= curr_rel_alt <= dest_rel_alt + threshold: Logger.PrintLog(self.LOG_INFO, "is_reached_to_relative_alt() - Returning True.") return True Logger.PrintLog(self.LOG_INFO, "is_reached_to_relative_alt() - Returning False.") return False
def add_land_mission(self, delay=0, script=""): Logger.PrintLog( self.LOG_INFO, "add_land_mission() - Adding land mission to local list.") self._mission_list.append( self.get_mission_item_proto(mission_type=Mission.Type.LAND, delay=delay, script=script))
def add_takeoff_mission(self, alt, delay=0, script=""): Logger.PrintLog( self.LOG_INFO, "add_takeoff_mission() - Adding takeoff mission to local list.") self._mission_list.append( self.get_mission_item_proto(mission_type=Mission.Type.TAKEOFF, alt=alt, delay=delay, script=script))
def is_reached_to_global_position(self, dest_lat, dest_lon, dest_rel_alt, threshold=0.5, acceptance_radius=1): if self.is_reached_to_relative_alt(dest_rel_alt, threshold): curr_global_pos = self._drone.telemetry().get_global_position() current_pos = { "lat": curr_global_pos["lat"], "lon": curr_global_pos["lon"] } dest_pos = {"lat": dest_lat, "lon": dest_lon} dist_to_dest = Math.calculate_global_position_distance( current_pos, dest_pos) Logger.PrintLog( self.LOG_INFO, "is_reached_to_global_position() - Current lat: {}, Current lon: {}, Current relative alt: {}, Destination lat: {}, Destination lon: {}, Destination relative altitude: {}, Threshold: {}." .format(str(current_pos["lat"]), str(current_pos["lon"]), str(curr_global_pos["relative_alt"]), str(dest_pos["lat"]), str(dest_pos["lon"]), str(dest_rel_alt), str(threshold))) if (acceptance_radius == 0 and dist_to_dest <= threshold) \ or dist_to_dest <= acceptance_radius: Logger.PrintLog( self.LOG_INFO, "is_reached_to_global_position() - Returning True.") return True else: Logger.PrintLog( self.LOG_INFO, "is_reached_to_global_position() - Distance to destination: {} m." .format(str(dist_to_dest))) Logger.PrintLog(self.LOG_INFO, "is_reached_to_global_position() - Returning False.") return False
def start(self) -> None: if self._serial is None: Logger.PrintLog(self.LOG_INFO, "start() - Cannot start. self._serial is None.") return self._serial.start() self._status = 1 t = threading.Thread(target=self._thread_handler, args=(self,)) t.daemon = False t.start()
def start(self) -> None: if not self._adc.get_is_started(): Logger.PrintLog( self.LOG_INFO, "start() - Cannot start. self._adc is not started.") return self._status = 1 t = threading.Thread(target=self._thread_handler, args=(self, )) t.daemon = False t.start()
def add_waypoint_mission(self, lat, lon, alt, delay=0, script=""): Logger.PrintLog( self.LOG_INFO, "add_waypoint_mission() - Adding waypoint mission to local list.") self._mission_list.append( self.get_mission_item_proto(mission_type=Mission.Type.WAYPOINT, lat=lat, lon=lon, alt=alt, delay=delay, script=script))
def exception_handler(self, file: str, ex: Exception) -> None: self._status = 0 file = file.split("\\")[-1].split("/")[-1] LOG_INFO = "tcp_client.py - exception_handler() - called by " + file ex_str = str(ex).lower() ex_type = type(ex) if ex_type == socket.timeout: Logger.PrintLog(LOG_INFO, "Connection with server has been timeouted.") elif ex_type == ConnectionResetError: Logger.PrintLog(LOG_INFO, "Connection with server has been reseted.") elif ex_type == ConnectionRefusedError: Logger.PrintLog(LOG_INFO, "Cannot connect to server. Connection refused.") sleep(config.Connection.CONNECT_DELAY) elif ex_type == ConnectionAbortedError: Logger.PrintLog(LOG_INFO, "Connection with server has been aborted.") elif ex_type == OSError \ and "winerror 10056" in ex_str: Logger.PrintLog(LOG_INFO, "Connection with server forced to be closed.") else: Logger.PrintException(LOG_INFO + " - UNKNOWN EXCEPTION", ex) self._socket.close() self._reset()
def set_home_position(self, retries=12, timeout=500): Logger.PrintLog( self.LOG_INFO, "set_home_position() - Attempting to set home position.") if self._drone.telemetry().get_flight_mode() != "GUIDED": Logger.PrintLog( self.LOG_INFO, "set_home_position() - Cannot set home position. Drone is not in GUIDED mode." ) return False def impl(): COMMAND = eb_mavutil.Enum.get_int_reference("MAV_CMD_DO_SET_HOME") self._drone.mav().command_long_send( self._drone._get_target_system(), self._drone._get_target_component(), COMMAND, 0, # params 1, 0, 0, 0, 0, 0, 0) res = self._drone.wait_cmd_ack(COMMAND, timeout) Logger.PrintLog( self.LOG_INFO, "set_home_position() - Result: {}.".format(str(res))) if res == 0: return True else: return False return Method.Repeat.until_value(impl, (), retries, ret_val=True)[0]
def set_descent_speed(self, spd, retries=4, timeout=1500): Logger.PrintLog( self.LOG_INFO, "set_descent_speed() - Setting descent speed to {} m/s.".format( str(spd))) if self._drone.telemetry().get_flight_mode() != "GUIDED": Logger.PrintLog( self.LOG_INFO, "set_descent_speed() - Cannot set descent speed. Drone is not in GUIDED mode." ) return False def impl(): COMMAND = eb_mavutil.Enum.get_int_reference( "MAV_CMD_DO_CHANGE_SPEED") self._drone.mav().command_long_send( self._drone._get_target_system(), self._drone._get_target_component(), COMMAND, 0, # params 3, spd, -1, 0, 0, 0, 0) res = self._drone.wait_cmd_ack(COMMAND, timeout) Logger.PrintLog( self.LOG_INFO, "set_descent_speed() - Result: {}.".format(str(res))) if res == 0: return True else: return False return Method.Repeat.until_value(impl, (), retries, ret_val=True)[0]
def takeoff(self, rel_alt, retries=12, timeout=500): Logger.PrintLog( self.LOG_INFO, "takeoff() - Attempting to taking off to altitude: {} m.".format( str(rel_alt))) if self._drone.telemetry().get_flight_mode() != "GUIDED": Logger.PrintLog( self.LOG_INFO, "takeoff() - Cannot takeoff. Drone is not in GUIDED mode.") return False def impl(): COMMAND = eb_mavutil.Enum.get_int_reference("MAV_CMD_NAV_TAKEOFF") self._drone.mav().command_long_send( self._drone._get_target_system(), self._drone._get_target_component(), COMMAND, 0, # params 0, 0, 0, 0, 0, 0, rel_alt) res = self._drone.wait_cmd_ack(COMMAND, timeout) Logger.PrintLog(self.LOG_INFO, "takeoff() - Result: {}.".format(str(res))) if res == 0: return True else: return False return Method.Repeat.until_value(impl, (), retries, ret_val=True)[0]