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 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 __init__(self, port_name: str, baudrate: int = 9600) -> None:
self.LOG_INFO = "gy_neo6mv2.py"
self._last_data = {}
self._serial = None
self._port_name = port_name
self._baudrate = baudrate
self._status = 0
try:
self._serial = SerialPort(self._port_name, self._baudrate)
except Exception as ex:
Logger.PrintException(self.LOG_INFO + " - __init__()", ex)
return
def __init__(self,
port_name: str = "/dev/ttyUSB0",
baudrate : int = 115200) -> None:
self.LOG_INFO = "tf_mini_lidar.py"
self._last_data = {}
self._port_name = port_name
self._baudrate = baudrate
self._status = 0
try:
self._serial = SerialPort(self._port_name, self._baudrate)
except Exception as ex:
Logger.PrintException(self.LOG_INFO + " - __init__()", ex)
return
def __init__(self,
base_addr: int = 0x68,
mag_addr: int = 0x0C,
data_rate: int = 10) -> None:
self.LOG_INFO = "mpu9255.py"
self._bus = None
self._addr = base_addr
self._mag_addr = mag_addr
self._data_rate = data_rate
self._last_data = {}
self._status = 0
try:
self._bus = SMBus(1)
except Exception as ex:
Logger.PrintException(self.LOG_INFO + " - __init__()", ex)
return
def __init__(self,
base_addr: int = 0x48,
channels: Optional[List[int]] = None,
gain: int = 1,
data_rate: int = 10) -> None:
self.LOG_INFO = "ads1115.py"
if channels is None: channels = []
self._adc = None
self._addr = base_addr
self._channels = channels
self._gain = gain
self._data_rate = data_rate
self._last_data = {}
self._status = 0
try:
self._adc = Adafruit_ADS1x15.ADS1115(self._addr)
except Exception as ex:
Logger.PrintException(self.LOG_INFO + " - __init__()", ex)
return
def _thread_handler(cls) -> None:
LOG_INFO = cls.LOG_INFO + "_thread_handler() - "
while cls._status:
if cls._camera.isOpened():
try:
_, frame = cls._camera.read()
if cls._writer is not None:
cls._writer.write(frame)
cls._last_frame = frame
except Exception as ex:
Logger.PrintException(LOG_INFO, ex)
sleep(0.1)
else:
Logger.PrintLog(LOG_INFO, "Camera is not open.")
sleep(1)
cls._camera.release()
if cls._writer is not None:
cls._writer.release()
def __init__(self,
base_addr : int = 0x68,
data_rate : int = 10) -> None:
self.LOG_INFO = "mpl3115a2.py"
self._addr = base_addr
self._bus = None
self._sensor = None
self._data_rate = data_rate
self._last_data = {
"pressure" : None, # pascal (kPa)
"altitude" : None, # meters
"temperature": None, # degree C,
"timestamp" : Time.get_current_timestamp("ms")
}
self._status = 0
self._next_measurement_type = 0 # 1- Altimeter, 2- Barometer
try:
self._bus = SMBus(1)
except Exception as ex:
Logger.PrintException(self.LOG_INFO + " - __init__()", ex)
return
def _thread_handler(cls):
LOG_INFO = cls.LOG_INFO + " - _thread_handler()"
Logger.PrintLog(LOG_INFO, "Mission tracker thread has started.")
emergency_mode = "LAND"
set_to_emergency_mode = False
while cls.get_status() == 1:
try:
is_completed = False
current_mission_index = cls.get_current_mission_index()
current_mission = cls._mission_list[current_mission_index]
current_flight_mode = cls._drone.telemetry().get_flight_mode()
if current_flight_mode != "GUIDED" \
and (current_flight_mode != "LAND" and current_mission.get("type") != Mission.Type.LAND):
Logger.PrintLog(
LOG_INFO,
"Flight mode has changed from GUIDED mode. Stopping. Current flight mode: {}."
.format(current_flight_mode))
break
if current_mission.get("status") == 0:
global_pos = cls._drone.telemetry().get_global_position()
current_mission.set_storage_variable(
"rel_alt", global_pos["relative_alt"])
current_mission.set_storage_variable(
"start_ms", Time.get_current_timestamp("ms"))
current_mission.set_storage_variable("retries", 0)
res = cls._execute_mission(current_mission,
cls._mission_retries,
cls._mission_retry_timeout)
if not res:
Logger.PrintLog(
LOG_INFO,
"_execute_mission() has failed. Stopping. Current mission index: {}."
.format(str(current_mission_index)))
set_to_emergency_mode = True
break
Logger.PrintLog(
LOG_INFO,
"Tracking the mission at index {}, type {}.".format(
str(current_mission_index),
Mission.Type.get_str(current_mission.get("type"))))
cls._mission_list[current_mission_index].set("status", 1)
# .\mission command execution
if current_mission.get("type") == Mission.Type.TAKEOFF:
Logger.PrintLog(LOG_INFO,
"Tracking TAKEOFF mission status.")
desired_alt = current_mission.get("alt")
threshold = 0.5
if cls._drone.control().is_reached_to_relative_alt(
desired_alt, threshold):
is_completed = True
elif current_mission.get("type") == Mission.Type.LAND:
Logger.PrintLog(LOG_INFO, "Tracking LAND mission status.")
threshold = 0.5
if cls._drone.control().is_reached_to_relative_alt(
0, threshold):
is_completed = True
elif current_mission.get("type") == Mission.Type.WAYPOINT:
Logger.PrintLog(LOG_INFO,
"Tracking WAYPOINT mission status.")
dest_pos = {
"lat": current_mission.get("lat"),
"lon": current_mission.get("lon")
}
desired_alt = current_mission.get("alt")
threshold = 0.15
# /!\ Toggle below line if odd behaviours observed on drone.
cls._execute_mission(current_mission, cls._mission_retries,
cls._mission_retry_timeout)
if cls._drone.control().is_reached_to_global_position(
dest_pos["lat"], dest_pos["lon"], desired_alt,
threshold, cls._wp_radius):
is_completed = True
elif current_mission.get("type") == Mission.Type.HOME_POINT:
is_completed = True
# .\mission controls
if not is_completed:
# not used currently
current_mission.set_storage_variable(
"retries",
current_mission.get_storage_variable("retries") + 1)
else:
Logger.PrintLog(
LOG_INFO,
"Mission at index {}, type {} has completed.".format(
str(cls.get_current_mission_index()),
Mission.Type.get_str(current_mission.get("type"))))
script = current_mission.get("script")
if script != "":
script = script + ".py"
script_path = cls._scripts_dir + script
if os.path.isfile(script_path):
with open(script_path, "r") as f:
Logger.PrintLog(
LOG_INFO,
"Executing script {}.".format(script))
cls._current_running_script = script
exec_return = {}
hold_pos = {"hold": True}
cls._drone.action().hold_global_position(
hold_pos, cls._pos_update_interval)
try:
exec(
compile(f.read(), script, "exec"), {
"drone": cls._drone,
"control": {
"position": {
"hold": hold_pos
}
}
}, exec_return)
except Exception as ex:
Logger.PrintLog(
LOG_INFO,
"An exception occured while running {}. Exception: {} - {}. Proceeding to next mission if there is."
.format(script,
type(ex).__name__, str(ex)))
Logger.PrintLog(
LOG_INFO,
"Script executed. Returned data: {}".
format(
str(exec_return.get("return_val",
{}))))
hold_pos["hold"] = False
Method.Dict.wait_until_value(
hold_pos, "ended", True)
else:
Logger.PrintLog(
LOG_INFO,
"Cannot find script {} in scripts path ({}). Skipping."
.format(script, cls._scripts_dir))
cls._current_running_script = ""
cls._mission_list[current_mission_index].set("status", 2)
if current_mission_index != cls.get_mission_count() - 1:
cls._current_mission_index += 1
else:
Logger.PrintLog(
LOG_INFO,
"All missions are successfully finished.")
if cls._mission_list[-1].get(
"type") != Mission.Type.LAND:
set_to_emergency_mode = True
break
if current_mission.get("delay") != 0:
Logger.PrintLog(
LOG_INFO,
"Delaying the next mission for {} seconds.".format(
current_mission.get("delay")))
hold_pos = {"hold": True}
cls._drone.action().hold_global_position(
hold_pos, cls._pos_update_interval)
delay_timestamp = Time.get_current_timestamp("ms")
while 1:
time_diff = Time.get_current_timestamp(
"ms") - delay_timestamp
if time_diff >= current_mission.get(
"delay") * 1000:
break
Logger.PrintLog(LOG_INFO, "Delay has ended.")
hold_pos["hold"] = False
Method.Dict.wait_until_value(hold_pos, "ended", True)
continue
# .\is mission completed check
sleep(1 / cls._control_rate)
except Exception as ex:
Logger.PrintException(LOG_INFO, ex)
set_to_emergency_mode = True
break
# .\while
if cls.get_status() == 2:
Logger.PrintLog(
LOG_INFO, "Mission at index {} is paused.".format(
str(cls.get_current_mission_index())))
cls._mission_list[cls.get_current_mission_index()].set("status", 0)
cls._mission_list[cls.get_current_mission_index()].set(
"is_uploaded", 0)
if cls.get_status() != 2:
cls._status = 0
if set_to_emergency_mode:
Logger.PrintLog(
LOG_INFO,
"!!! EMERGENCY SITUATION DETECTED !!! Setting flight mode to {}."
.format(emergency_mode))
cls._drone.action().set_flight_mode(emergency_mode)
Logger.PrintLog(LOG_INFO, "Mission tracker thread has ended.")
