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]