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 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 wait_until_value(dictionary: dict,
key: str,
value: any,
timeout_ms: int = 10000) -> None:
if timeout_ms < 0: raise ValueError("Timeout < 0.")
start_timestamp = Time.get_current_timestamp("ms")
while timeout_ms == 0 \
or Time.get_current_timestamp("ms") - start_timestamp <= timeout_ms:
if key not in dictionary: continue
if dictionary[key] == value: return
raise TimeoutError
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 _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 wait_mission_ack(self, timeout=5000):
if timeout <= 0:
raise ValueError("wait_mission_ack() - Timeout is <= 0.")
start_timestamp = Time.get_current_timestamp("ms")
while 1:
if "MISSION_ACK" not in self._messages:
if Time.get_current_timestamp("ms") - start_timestamp >= timeout:
raise TimeoutError("wait_mission_ack() timed out.")
continue
result = self._messages["MISSION_ACK"]["type"]
del self._messages["MISSION_ACK"]
return result
def _thread_handler(cls) -> None:
while cls._status:
try:
block_data = cls._read_block_data(0x00, 6)
if block_data[0] & 0x08:
pressure_raw = ((block_data[1] << 16
| block_data[2] << 8
| block_data[3]) >> 4) / 16.0
temp_raw = ((block_data[4] << 8
| block_data[5]) >> 4) / 16.0
cls._last_data["temperature"] = temp_raw
if cls._next_measurement_type == 1:
cls._last_data["altitude"] = pressure_raw
cls._next_measurement_type = 2
cls._write_byte(MPL3115A2.REGISTER.CONTROL_1, 0x39)
elif cls._next_measurement_type == 2:
cls._last_data["pressure"] = pressure_raw * 16 / 4.0 / 1000.0
cls._next_measurement_type = 1
cls._write_byte(MPL3115A2.REGISTER.CONTROL_1, 0xB9)
sleep(1)
cls._last_data["timestamp"] = Time.get_current_timestamp("ms")
except TypeError: pass
sleep(1 / cls._data_rate)
def get_attitude(self):
try:
self._drone._attitude["last_update"] = Time.get_current_timestamp(
"ms") - self._drone._attitude["timestamp"]
except KeyError:
pass
return self._drone._attitude
def wait_cmd_ack(self, cmd, timeout=5000):
if timeout <= 0:
raise ValueError("wait_cmd_ack() - Timeout is <= 0.")
start_timestamp = Time.get_current_timestamp("ms")
while 1:
if cmd not in self._messages["COMMAND_ACK"]:
if Time.get_current_timestamp("ms") - start_timestamp >= timeout:
raise TimeoutError("wait_cmd_ack() for command {} has timed out."
.format(str(cmd)))
continue
result = self._messages["COMMAND_ACK"][cmd]["result"]
del self._messages["COMMAND_ACK"][cmd]
return result
def get_last_data(self) -> Optional[dict]:
if self._last_data == {}:
return {
"voltage": None,
"timestamp": Time.get_current_timestamp("ms")
}
return self._last_data
def get_global_position(self):
try:
self._drone._global_position[
"last_update"] = Time.get_current_timestamp(
"ms") - self._drone._global_position["timestamp"]
except KeyError:
pass
return self._drone._global_position
def get_last_data(self) -> Optional[dict]:
if self._last_data == {}:
return {
"timestamp" : Time.get_current_timestamp("ms"),
"distance" : None,
"signal_strength" : None,
"mode" : None
}
return self._last_data
def get_last_data(self) -> Optional[dict]:
if self._last_data == {}:
return {
"pressure" : None,
"altitude" : None,
"temperature": None,
"timestamp" : Time.get_current_timestamp("ms")
}
return self._last_data
def until_value(func: Callable,
args: tuple,
timeout: int = 0,
repeat_delay_ms: Optional[int] = None,
ret_val: any = True) -> None:
if repeat_delay_ms is not None \
and repeat_delay_ms <= 0:
raise ValueError("delay <= 0")
start_time = Time.get_current_timestamp("ms")
while timeout == 0 \
or Time.get_current_timestamp("ms") - start_time < timeout:
if func(*args) == ret_val: return
if repeat_delay_ms is not None:
sleep(repeat_delay_ms / 1000)
raise TimeoutError
def wait_msg(self, msg, timeout=5000):
if timeout <= 0:
raise ValueError("wait_msg() - Timeout is <= 0.")
elif msg == "COMMAND_ACK":
raise NotImplementedError
start_timestamp = Time.get_current_timestamp("ms")
while 1:
if msg not in self._messages:
if Time.get_current_timestamp("ms") - start_timestamp >= timeout:
raise TimeoutError("wait_msg() for message {} has timed out."
.format(str(msg)))
continue
msg_data = self._messages[msg]
del self._messages[msg]
return msg_data
def get_last_data(self) -> Optional[dict]:
if self._last_data == {}:
return {
"timestamp": Time.get_current_timestamp("ms"),
"latitude": None,
"longitude": None,
"fix": None,
"sat_count": None,
"altitude": None
}
return self._last_data
def _thread_handler(cls) -> None:
while cls._status:
voltage = cls._adc.get_last_data()
if voltage is None: continue
voltage = voltage[cls._channel]["voltage"] / (10**3) # V
voltage = voltage * 5. / 1023. # max471 conversion
cls._last_data = {
"voltage": voltage,
"timestamp": Time.get_current_timestamp("ms")
}
def _thread_handler(cls) -> None:
while cls._status:
for channel in cls._channels:
if channel > 3: continue
try:
mV = cls._adc.read_adc(channel, cls._gain)
except OSError:
continue
if "channel" not in cls._last_data:
cls._last_data["channel"] = {}
cls._last_data[channel] = {
"voltage": mV,
"timestamp": Time.get_current_timestamp("ms")
}
sleep(1 / cls._data_rate)
def _thread_handler(cls) -> None:
while cls._status:
if cls._serial.in_waiting() >= 9:
recv_data = cls._serial.read(9)
if recv_data[:2] == b"\x59\x59":
distance = (recv_data[3] << 8) | recv_data[2]
signal_strength = (recv_data[5] << 8) | recv_data[4]
mode = recv_data[6]
checksum = recv_data[8]
cls._last_data = {
"timestamp" : Time.get_current_timestamp("ms"),
"distance" : distance,
"signal_strength" : signal_strength,
"mode" : mode
}
cls._serial.flush_input_buffer()
cls._serial.stop()
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 get_last_data(self) -> Optional[dict]:
if self._last_data == {}:
return {
"accel": {
"raw": {
"x": None,
"y": None,
"z": None
},
"scaled": {
"x": None,
"y": None,
"z": None
}
},
"gyro": {
"raw": {
"x": None,
"y": None,
"z": None
},
"scaled": {
"x": None,
"y": None,
"z": None
}
},
"mag": {
"x": None,
"y": None,
"z": None
},
"timestamp": Time.get_current_timestamp("ms")
}
return self._last_data
def get_battery(self):
self._drone._battery["last_update"] = Time.get_current_timestamp(
"ms") - self._drone._battery["timestamp"]
return self._drone._battery
def get_throttle(self):
return self._drone._throttle[0], Time.get_current_timestamp(
"ms") - self._drone._throttle[1]
def get_last_heartbeat(self, ret_diff=False):
if ret_diff:
return Time.get_current_timestamp(
"ms") - self._drone._last_heartbeat
return self._drone._last_heartbeat
def get_boot_time(self):
return self._drone._boot_time[0], Time.get_current_timestamp(
"ms") - self._drone._boot_time[1]
def get_ground_speed(self):
return self._drone._ground_speed[0], Time.get_current_timestamp(
"ms") - self._drone._ground_speed[1]
def get_altitude(self):
return self._drone._alt[0], Time.get_current_timestamp(
"ms") - self._drone._alt[1]
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.")
def get_climb_rate(self):
return self._drone._climb_rate[0], Time.get_current_timestamp(
"ms") - self._drone._climb_rate[1]
def get_heading(self):
return self._drone._heading[0], Time.get_current_timestamp(
"ms") - self._drone._heading[1]