def __init__(self, espdrone=None):
"""
Initialize the Extpos object.
"""
self._ed = espdrone
self.receivedLocationPacket = Caller()
self._ed.add_port_callback(CRTPPort.LOCALIZATION, self._incoming)
def __init__(self, espdrone):
"""
Initialize the console and register it to receive data from the copter.
"""
self.receivedChar = Caller()
self.ed = espdrone
self.ed.add_port_callback(CRTPPort.CONSOLE, self.incoming)
def __init__(self, espdrone = None):
"""
Initialize the camera object.
"""
self._ed = espdrone
self.image_received_cb = Caller()
self._stream = False
self._image = None
self._fps = None
class Camera():
"""
Handle camera-related data communication with the Espdrone
"""
CAMERA_BUFFER_SIZE = 4096
def __init__(self, espdrone = None):
"""
Initialize the camera object.
"""
self._ed = espdrone
self.image_received_cb = Caller()
self._stream = False
self._image = None
self._fps = None
def _capture_frames(self):
self._cap = cv2.VideoCapture(self._url)
while self._stream and self._ed.link:
start_time = time.time()
ret, self._image = self._cap.read()
self._fps = 1 / (time.time() - start_time)
if self._stream and ret:
self.image_received_cb.call(self._image, self._fps)
self._cap.release()
self._stream = False
def _is_streaming(self):
return (hasattr(self, '_thread') and self._thread.isAlive())
def start(self):
try:
self._url = 'http://' + self._ed.link_uri + "/stream.jpg"
if not self._is_streaming():
self._stream = True
self._thread = threading.Thread(target=self._capture_frames, daemon=True)
self._thread.start()
except Exception as e:
print(e)
def stop(self):
if hasattr(self, '_thread'):
self._stream = False
self._thread.join()
@property
def image(self):
if self._is_streaming():
return self._image
return None
@property
def fps(self):
if self._is_streaming():
return self._fps
return -1
def setUp(self):
self.ed_mock = MagicMock(spec=Espdrone)
self.ed_mock.disconnected = Caller()
self.log_mock = MagicMock(spec=Log)
self.ed_mock.log = self.log_mock
self.log_config_mock = MagicMock(spec=LogConfig)
self.log_config_mock.data_received_cb = Caller()
self.sut = SyncLogger(self.ed_mock, self.log_config_mock)
def setUp(self):
self.uri = 'radio://0/60/2M'
self.ed_mock = MagicMock(spec=Espdrone)
self.ed_mock.connected = Caller()
self.ed_mock.connection_failed = Caller()
self.ed_mock.disconnected = Caller()
self.ed_mock.open_link = AsyncCallbackCaller(cb=self.ed_mock.connected,
args=[self.uri]).trigger
self.sut = SyncEspdrone(self.uri, self.ed_mock)
def add_update_callback(self, group=None, name=None, cb=None):
"""
Add a callback for a specific parameter name. This callback will be
executed when a new value is read from the Espdrone.
"""
if not group and not name:
self.all_update_callback.add_callback(cb)
elif not name:
if group not in self.group_update_callbacks:
self.group_update_callbacks[group] = Caller()
self.group_update_callbacks[group].add_callback(cb)
else:
paramname = '{}.{}'.format(group, name)
if paramname not in self.param_update_callbacks:
self.param_update_callbacks[paramname] = Caller()
self.param_update_callbacks[paramname].add_callback(cb)
def __init__(self, espdrone=None):
self.log_blocks = []
# Called with newly created blocks
self.block_added_cb = Caller()
self.ed = espdrone
self.toc = None
self.ed.add_port_callback(CRTPPort.LOGGING, self._new_packet_cb)
self.toc_updated = Caller()
self.state = IDLE
self.fake_toc_crc = 0xDEADBEEF
self._refresh_callback = None
self._toc_cache = None
self._config_id_counter = 1
self._useV2 = False
def __init__(self, espdrone):
self.toc = Toc()
self.ed = espdrone
self._useV2 = False
self.param_update_callbacks = {}
self.group_update_callbacks = {}
self.all_update_callback = Caller()
self.param_updater = None
self.param_updater = _ParamUpdater(self.ed, self._useV2,
self._param_updated)
self.param_updater.start()
self.ed.disconnected.add_callback(self._disconnected)
self.all_updated = Caller()
self.is_updated = False
self.values = {}
class Console:
"""
Espdrone console is used to receive characters printed using printf
from the firmware.
"""
def __init__(self, espdrone):
"""
Initialize the console and register it to receive data from the copter.
"""
self.receivedChar = Caller()
self.ed = espdrone
self.ed.add_port_callback(CRTPPort.CONSOLE, self.incoming)
def incoming(self, packet):
"""
Callback for data received from the copter.
"""
# This might be done prettier ;-)
console_text = packet.data.decode('UTF-8')
self.receivedChar.call(console_text)
class PeriodicTimer:
"""Create a periodic timer that will periodically call a callback"""
def __init__(self, period, callback):
self._callbacks = Caller()
self._callbacks.add_callback(callback)
self._started = False
self._period = period
self._thread = None
def start(self):
"""Start the timer"""
if self._thread:
logger.warning("Timer already started, not restarting")
return
self._thread = _PeriodicTimerThread(self._period, self._callbacks)
self._thread.setDaemon(True)
self._thread.start()
def stop(self):
"""Stop the timer"""
if self._thread:
self._thread.stop()
self._thread = None
def __init__(self, espdrone=None):
"""Instantiate class and connect callbacks"""
# Called when new memories have been added
self.mem_added_cb = Caller()
# Called when new data has been read
self.mem_read_cb = Caller()
self.mem_write_cb = Caller()
self.ed = espdrone
self.ed.add_port_callback(CRTPPort.MEM, self._new_packet_cb)
self.ed.disconnected.add_callback(self._disconnected)
self._write_requests_lock = Lock()
self._clear_state()
def __init__(self, name, period_in_ms):
"""Initialize the entry"""
self.data_received_cb = Caller()
self.error_cb = Caller()
self.started_cb = Caller()
self.added_cb = Caller()
self.err_no = 0
# These 3 variables are set by the log subsystem when the bock is added
self.id = 0
self.ed = None
self.useV2 = False
self.period = int(period_in_ms / 10)
self.period_in_ms = period_in_ms
self._added = False
self._started = False
self.valid = False
self.variables = []
self.default_fetch_as = []
self.name = name
def setUp(self):
self.ed_mock = MagicMock(spec=Espdrone)
self.log_config_mock = MagicMock(spec=LogConfig)
self.log_config_mock.data_received_cb = Caller()
self.log_mock = MagicMock(spec=Log)
self.ed_mock.log = self.log_mock
self.front_data = 2345
self.back_data = 2345
self.left_data = 123
self.right_data = 5432
self.up_data = 3456
self.down_data = 1212
self.log_data = {
self.FRONT: self.front_data,
self.BACK: self.back_data,
self.LEFT: self.left_data,
self.RIGHT: self.right_data,
self.UP: self.up_data,
self.DOWN: self.down_data,
}
class LogConfig(object):
"""Representation of one log configuration that enables logging
from the Espdrone"""
def __init__(self, name, period_in_ms):
"""Initialize the entry"""
self.data_received_cb = Caller()
self.error_cb = Caller()
self.started_cb = Caller()
self.added_cb = Caller()
self.err_no = 0
# These 3 variables are set by the log subsystem when the bock is added
self.id = 0
self.ed = None
self.useV2 = False
self.period = int(period_in_ms / 10)
self.period_in_ms = period_in_ms
self._added = False
self._started = False
self.valid = False
self.variables = []
self.default_fetch_as = []
self.name = name
def add_variable(self, name, fetch_as=None):
"""Add a new variable to the configuration.
name - Complete name of the variable in the form group.name
fetch_as - String representation of the type the variable should be
fetched as (i.e uint8_t, float, FP16, etc)
If no fetch_as type is supplied, then the stored as type will be used
(i.e the type of the fetched variable is the same as it's stored in the
Espdrone)."""
if fetch_as:
self.variables.append(LogVariable(name, fetch_as))
else:
# We cannot determine the default type until we have connected. So
# save the name and we will add these once we are connected.
self.default_fetch_as.append(name)
def add_memory(self, name, fetch_as, stored_as, address):
"""Add a raw memory position to log.
name - Arbitrary name of the variable
fetch_as - String representation of the type of the data the memory
should be fetch as (i.e uint8_t, float, FP16)
stored_as - String representation of the type the data is stored as
in the Espdrone
address - The address of the data
"""
self.variables.append(
LogVariable(name, fetch_as, LogVariable.MEM_TYPE, stored_as,
address))
def _set_added(self, added):
if added != self._added:
self.added_cb.call(self, added)
self._added = added
def _get_added(self):
return self._added
def _set_started(self, started):
if started != self._started:
self.started_cb.call(self, started)
self._started = started
def _get_started(self):
return self._started
added = property(_get_added, _set_added)
started = property(_get_started, _set_started)
def create(self):
"""Save the log configuration in the Espdrone"""
pk = CRTPPacket()
pk.set_header(5, CHAN_SETTINGS)
if self.useV2:
pk.data = (CMD_CREATE_BLOCK_V2, self.id)
else:
pk.data = (CMD_CREATE_BLOCK, self.id)
for var in self.variables:
if (var.is_toc_variable() is False): # Memory location
logger.debug('Logging to raw memory %d, 0x%04X',
var.get_storage_and_fetch_byte(), var.address)
pk.data.append(
struct.pack('<B', var.get_storage_and_fetch_byte()))
pk.data.append(struct.pack('<I', var.address))
else: # Item in TOC
logger.debug('Adding %s with id=%d and type=0x%02X', var.name,
self.ed.log.toc.get_element_id(var.name),
var.get_storage_and_fetch_byte())
pk.data.append(var.get_storage_and_fetch_byte())
if self.useV2:
ident = self.ed.log.toc.get_element_id(var.name)
pk.data.append(ident & 0x0ff)
pk.data.append((ident >> 8) & 0x0ff)
else:
pk.data.append(self.ed.log.toc.get_element_id(var.name))
logger.debug('Adding log block id {}'.format(self.id))
if self.useV2:
self.ed.send_packet(pk,
expected_reply=(CMD_CREATE_BLOCK_V2, self.id))
else:
self.ed.send_packet(pk, expected_reply=(CMD_CREATE_BLOCK, self.id))
def start(self):
"""Start the logging for this entry"""
if (self.ed.link is not None):
if (self._added is False):
self.create()
logger.debug('First time block is started, add block')
else:
logger.debug(
'Block already registered, starting logging'
' for id=%d', self.id)
pk = CRTPPacket()
pk.set_header(5, CHAN_SETTINGS)
pk.data = (CMD_START_LOGGING, self.id, self.period)
self.ed.send_packet(pk,
expected_reply=(CMD_START_LOGGING,
self.id))
def stop(self):
"""Stop the logging for this entry"""
if (self.ed.link is not None):
if (self.id is None):
logger.warning('Stopping block, but no block registered')
else:
logger.debug('Sending stop logging for block id=%d', self.id)
pk = CRTPPacket()
pk.set_header(5, CHAN_SETTINGS)
pk.data = (CMD_STOP_LOGGING, self.id)
self.ed.send_packet(pk,
expected_reply=(CMD_STOP_LOGGING, self.id))
def delete(self):
"""Delete this entry in the Espdrone"""
if (self.ed.link is not None):
if (self.id is None):
logger.warning('Delete block, but no block registered')
else:
logger.debug(
'LogEntry: Sending delete logging for block id=%d' %
self.id)
pk = CRTPPacket()
pk.set_header(5, CHAN_SETTINGS)
pk.data = (CMD_DELETE_BLOCK, self.id)
self.ed.send_packet(pk,
expected_reply=(CMD_DELETE_BLOCK, self.id))
def unpack_log_data(self, log_data, timestamp):
"""Unpack received logging data so it repreloggingsent real values according
to the configuration in the entry"""
ret_data = {}
data_index = 0
for var in self.variables:
size = LogTocElement.get_size_from_id(var.fetch_as)
name = var.name
unpackstring = LogTocElement.get_unpack_string_from_id(
var.fetch_as)
value = struct.unpack(unpackstring,
log_data[data_index:data_index + size])[0]
data_index += size
ret_data[name] = value
self.data_received_cb.call(timestamp, ret_data, self)
def __init__(self, do_device_discovery=True):
self._input_device = None
self._mux = [
NoMux(self),
TakeOverSelectiveMux(self),
TakeOverMux(self)
]
# Set NoMux as default
self._selected_mux = self._mux[0]
self.min_thrust = 0
self.max_thrust = 0
self._thrust_slew_rate = 0
self.thrust_slew_enabled = False
self.thrust_slew_limit = 0
self.has_pressure_sensor = False
self._hover_max_height = MAX_TARGET_HEIGHT
self.max_rp_angle = 0
self.max_yaw_rate = 0
try:
self.set_assisted_control(Config().get("assistedControl"))
except KeyError:
self.set_assisted_control(JoystickReader.ASSISTED_CONTROL_ALTHOLD)
self._old_thrust = 0
self._old_raw_thrust = 0
self.springy_throttle = True
self._target_height = INITAL_TAGET_HEIGHT
self.trim_roll = Config().get("trim_roll")
self.trim_pitch = Config().get("trim_pitch")
self._rp_dead_band = 0.1
self._input_map = None
if Config().get("flightmode") is "Normal":
self.max_yaw_rate = Config().get("normal_max_yaw")
self.max_rp_angle = Config().get("normal_max_rp")
# Values are stored at %, so use the functions to set the values
self.min_thrust = Config().get("normal_min_thrust")
self.max_thrust = Config().get("normal_max_thrust")
self.thrust_slew_limit = Config().get("normal_slew_limit")
self.thrust_slew_rate = Config().get("normal_slew_rate")
else:
self.max_yaw_rate = Config().get("max_yaw")
self.max_rp_angle = Config().get("max_rp")
# Values are stored at %, so use the functions to set the values
self.min_thrust = Config().get("min_thrust")
self.max_thrust = Config().get("max_thrust")
self.thrust_slew_limit = Config().get("slew_limit")
self.thrust_slew_rate = Config().get("slew_rate")
self._dev_blacklist = None
if len(Config().get("input_device_blacklist")) > 0:
self._dev_blacklist = re.compile(
Config().get("input_device_blacklist"))
logger.info("Using device blacklist [{}]".format(
Config().get("input_device_blacklist")))
self._available_devices = {}
# TODO: The polling interval should be set from config file
self._read_timer = PeriodicTimer(INPUT_READ_PERIOD, self.read_input)
if do_device_discovery:
self._discovery_timer = PeriodicTimer(1.0,
self._do_device_discovery)
self._discovery_timer.start()
# Check if user config exists, otherwise copy files
if not os.path.exists(ConfigManager().configs_dir):
logger.info("No user config found, copying dist files")
os.makedirs(ConfigManager().configs_dir)
for f in glob.glob(edclient.module_path +
"/configs/input/[A-Za-z]*.json"):
dest = os.path.join(ConfigManager().configs_dir,
os.path.basename(f))
if not os.path.isfile(dest):
logger.debug("Copying %s", f)
shutil.copy2(f, ConfigManager().configs_dir)
ConfigManager().get_list_of_configs()
self.input_updated = Caller()
self.assisted_input_updated = Caller()
self.heighthold_input_updated = Caller()
self.hover_input_updated = Caller()
self.rp_trim_updated = Caller()
self.emergency_stop_updated = Caller()
self.device_discovery = Caller()
self.device_error = Caller()
self.assisted_control_updated = Caller()
self.alt1_updated = Caller()
self.alt2_updated = Caller()
# Call with 3 bools (rp_limiting, yaw_limiting, thrust_limiting)
self.limiting_updated = Caller()
class JoystickReader(object):
"""
Thread that will read input from devices/joysticks and send control-set
points to the Espdrone
"""
inputConfig = []
ASSISTED_CONTROL_ALTHOLD = 0
ASSISTED_CONTROL_POSHOLD = 1
ASSISTED_CONTROL_HEIGHTHOLD = 2
ASSISTED_CONTROL_HOVER = 3
def __init__(self, do_device_discovery=True):
self._input_device = None
self._mux = [
NoMux(self),
TakeOverSelectiveMux(self),
TakeOverMux(self)
]
# Set NoMux as default
self._selected_mux = self._mux[0]
self.min_thrust = 0
self.max_thrust = 0
self._thrust_slew_rate = 0
self.thrust_slew_enabled = False
self.thrust_slew_limit = 0
self.has_pressure_sensor = False
self._hover_max_height = MAX_TARGET_HEIGHT
self.max_rp_angle = 0
self.max_yaw_rate = 0
try:
self.set_assisted_control(Config().get("assistedControl"))
except KeyError:
self.set_assisted_control(JoystickReader.ASSISTED_CONTROL_ALTHOLD)
self._old_thrust = 0
self._old_raw_thrust = 0
self.springy_throttle = True
self._target_height = INITAL_TAGET_HEIGHT
self.trim_roll = Config().get("trim_roll")
self.trim_pitch = Config().get("trim_pitch")
self._rp_dead_band = 0.1
self._input_map = None
if Config().get("flightmode") is "Normal":
self.max_yaw_rate = Config().get("normal_max_yaw")
self.max_rp_angle = Config().get("normal_max_rp")
# Values are stored at %, so use the functions to set the values
self.min_thrust = Config().get("normal_min_thrust")
self.max_thrust = Config().get("normal_max_thrust")
self.thrust_slew_limit = Config().get("normal_slew_limit")
self.thrust_slew_rate = Config().get("normal_slew_rate")
else:
self.max_yaw_rate = Config().get("max_yaw")
self.max_rp_angle = Config().get("max_rp")
# Values are stored at %, so use the functions to set the values
self.min_thrust = Config().get("min_thrust")
self.max_thrust = Config().get("max_thrust")
self.thrust_slew_limit = Config().get("slew_limit")
self.thrust_slew_rate = Config().get("slew_rate")
self._dev_blacklist = None
if len(Config().get("input_device_blacklist")) > 0:
self._dev_blacklist = re.compile(
Config().get("input_device_blacklist"))
logger.info("Using device blacklist [{}]".format(
Config().get("input_device_blacklist")))
self._available_devices = {}
# TODO: The polling interval should be set from config file
self._read_timer = PeriodicTimer(INPUT_READ_PERIOD, self.read_input)
if do_device_discovery:
self._discovery_timer = PeriodicTimer(1.0,
self._do_device_discovery)
self._discovery_timer.start()
# Check if user config exists, otherwise copy files
if not os.path.exists(ConfigManager().configs_dir):
logger.info("No user config found, copying dist files")
os.makedirs(ConfigManager().configs_dir)
for f in glob.glob(edclient.module_path +
"/configs/input/[A-Za-z]*.json"):
dest = os.path.join(ConfigManager().configs_dir,
os.path.basename(f))
if not os.path.isfile(dest):
logger.debug("Copying %s", f)
shutil.copy2(f, ConfigManager().configs_dir)
ConfigManager().get_list_of_configs()
self.input_updated = Caller()
self.assisted_input_updated = Caller()
self.heighthold_input_updated = Caller()
self.hover_input_updated = Caller()
self.rp_trim_updated = Caller()
self.emergency_stop_updated = Caller()
self.device_discovery = Caller()
self.device_error = Caller()
self.assisted_control_updated = Caller()
self.alt1_updated = Caller()
self.alt2_updated = Caller()
# Call with 3 bools (rp_limiting, yaw_limiting, thrust_limiting)
self.limiting_updated = Caller()
def _get_device_from_name(self, device_name):
"""Get the raw device from a name"""
for d in readers.devices():
if d.name == device_name:
return d
return None
def set_hover_max_height(self, height):
self._hover_max_height = height
def set_alt_hold_available(self, available):
"""Set if altitude hold is available or not (depending on HW)"""
self.has_pressure_sensor = available
def _do_device_discovery(self):
devs = self.available_devices()
# This is done so that devs can easily get access
# to limits without creating lots of extra code
for d in devs:
d.input = self
if len(devs):
self.device_discovery.call(devs)
self._discovery_timer.stop()
def available_mux(self):
return self._mux
def set_mux(self, name=None, mux=None):
old_mux = self._selected_mux
if name:
for m in self._mux:
if m.name == name:
self._selected_mux = m
elif mux:
self._selected_mux = mux
old_mux.close()
logger.info("Selected MUX: {}".format(self._selected_mux.name))
def set_assisted_control(self, mode):
self._assisted_control = mode
def get_assisted_control(self):
return self._assisted_control
def available_devices(self):
"""List all available and approved input devices.
This function will filter available devices by using the
blacklist configuration and only return approved devices."""
devs = readers.devices()
devs += interfaces.devices()
approved_devs = []
for dev in devs:
if ((not self._dev_blacklist)
or (self._dev_blacklist
and not self._dev_blacklist.match(dev.name))):
dev.input = self
approved_devs.append(dev)
return approved_devs
def enableRawReading(self, device_name):
"""
Enable raw reading of the input device with id deviceId. This is used
to get raw values for setting up of input devices. Values are read
without using a mapping.
"""
if self._input_device:
self._input_device.close()
self._input_device = None
for d in readers.devices():
if d.name == device_name:
self._input_device = d
# Set the mapping to None to get raw values
self._input_device.input_map = None
self._input_device.open()
def get_saved_device_mapping(self, device_name):
"""Return the saved mapping for a given device"""
config = None
device_config_mapping = Config().get("device_config_mapping")
if device_name in list(device_config_mapping.keys()):
config = device_config_mapping[device_name]
logging.debug("For [{}] we recommend [{}]".format(device_name, config))
return config
def stop_raw_reading(self):
"""Disable raw reading of input device."""
if self._input_device:
self._input_device.close()
self._input_device = None
def read_raw_values(self):
""" Read raw values from the input device."""
[axes, buttons,
mapped_values] = self._input_device.read(include_raw=True)
dict_axes = {}
dict_buttons = {}
for i, a in enumerate(axes):
dict_axes[i] = a
for i, b in enumerate(buttons):
dict_buttons[i] = b
return [dict_axes, dict_buttons, mapped_values]
def set_raw_input_map(self, input_map):
"""Set an input device map"""
# TODO: Will not work!
if self._input_device:
self._input_device.input_map = input_map
def set_input_map(self, device_name, input_map_name):
"""Load and set an input device map with the given name"""
dev = self._get_device_from_name(device_name)
settings = ConfigManager().get_settings(input_map_name)
if settings:
self.springy_throttle = settings["springythrottle"]
self._rp_dead_band = settings["rp_dead_band"]
self._input_map = ConfigManager().get_config(input_map_name)
dev.input_map = self._input_map
dev.input_map_name = input_map_name
Config().get("device_config_mapping")[device_name] = input_map_name
dev.set_dead_band(self._rp_dead_band)
def start_input(self, device_name, role="Device", config_name=None):
"""
Start reading input from the device with name device_name using config
config_name. Returns True if device supports mapping, otherwise False
"""
try:
# device_id = self._available_devices[device_name]
# Check if we supplied a new map, if not use the preferred one
device = self._get_device_from_name(device_name)
self._selected_mux.add_device(device, role)
# Update the UI with the limiting for this device
self.limiting_updated.call(device.limit_rp, device.limit_yaw,
device.limit_thrust)
self._read_timer.start()
return device.supports_mapping
except Exception:
self.device_error.call(
"Error while opening/initializing input device\n\n%s" %
(traceback.format_exc()))
if not self._input_device:
self.device_error.call(
"Could not find device {}".format(device_name))
return False
def resume_input(self):
self._selected_mux.resume()
self._read_timer.start()
def pause_input(self, device_name=None):
"""Stop reading from the input device."""
self._read_timer.stop()
self._selected_mux.pause()
def _set_thrust_slew_rate(self, rate):
self._thrust_slew_rate = rate
if rate > 0:
self.thrust_slew_enabled = True
else:
self.thrust_slew_enabled = False
def _get_thrust_slew_rate(self):
return self._thrust_slew_rate
def read_input(self):
"""Read input data from the selected device"""
try:
data = self._selected_mux.read()
if data:
if data.toggled.assistedControl:
if self._assisted_control == \
JoystickReader.ASSISTED_CONTROL_POSHOLD or \
self._assisted_control == \
JoystickReader.ASSISTED_CONTROL_HOVER:
if data.assistedControl and self._assisted_control != \
JoystickReader.ASSISTED_CONTROL_HOVER:
for d in self._selected_mux.devices():
d.limit_thrust = False
d.limit_rp = False
elif data.assistedControl:
for d in self._selected_mux.devices():
d.limit_thrust = True
d.limit_rp = False
else:
for d in self._selected_mux.devices():
d.limit_thrust = True
d.limit_rp = True
if self._assisted_control == \
JoystickReader.ASSISTED_CONTROL_ALTHOLD:
self.assisted_control_updated.call(
data.assistedControl)
if ((self._assisted_control
== JoystickReader.ASSISTED_CONTROL_HEIGHTHOLD)
or (self._assisted_control
== JoystickReader.ASSISTED_CONTROL_HOVER)):
try:
self.assisted_control_updated.call(
data.assistedControl)
if not data.assistedControl:
# Reset height controller state to initial
# target height both in the UI and in the
# Espdrone.
# TODO: Implement a proper state update of the
# input layer
self.heighthold_input_updated.\
call(0, 0,
0, INITAL_TAGET_HEIGHT)
self.hover_input_updated.\
call(0, 0,
0, INITAL_TAGET_HEIGHT)
except Exception as e:
logger.warning(
"Exception while doing callback from "
"input-device for assited "
"control: {}".format(e))
if data.toggled.estop:
try:
self.emergency_stop_updated.call(data.estop)
except Exception as e:
logger.warning("Exception while doing callback from"
"input-device for estop: {}".format(e))
if data.toggled.alt1:
try:
self.alt1_updated.call(data.alt1)
except Exception as e:
logger.warning("Exception while doing callback from"
"input-device for alt1: {}".format(e))
if data.toggled.alt2:
try:
self.alt2_updated.call(data.alt2)
except Exception as e:
logger.warning("Exception while doing callback from"
"input-device for alt2: {}".format(e))
# Reset height target when height-hold is not selected
if not data.assistedControl or \
(self._assisted_control !=
JoystickReader.ASSISTED_CONTROL_HEIGHTHOLD and
self._assisted_control !=
JoystickReader.ASSISTED_CONTROL_HOVER):
self._target_height = INITAL_TAGET_HEIGHT
if self._assisted_control == \
JoystickReader.ASSISTED_CONTROL_POSHOLD \
and data.assistedControl:
vx = data.roll
vy = data.pitch
vz = data.thrust
yawrate = data.yaw
# The odd use of vx and vy is to map forward on the
# physical joystick to positiv X-axis
self.assisted_input_updated.call(vy, -vx, vz, yawrate)
elif self._assisted_control == \
JoystickReader.ASSISTED_CONTROL_HOVER \
and data.assistedControl:
vx = data.roll
vy = data.pitch
# Scale thrust to a value between -1.0 to 1.0
vz = (data.thrust - 32767) / 32767.0
# Integrate velosity setpoint
self._target_height += vz * INPUT_READ_PERIOD
# Cap target height
if self._target_height > self._hover_max_height:
self._target_height = self._hover_max_height
if self._target_height < MIN_HOVER_HEIGHT:
self._target_height = MIN_HOVER_HEIGHT
yawrate = data.yaw
# The odd use of vx and vy is to map forward on the
# physical joystick to positiv X-axis
self.hover_input_updated.call(vy, -vx, yawrate,
self._target_height)
else:
# Update the user roll/pitch trim from device
if data.toggled.pitchNeg and data.pitchNeg:
self.trim_pitch -= .2
if data.toggled.pitchPos and data.pitchPos:
self.trim_pitch += .2
if data.toggled.rollNeg and data.rollNeg:
self.trim_roll -= .2
if data.toggled.rollPos and data.rollPos:
self.trim_roll += .2
if data.toggled.pitchNeg or data.toggled.pitchPos or \
data.toggled.rollNeg or data.toggled.rollPos:
self.rp_trim_updated.call(self.trim_roll,
self.trim_pitch)
if self._assisted_control == \
JoystickReader.ASSISTED_CONTROL_HEIGHTHOLD \
and data.assistedControl:
roll = data.roll + self.trim_roll
pitch = data.pitch + self.trim_pitch
yawrate = data.yaw
# Scale thrust to a value between -1.0 to 1.0
vz = (data.thrust - 32767) / 32767.0
# Integrate velosity setpoint
self._target_height += vz * INPUT_READ_PERIOD
# Cap target height
if self._target_height > self._hover_max_height:
self._target_height = self._hover_max_height
if self._target_height < MIN_TARGET_HEIGHT:
self._target_height = MIN_TARGET_HEIGHT
self.heighthold_input_updated.call(
roll, -pitch, yawrate, self._target_height)
else:
# Using alt hold the data is not in a percentage
if not data.assistedControl:
data.thrust = JoystickReader.p2t(data.thrust)
# Thrust might be <0 here, make sure it's not otherwise
# we'll get an error.
if data.thrust < 0:
data.thrust = 0
if data.thrust > 0xFFFF:
data.thrust = 0xFFFF
self.input_updated.call(data.roll + self.trim_roll,
data.pitch + self.trim_pitch,
data.yaw, data.thrust)
else:
self.input_updated.call(0, 0, 0, 0)
except Exception:
logger.warning("Exception while reading inputdevice: %s",
traceback.format_exc())
self.device_error.call("Error reading from input device\n\n%s" %
traceback.format_exc())
self.input_updated.call(0, 0, 0, 0)
self._read_timer.stop()
@staticmethod
def p2t(percentage):
"""Convert a percentage to raw thrust"""
return int(MAX_THRUST * (percentage / 100.0))
thrust_slew_rate = property(_get_thrust_slew_rate, _set_thrust_slew_rate)
class Memory():
"""Access memories on the Espdrone"""
# These codes can be decoded using os.stderror, but
# some of the text messages will look very strange
# in the UI, so they are redefined here
_err_codes = {
errno.ENOMEM: 'No more memory available',
errno.ENOEXEC: 'Command not found',
errno.ENOENT: 'No such block id',
errno.E2BIG: 'Block too large',
errno.EEXIST: 'Block already exists'
}
def __init__(self, espdrone=None):
"""Instantiate class and connect callbacks"""
# Called when new memories have been added
self.mem_added_cb = Caller()
# Called when new data has been read
self.mem_read_cb = Caller()
self.mem_write_cb = Caller()
self.ed = espdrone
self.ed.add_port_callback(CRTPPort.MEM, self._new_packet_cb)
self.ed.disconnected.add_callback(self._disconnected)
self._write_requests_lock = Lock()
self._clear_state()
def _clear_state(self):
self.mems = []
self._refresh_callback = None
self._fetch_id = 0
self.nbr_of_mems = 0
self._ow_mem_fetch_index = 0
self._elem_data = ()
self._read_requests = {}
self._write_requests = {}
self._ow_mems_left_to_update = []
self._getting_count = False
def _mem_update_done(self, mem):
"""
Callback from each individual memory (only 1-wire) when reading of
header/elements are done
"""
if mem.id in self._ow_mems_left_to_update:
self._ow_mems_left_to_update.remove(mem.id)
logger.debug(mem)
if len(self._ow_mems_left_to_update) == 0:
if self._refresh_callback:
self._refresh_callback()
self._refresh_callback = None
def get_mem(self, id):
"""Fetch the memory with the supplied id"""
for m in self.mems:
if m.id == id:
return m
return None
def get_mems(self, type):
"""Fetch all the memories of the supplied type"""
ret = ()
for m in self.mems:
if m.type == type:
ret += (m,)
return ret
def ow_search(self, vid=0xBC, pid=None, name=None):
"""Search for specific memory id/name and return it"""
for m in self.get_mems(MemoryElement.TYPE_1W):
if pid and m.pid == pid or name and m.name == name:
return m
return None
def write(self, memory, addr, data, flush_queue=False):
"""Write the specified data to the given memory at the given address"""
wreq = _WriteRequest(memory, addr, data, self.ed)
if memory.id not in self._write_requests:
self._write_requests[memory.id] = []
# Workaround until we secure the uplink and change messages for
# mems to non-blocking
self._write_requests_lock.acquire()
if flush_queue:
self._write_requests[memory.id] = self._write_requests[
memory.id][:1]
self._write_requests[memory.id].insert(len(self._write_requests), wreq)
if len(self._write_requests[memory.id]) == 1:
wreq.start()
self._write_requests_lock.release()
return True
def read(self, memory, addr, length):
"""
Read the specified amount of bytes from the given memory at the given
address
"""
if memory.id in self._read_requests:
logger.warning('There is already a read operation ongoing for '
'memory id {}'.format(memory.id))
return False
rreq = _ReadRequest(memory, addr, length, self.ed)
self._read_requests[memory.id] = rreq
rreq.start()
return True
def refresh(self, refresh_done_callback):
"""Start fetching all the detected memories"""
self._refresh_callback = refresh_done_callback
self._fetch_id = 0
for m in self.mems:
try:
self.mem_read_cb.remove_callback(m.new_data)
m.disconnect()
except Exception as e:
logger.info(
'Error when removing memory after update: {}'.format(e))
self.mems = []
self.nbr_of_mems = 0
self._getting_count = False
logger.debug('Requesting number of memories')
pk = CRTPPacket()
pk.set_header(CRTPPort.MEM, CHAN_INFO)
pk.data = (CMD_INFO_NBR,)
self.ed.send_packet(pk, expected_reply=(CMD_INFO_NBR,))
def _disconnected(self, uri):
"""The link to the Espdrone has been broken. Reset state"""
self._clear_state()
def _new_packet_cb(self, packet):
"""Callback for newly arrived packets for the memory port"""
chan = packet.channel
cmd = packet.data[0]
payload = packet.data[1:]
if chan == CHAN_INFO:
if cmd == CMD_INFO_NBR:
self.nbr_of_mems = payload[0]
logger.info('{} memories found'.format(self.nbr_of_mems))
# Start requesting information about the memories,
# if there are any...
if self.nbr_of_mems > 0:
if not self._getting_count:
self._getting_count = True
logger.debug('Requesting first id')
pk = CRTPPacket()
pk.set_header(CRTPPort.MEM, CHAN_INFO)
pk.data = (CMD_INFO_DETAILS, 0)
self.ed.send_packet(pk, expected_reply=(
CMD_INFO_DETAILS, 0))
else:
self._refresh_callback()
if cmd == CMD_INFO_DETAILS:
# Did we get a good reply, otherwise try again:
if len(payload) < 5:
# Workaround for 1-wire bug when memory is detected
# but updating the info crashes the communication with
# the 1-wire. Fail by saying we only found 1 memory
# (the I2C).
logger.error(
'-------->Got good count, but no info on mem!')
self.nbr_of_mems = 1
if self._refresh_callback:
self._refresh_callback()
self._refresh_callback = None
return
# Create information about a new memory
# Id - 1 byte
mem_id = payload[0]
# Type - 1 byte
mem_type = payload[1]
# Size 4 bytes (as addr)
mem_size = struct.unpack('I', payload[2:6])[0]
# Addr (only valid for 1-wire?)
mem_addr_raw = struct.unpack('B' * 8, payload[6:14])
mem_addr = ''
for m in mem_addr_raw:
mem_addr += '{:02X}'.format(m)
if (not self.get_mem(mem_id)):
if mem_type == MemoryElement.TYPE_1W:
mem = OWElement(id=mem_id, type=mem_type,
size=mem_size,
addr=mem_addr, mem_handler=self)
self.mem_read_cb.add_callback(mem.new_data)
self.mem_write_cb.add_callback(mem.write_done)
self._ow_mems_left_to_update.append(mem.id)
elif mem_type == MemoryElement.TYPE_I2C:
mem = I2CElement(id=mem_id, type=mem_type,
size=mem_size,
mem_handler=self)
self.mem_read_cb.add_callback(mem.new_data)
self.mem_write_cb.add_callback(mem.write_done)
elif mem_type == MemoryElement.TYPE_DRIVER_LED:
mem = LEDDriverMemory(id=mem_id, type=mem_type,
size=mem_size, mem_handler=self)
logger.debug(mem)
self.mem_read_cb.add_callback(mem.new_data)
self.mem_write_cb.add_callback(mem.write_done)
elif mem_type == MemoryElement.TYPE_LOCO:
mem = LocoMemory(id=mem_id, type=mem_type,
size=mem_size, mem_handler=self)
logger.debug(mem)
self.mem_read_cb.add_callback(mem.new_data)
elif mem_type == MemoryElement.TYPE_TRAJ:
mem = TrajectoryMemory(id=mem_id, type=mem_type,
size=mem_size, mem_handler=self)
logger.debug(mem)
self.mem_write_cb.add_callback(mem.write_done)
elif mem_type == MemoryElement.TYPE_LOCO2:
mem = LocoMemory2(id=mem_id, type=mem_type,
size=mem_size, mem_handler=self)
logger.debug(mem)
self.mem_read_cb.add_callback(mem.new_data)
elif mem_type == MemoryElement.TYPE_LH:
mem = LighthouseMemory(id=mem_id, type=mem_type,
size=mem_size, mem_handler=self)
logger.debug(mem)
self.mem_read_cb.add_callback(mem.new_data)
self.mem_write_cb.add_callback(mem.write_done)
elif mem_type == MemoryElement.TYPE_MEMORY_TESTER:
mem = MemoryTester(id=mem_id, type=mem_type,
size=mem_size, mem_handler=self)
logger.debug(mem)
self.mem_read_cb.add_callback(mem.new_data)
self.mem_write_cb.add_callback(mem.write_done)
else:
mem = MemoryElement(id=mem_id, type=mem_type,
size=mem_size, mem_handler=self)
logger.debug(mem)
self.mems.append(mem)
self.mem_added_cb.call(mem)
self._fetch_id = mem_id + 1
if self.nbr_of_mems - 1 >= self._fetch_id:
logger.debug(
'Requesting information about memory {}'.format(
self._fetch_id))
pk = CRTPPacket()
pk.set_header(CRTPPort.MEM, CHAN_INFO)
pk.data = (CMD_INFO_DETAILS, self._fetch_id)
self.ed.send_packet(pk, expected_reply=(
CMD_INFO_DETAILS, self._fetch_id))
else:
logger.debug(
'Done getting all the memories, start reading the OWs')
ows = self.get_mems(MemoryElement.TYPE_1W)
# If there are any OW mems start reading them, otherwise
# we are done
for ow_mem in ows:
ow_mem.update(self._mem_update_done)
if len(ows) == 0:
if self._refresh_callback:
self._refresh_callback()
self._refresh_callback = None
if chan == CHAN_WRITE:
id = cmd
(addr, status) = struct.unpack('<IB', payload[0:5])
logger.debug(
'WRITE: Mem={}, addr=0x{:X}, status=0x{}'.format(
id, addr, status))
# Find the read request
if id in self._write_requests:
self._write_requests_lock.acquire()
wreq = self._write_requests[id][0]
if status == 0:
if wreq.write_done(addr):
# self._write_requests.pop(id, None)
# Remove the first item
self._write_requests[id].pop(0)
self.mem_write_cb.call(wreq.mem, wreq.addr)
# Get a new one to start (if there are any)
if len(self._write_requests[id]) > 0:
self._write_requests[id][0].start()
else:
logger.debug(
'Status {}: write resending...'.format(status))
wreq.resend()
self._write_requests_lock.release()
if chan == CHAN_READ:
id = cmd
(addr, status) = struct.unpack('<IB', payload[0:5])
data = struct.unpack('B' * len(payload[5:]), payload[5:])
logger.debug('READ: Mem={}, addr=0x{:X}, status=0x{}, '
'data={}'.format(id, addr, status, data))
# Find the read request
if id in self._read_requests:
logger.debug(
'READING: We are still interested in request for '
'mem {}'.format(id))
rreq = self._read_requests[id]
if status == 0:
if rreq.add_data(addr, payload[5:]):
self._read_requests.pop(id, None)
self.mem_read_cb.call(rreq.mem, rreq.addr, rreq.data)
else:
logger.debug('Status {}: resending...'.format(status))
rreq.resend()
class Log():
"""Create log configuration"""
# These codes can be decoded using os.stderror, but
# some of the text messages will look very strange
# in the UI, so they are redefined here
_err_codes = {
errno.ENOMEM: 'No more memory available',
errno.ENOEXEC: 'Command not found',
errno.ENOENT: 'No such block id',
errno.E2BIG: 'Block too large',
errno.EEXIST: 'Block already exists'
}
def __init__(self, espdrone=None):
self.log_blocks = []
# Called with newly created blocks
self.block_added_cb = Caller()
self.ed = espdrone
self.toc = None
self.ed.add_port_callback(CRTPPort.LOGGING, self._new_packet_cb)
self.toc_updated = Caller()
self.state = IDLE
self.fake_toc_crc = 0xDEADBEEF
self._refresh_callback = None
self._toc_cache = None
self._config_id_counter = 1
self._useV2 = False
def add_config(self, logconf):
"""Add a log configuration to the logging framework.
When doing this the contents of the log configuration will be validated
and listeners for new log configurations will be notified. When
validating the configuration the variables are checked against the TOC
to see that they actually exist. If they don't then the configuration
cannot be used. Since a valid TOC is required, a Espdrone has to be
connected when calling this method, otherwise it will fail."""
if not self.ed.link:
logger.error('Cannot add configs without being connected to a '
'Espdrone!')
return
# If the log configuration contains variables that we added without
# type (i.e we want the stored as type for fetching as well) then
# resolve this now and add them to the block again.
for name in logconf.default_fetch_as:
var = self.toc.get_element_by_complete_name(name)
if not var:
logger.warning('%s not in TOC, this block cannot be used!',
name)
logconf.valid = False
raise KeyError('Variable {} not in TOC'.format(name))
# Now that we know what type this variable has, add it to the log
# config again with the correct type
logconf.add_variable(name, var.ctype)
# Now check that all the added variables are in the TOC and that
# the total size constraint of a data packet with logging data is
# not
size = 0
for var in logconf.variables:
size += LogTocElement.get_size_from_id(var.fetch_as)
# Check that we are able to find the variable in the TOC so
# we can return error already now and not when the config is sent
if var.is_toc_variable():
if (self.toc.get_element_by_complete_name(var.name) is None):
logger.warning(
'Log: %s not in TOC, this block cannot be used!',
var.name)
logconf.valid = False
raise KeyError('Variable {} not in TOC'.format(var.name))
if (size <= MAX_LOG_DATA_PACKET_SIZE
and (logconf.period > 0 and logconf.period < 0xFF)):
logconf.valid = True
logconf.ed = self.ed
logconf.id = self._config_id_counter
logconf.useV2 = self._useV2
self._config_id_counter = (self._config_id_counter + 1) % 255
self.log_blocks.append(logconf)
self.block_added_cb.call(logconf)
else:
logconf.valid = False
raise AttributeError(
'The log configuration is too large or has an invalid '
'parameter')
def refresh_toc(self, refresh_done_callback, toc_cache):
"""Start refreshing the table of loggale variables"""
self._useV2 = self.ed.platform.get_protocol_version() >= 4
self._toc_cache = toc_cache
self._refresh_callback = refresh_done_callback
self.toc = None
pk = CRTPPacket()
pk.set_header(CRTPPort.LOGGING, CHAN_SETTINGS)
pk.data = (CMD_RESET_LOGGING, )
self.ed.send_packet(pk, expected_reply=(CMD_RESET_LOGGING, ))
def _find_block(self, id):
for block in self.log_blocks:
if block.id == id:
return block
return None
def _new_packet_cb(self, packet):
"""Callback for newly arrived packets with TOC information"""
chan = packet.channel
cmd = packet.data[0]
payload = packet.data[1:]
if (chan == CHAN_SETTINGS):
id = payload[0]
error_status = payload[1]
block = self._find_block(id)
if cmd == CMD_CREATE_BLOCK or cmd == CMD_CREATE_BLOCK_V2:
if (block is not None):
if error_status == 0 or error_status == errno.EEXIST:
if not block.added:
logger.debug('Have successfully added id=%d', id)
pk = CRTPPacket()
pk.set_header(5, CHAN_SETTINGS)
pk.data = (CMD_START_LOGGING, id, block.period)
self.ed.send_packet(
pk, expected_reply=(CMD_START_LOGGING, id))
block.added = True
else:
msg = self._err_codes[error_status]
logger.warning('Error %d when adding id=%d (%s)',
error_status, id, msg)
block.err_no = error_status
block.added_cb.call(False)
block.error_cb.call(block, msg)
else:
logger.warning('No LogEntry to assign block to !!!')
if (cmd == CMD_START_LOGGING):
if (error_status == 0x00):
logger.info('Have successfully started logging for id=%d',
id)
if block:
block.started = True
else:
msg = self._err_codes[error_status]
logger.warning('Error %d when starting id=%d (%s)',
error_status, id, msg)
if block:
block.err_no = error_status
block.started_cb.call(self, False)
# This is a temporary fix, we are adding a new issue
# for this. For some reason we get an error back after
# the block has been started and added. This will show
# an error in the UI, but everything is still working.
# block.error_cb.call(block, msg)
if (cmd == CMD_STOP_LOGGING):
if (error_status == 0x00):
logger.info('Have successfully stopped logging for id=%d',
id)
if block:
block.started = False
if (cmd == CMD_DELETE_BLOCK):
# Accept deletion of a block that isn't added. This could
# happen due to timing (i.e add/start/delete in fast sequence)
if error_status == 0x00 or error_status == errno.ENOENT:
logger.info('Have successfully deleted id=%d', id)
if block:
block.started = False
block.added = False
if (cmd == CMD_RESET_LOGGING):
# Guard against multiple responses due to re-sending
if not self.toc:
logger.debug('Logging reset, continue with TOC download')
self.log_blocks = []
self.toc = Toc()
toc_fetcher = Toedetcher(self.ed, LogTocElement,
CRTPPort.LOGGING, self.toc,
self._refresh_callback,
self._toc_cache)
toc_fetcher.start()
if (chan == CHAN_LOGDATA):
chan = packet.channel
id = packet.data[0]
block = self._find_block(id)
timestamps = struct.unpack('<BBB', packet.data[1:4])
timestamp = (timestamps[0] | timestamps[1] << 8
| timestamps[2] << 16)
logdata = packet.data[4:]
if (block is not None):
block.unpack_log_data(logdata, timestamp)
else:
logger.warning('Error no LogEntry to handle id=%d', id)
def setUp(self):
self.callback_count = 0
self.sut = Caller()
class ConfigManager(metaclass=Singleton):
""" Singleton class for managing input processing """
conf_needs_reload = Caller()
configs_dir = edclient.config_path + "/input"
def __init__(self):
"""Initialize and create empty config list"""
self._list_of_configs = []
def get_config(self, config_name):
"""Get the button and axis mappings for an input device."""
try:
idx = self._list_of_configs.index(config_name)
return self._input_config[idx]
except Exception:
return None
def get_settings(self, config_name):
"""Get the settings for an input device."""
try:
idx = self._list_of_configs.index(config_name)
return self._input_settings[idx]
except Exception:
return None
def get_list_of_configs(self):
"""Reload the configurations from file"""
try:
configs = [os.path.basename(f) for f in
glob.glob(self.configs_dir + "/[A-Za-z]*.json")]
self._input_config = []
self._input_settings = []
self._list_of_configs = []
for conf in configs:
logger.debug("Parsing [%s]", conf)
json_data = open(self.configs_dir + "/%s" % conf)
data = json.load(json_data)
new_input_device = {}
new_input_settings = {"updateperiod": 10,
"springythrottle": True,
"rp_dead_band": 0.05}
for s in data["inputconfig"]["inputdevice"]:
if s == "axis":
for a in data["inputconfig"]["inputdevice"]["axis"]:
axis = {}
axis["scale"] = a["scale"]
axis["offset"] = a[
"offset"] if "offset" in a else 0.0
axis["type"] = a["type"]
axis["key"] = a["key"]
axis["name"] = a["name"]
self._translate_for_backwards_compatibility(axis)
try:
ids = a["ids"]
except Exception:
ids = [a["id"]]
for id in ids:
locaxis = copy.deepcopy(axis)
if "ids" in a:
if id == a["ids"][0]:
locaxis["scale"] = locaxis[
"scale"] * -1
locaxis["id"] = id
# 'type'-'id' defines unique index for axis
index = "%s-%d" % (a["type"], id)
new_input_device[index] = locaxis
else:
new_input_settings[s] = data[
"inputconfig"]["inputdevice"][s]
self._input_config.append(new_input_device)
self._input_settings.append(new_input_settings)
json_data.close()
self._list_of_configs.append(conf[:-5])
except Exception as e:
logger.warning("Exception while parsing inputconfig file: %s ", e)
return self._list_of_configs
def save_config(self, input_map, config_name):
"""Save a configuration to file"""
mapping = {'inputconfig': {'inputdevice': {'axis': []}}}
# Create intermediate structure for the configuration file
funcs = {}
for m in input_map:
key = input_map[m]["key"]
if key not in funcs:
funcs[key] = []
funcs[key].append(input_map[m])
# Create a mapping for each axis, take care to handle
# split axis configurations
for a in funcs:
func = funcs[a]
axis = {}
# Check for split axis
if len(func) > 1:
axis["ids"] = [func[0]["id"], func[1]["id"]]
axis["scale"] = func[1]["scale"]
else:
axis["id"] = func[0]["id"]
axis["scale"] = func[0]["scale"]
axis["key"] = func[0]["key"]
axis["name"] = func[0]["key"] # Name isn't used...
axis["type"] = func[0]["type"]
mapping["inputconfig"]["inputdevice"]["axis"].append(axis)
mapping["inputconfig"]['inputdevice']['name'] = config_name
mapping["inputconfig"]['inputdevice']['updateperiod'] = 10
filename = ConfigManager().configs_dir + "/%s.json" % config_name
logger.info("Saving config to [%s]", filename)
json_data = open(filename, 'w')
json_data.write(json.dumps(mapping, indent=2))
json_data.close()
self.conf_needs_reload.call(config_name)
def _translate_for_backwards_compatibility(self, axis):
"""Handle changes in the config file format"""
# The parameter that used to be called 'althold' has been renamed to
# 'assistedControl'
althold = 'althold'
assistedControl = 'assistedControl'
if axis['key'] == althold:
axis['key'] = assistedControl
if axis['name'] == althold:
axis['name'] = assistedControl
def __init__(self, period, callback):
self._callbacks = Caller()
self._callbacks.add_callback(callback)
self._started = False
self._period = period
self._thread = None
class Param():
"""
Used to read and write parameter values in the Espdrone.
"""
def __init__(self, espdrone):
self.toc = Toc()
self.ed = espdrone
self._useV2 = False
self.param_update_callbacks = {}
self.group_update_callbacks = {}
self.all_update_callback = Caller()
self.param_updater = None
self.param_updater = _ParamUpdater(self.ed, self._useV2,
self._param_updated)
self.param_updater.start()
self.ed.disconnected.add_callback(self._disconnected)
self.all_updated = Caller()
self.is_updated = False
self.values = {}
def request_update_of_all_params(self):
"""Request an update of all the parameters in the TOC"""
for group in self.toc.toc:
for name in self.toc.toc[group]:
complete_name = '%s.%s' % (group, name)
self.request_param_update(complete_name)
def _check_if_all_updated(self):
"""Check if all parameters from the TOC has at least been fetched
once"""
for g in self.toc.toc:
if g not in self.values:
return False
for n in self.toc.toc[g]:
if n not in self.values[g]:
return False
return True
def _param_updated(self, pk):
"""Callback with data for an updated parameter"""
if self._useV2:
var_id = struct.unpack('<H', pk.data[:2])[0]
else:
var_id = pk.data[0]
element = self.toc.get_element_by_id(var_id)
if element:
if self._useV2:
s = struct.unpack(element.pytype, pk.data[2:])[0]
else:
s = struct.unpack(element.pytype, pk.data[1:])[0]
s = s.__str__()
complete_name = '%s.%s' % (element.group, element.name)
# Save the value for synchronous access
if element.group not in self.values:
self.values[element.group] = {}
self.values[element.group][element.name] = s
logger.debug('Updated parameter [%s]' % complete_name)
if complete_name in self.param_update_callbacks:
self.param_update_callbacks[complete_name].call(
complete_name, s)
if element.group in self.group_update_callbacks:
self.group_update_callbacks[element.group].call(
complete_name, s)
self.all_update_callback.call(complete_name, s)
# Once all the parameters are updated call the
# callback for "everything updated" (after all the param
# updated callbacks)
if self._check_if_all_updated() and not self.is_updated:
self.is_updated = True
self.all_updated.call()
else:
logger.debug('Variable id [%d] not found in TOC', var_id)
def remove_update_callback(self, group, name=None, cb=None):
"""Remove the supplied callback for a group or a group.name"""
if not cb:
return
if not name:
if group in self.group_update_callbacks:
self.group_update_callbacks[group].remove_callback(cb)
else:
paramname = '{}.{}'.format(group, name)
if paramname in self.param_update_callbacks:
self.param_update_callbacks[paramname].remove_callback(cb)
def add_update_callback(self, group=None, name=None, cb=None):
"""
Add a callback for a specific parameter name. This callback will be
executed when a new value is read from the Espdrone.
"""
if not group and not name:
self.all_update_callback.add_callback(cb)
elif not name:
if group not in self.group_update_callbacks:
self.group_update_callbacks[group] = Caller()
self.group_update_callbacks[group].add_callback(cb)
else:
paramname = '{}.{}'.format(group, name)
if paramname not in self.param_update_callbacks:
self.param_update_callbacks[paramname] = Caller()
self.param_update_callbacks[paramname].add_callback(cb)
def refresh_toc(self, refresh_done_callback, toc_cache):
"""
Initiate a refresh of the parameter TOC.
"""
self._useV2 = self.ed.platform.get_protocol_version() >= 4
toc_fetcher = Toedetcher(self.ed, ParamTocElement, CRTPPort.PARAM,
self.toc, refresh_done_callback, toc_cache)
toc_fetcher.start()
def _disconnected(self, uri):
"""Disconnected callback from Espdrone API"""
self.param_updater.close()
self.is_updated = False
# Clear all values from the previous Espdrone
self.toc = Toc()
self.values = {}
def request_param_update(self, complete_name):
"""
Request an update of the value for the supplied parameter.
"""
self.param_updater.request_param_update(
self.toc.get_element_id(complete_name))
def set_value(self, complete_name, value):
"""
Set the value for the supplied parameter.
"""
element = self.toc.get_element_by_complete_name(complete_name)
if not element:
logger.warning("Cannot set value for [%s], it's not in the TOC!",
complete_name)
raise KeyError('{} not in param TOC'.format(complete_name))
elif element.access == ParamTocElement.RO_ACCESS:
logger.debug('[%s] is read only, no trying to set value',
complete_name)
raise AttributeError('{} is read-only!'.format(complete_name))
else:
varid = element.ident
pk = CRTPPacket()
pk.set_header(CRTPPort.PARAM, WRITE_CHANNEL)
if self._useV2:
pk.data = struct.pack('<H', varid)
else:
pk.data = struct.pack('<B', varid)
try:
value_nr = eval(value)
except TypeError:
value_nr = value
pk.data += struct.pack(element.pytype, value_nr)
self.param_updater.request_param_setvalue(pk)
def __init__(self, name=None, link=None, ro_cache=None, rw_cache=None):
"""
Create the objects from this module and register callbacks.
ro_cache -- Path to read-only cache (string)
rw_cache -- Path to read-write cache (string)
"""
# Called on disconnect, no matter the reason
self.disconnected = Caller()
# Called on unintentional disconnect only
self.connection_lost = Caller()
# Called when the first packet in a new link is received
self.link_established = Caller()
# Called when the user requests a connection
self.connection_requested = Caller()
# Called when the link is established and the TOCs (that are not
# cached) have been downloaded
self.connected = Caller()
# Called if establishing of the link fails (i.e times out)
self.connection_failed = Caller()
# Called for every packet received
self.packet_received = Caller()
# Called for every packet sent
self.packet_sent = Caller()
# Called when the link driver updates the link quality measurement
self.link_quality_updated = Caller()
self.state = State.DISCONNECTED
self.link = link
self.name = name
self._toc_cache = TocCache(ro_cache=ro_cache,
rw_cache=rw_cache)
self.incoming = _IncomingPacketHandler(self)
self.incoming.setDaemon(True)
self.incoming.start()
self.camera = Camera(self)
self.commander = Commander(self)
self.high_level_commander = HighLevelCommander(self)
self.loc = Localization(self)
self.extpos = Extpos(self)
self.log = Log(self)
self.console = Console(self)
self.param = Param(self)
self.mem = Memory(self)
self.platform = PlatformService(self)
self.link_uri = ''
# Used for retry when no reply was sent back
self.packet_received.add_callback(self._check_for_initial_packet_cb)
self.packet_received.add_callback(self._check_for_answers)
self._answer_patterns = {}
self._send_lock = Lock()
self.connected_ts = None
# Connect callbacks to logger
self.disconnected.add_callback(
lambda uri: logger.info('Callback->Disconnected from [%s]', uri))
self.disconnected.add_callback(self._disconnected)
self.link_established.add_callback(
lambda uri: logger.info('Callback->Connected to [%s]', uri))
self.connection_lost.add_callback(
lambda uri, errmsg: logger.info(
'Callback->Connection lost to [%s]: %s', uri, errmsg))
self.connection_failed.add_callback(
lambda uri, errmsg: logger.info(
'Callback->Connected failed to [%s]: %s', uri, errmsg))
self.connection_requested.add_callback(
lambda uri: logger.info(
'Callback->Connection initialized[%s]', uri))
self.connected.add_callback(
lambda uri: logger.info(
'Callback->Connection setup finished [%s]', uri))
class Espdrone():
"""The Espdrone class"""
def __init__(self, name=None, link=None, ro_cache=None, rw_cache=None):
"""
Create the objects from this module and register callbacks.
ro_cache -- Path to read-only cache (string)
rw_cache -- Path to read-write cache (string)
"""
# Called on disconnect, no matter the reason
self.disconnected = Caller()
# Called on unintentional disconnect only
self.connection_lost = Caller()
# Called when the first packet in a new link is received
self.link_established = Caller()
# Called when the user requests a connection
self.connection_requested = Caller()
# Called when the link is established and the TOCs (that are not
# cached) have been downloaded
self.connected = Caller()
# Called if establishing of the link fails (i.e times out)
self.connection_failed = Caller()
# Called for every packet received
self.packet_received = Caller()
# Called for every packet sent
self.packet_sent = Caller()
# Called when the link driver updates the link quality measurement
self.link_quality_updated = Caller()
self.state = State.DISCONNECTED
self.link = link
self.name = name
self._toc_cache = TocCache(ro_cache=ro_cache,
rw_cache=rw_cache)
self.incoming = _IncomingPacketHandler(self)
self.incoming.setDaemon(True)
self.incoming.start()
self.camera = Camera(self)
self.commander = Commander(self)
self.high_level_commander = HighLevelCommander(self)
self.loc = Localization(self)
self.extpos = Extpos(self)
self.log = Log(self)
self.console = Console(self)
self.param = Param(self)
self.mem = Memory(self)
self.platform = PlatformService(self)
self.link_uri = ''
# Used for retry when no reply was sent back
self.packet_received.add_callback(self._check_for_initial_packet_cb)
self.packet_received.add_callback(self._check_for_answers)
self._answer_patterns = {}
self._send_lock = Lock()
self.connected_ts = None
# Connect callbacks to logger
self.disconnected.add_callback(
lambda uri: logger.info('Callback->Disconnected from [%s]', uri))
self.disconnected.add_callback(self._disconnected)
self.link_established.add_callback(
lambda uri: logger.info('Callback->Connected to [%s]', uri))
self.connection_lost.add_callback(
lambda uri, errmsg: logger.info(
'Callback->Connection lost to [%s]: %s', uri, errmsg))
self.connection_failed.add_callback(
lambda uri, errmsg: logger.info(
'Callback->Connected failed to [%s]: %s', uri, errmsg))
self.connection_requested.add_callback(
lambda uri: logger.info(
'Callback->Connection initialized[%s]', uri))
self.connected.add_callback(
lambda uri: logger.info(
'Callback->Connection setup finished [%s]', uri))
def _disconnected(self, link_uri):
""" Callback when disconnected."""
self.connected_ts = None
def _start_connection_setup(self):
"""Start the connection setup by refreshing the TOCs"""
logger.info('We are connected[%s], request connection setup',
self.link_uri)
self.platform.fetch_platform_informations(self._platform_info_fetched)
def _platform_info_fetched(self):
self.log.refresh_toc(self._log_toc_updated_cb, self._toc_cache)
def _param_toc_updated_cb(self):
"""Called when the param TOC has been fully updated"""
logger.info('Param TOC finished updating')
self.connected_ts = datetime.datetime.now()
self.connected.call(self.link_uri)
# Trigger the update for all the parameters
self.param.request_update_of_all_params()
def _mems_updated_cb(self):
"""Called when the memories have been identified"""
logger.info('Memories finished updating')
self.param.refresh_toc(self._param_toc_updated_cb, self._toc_cache)
def _log_toc_updated_cb(self):
"""Called when the log TOC has been fully updated"""
logger.info('Log TOC finished updating')
self.mem.refresh(self._mems_updated_cb)
def _link_error_cb(self, errmsg):
"""Called from the link driver when there's an error"""
logger.warning('Got link error callback [%s] in state [%s]',
errmsg, self.state)
if (self.link is not None):
self.link.close()
self.link = None
if (self.state == State.INITIALIZED):
self.connection_failed.call(self.link_uri, errmsg)
if (self.state == State.CONNECTED or
self.state == State.SETUP_FINISHED):
self.disconnected.call(self.link_uri)
self.connection_lost.call(self.link_uri, errmsg)
self.state = State.DISCONNECTED
def _link_quality_cb(self, percentage):
"""Called from link driver to report link quality"""
self.link_quality_updated.call(percentage)
def _check_for_initial_packet_cb(self, data):
"""
Called when first packet arrives from Espdrone.
This is used to determine if we are connected to something that is
answering.
"""
self.state = State.CONNECTED
self.link_established.call(self.link_uri)
self.packet_received.remove_callback(self._check_for_initial_packet_cb)
def open_link(self, link_uri):
"""
Open the communication link to a copter at the given URI and setup the
connection (download log/parameter TOC).
"""
self.connection_requested.call(link_uri)
self.state = State.INITIALIZED
self.link_uri = link_uri
try:
self.link = edlib.crtp.get_link_driver(
link_uri, self._link_quality_cb, self._link_error_cb)
if not self.link:
message = 'No driver found or malformed URI: {}' \
.format(link_uri)
logger.warning(message)
self.connection_failed.call(link_uri, message)
else:
# Add a callback so we can check that any data is coming
# back from the copter
self.packet_received.add_callback(
self._check_for_initial_packet_cb)
self._start_connection_setup()
except Exception as ex: # pylint: disable=W0703
# We want to catch every possible exception here and show
# it in the user interface
import traceback
logger.error("Couldn't load link driver: %s\n\n%s",
ex, traceback.format_exc())
exception_text = "Couldn't load link driver: %s\n\n%s" % (
ex, traceback.format_exc())
if self.link:
self.link.close()
self.link = None
self.connection_failed.call(link_uri, exception_text)
raise ConnectionError()
def close_link(self):
"""Close the communication link."""
logger.info('Closing link')
if (self.link is not None):
self.commander.send_setpoint(0, 0, 0, 0)
if (self.link is not None):
self.link.close()
self.link = None
self._answer_patterns = {}
self.disconnected.call(self.link_uri)
"""Check if the communication link is open or not."""
def is_connected(self):
return self.connected_ts is not None
def add_port_callback(self, port, cb):
"""Add a callback to cb on port"""
self.incoming.add_port_callback(port, cb)
def remove_port_callback(self, port, cb):
"""Remove the callback cb on port"""
self.incoming.remove_port_callback(port, cb)
def _no_answer_do_retry(self, pk, pattern):
"""Resend packets that we have not gotten answers to"""
logger.info('Resending for pattern %s', pattern)
# Set the timer to None before trying to send again
self.send_packet(pk, expected_reply=pattern, resend=True)
def _check_for_answers(self, pk):
"""
Callback called for every packet received to check if we are
waiting for an answer on this port. If so, then cancel the retry
timer.
"""
longest_match = ()
if len(self._answer_patterns) > 0:
data = (pk.header,) + tuple(pk.data)
for p in list(self._answer_patterns.keys()):
logger.debug('Looking for pattern match on %s vs %s', p, data)
if len(p) <= len(data):
if p == data[0:len(p)]:
match = data[0:len(p)]
if len(match) >= len(longest_match):
logger.debug('Found new longest match %s', match)
longest_match = match
if len(longest_match) > 0:
self._answer_patterns[longest_match].cancel()
del self._answer_patterns[longest_match]
def send_packet(self, pk, expected_reply=(), resend=False, timeout=0.1):
"""
Send a packet through the link interface.
pk -- Packet to send
expect_answer -- True if a packet from the Espdrone is expected to
be sent back, otherwise false
"""
self._send_lock.acquire()
if self.link is not None:
if len(expected_reply) > 0 and not resend and \
self.link.needs_resending:
pattern = (pk.header,) + expected_reply
logger.debug(
'Sending packet and expecting the %s pattern back',
pattern)
new_timer = Timer(timeout,
lambda: self._no_answer_do_retry(pk,
pattern))
self._answer_patterns[pattern] = new_timer
new_timer.start()
elif resend:
# Check if we have gotten an answer, if not try again
pattern = expected_reply
if pattern in self._answer_patterns:
logger.debug('We want to resend and the pattern is there')
if self._answer_patterns[pattern]:
new_timer = Timer(timeout,
lambda:
self._no_answer_do_retry(
pk, pattern))
self._answer_patterns[pattern] = new_timer
new_timer.start()
else:
logger.debug('Resend requested, but no pattern found: %s',
self._answer_patterns)
self.link.send_packet(pk)
self.packet_sent.call(pk)
self._send_lock.release()
class Localization():
"""
Handle localization-related data communication with the Espdrone
"""
# Implemented channels
POSITION_CH = 0
GENERIC_CH = 1
# Location message types for generig channel
RANGE_STREAM_REPORT = 0
RANGE_STREAM_REPORT_FP16 = 1
LPS_SHORT_LPP_PACKET = 2
EMERGENCY_STOP = 3
EMERGENCY_STOP_WATCHDOG = 4
COMM_GNSS_NMEA = 6
COMM_GNSS_PROPRIETARY = 7
EXT_POSE = 8
EXT_POSE_PACKED = 9
EMERGENCY_RESET = 10
def __init__(self, espdrone=None):
"""
Initialize the Extpos object.
"""
self._ed = espdrone
self.receivedLocationPacket = Caller()
self._ed.add_port_callback(CRTPPort.LOCALIZATION, self._incoming)
def _incoming(self, packet):
"""
Callback for data received from the copter.
"""
if len(packet.data) < 1:
logger.warning('Localization packet received with incorrect' +
'length (length is {})'.format(len(packet.data)))
return
pk_type = struct.unpack('<B', packet.data[:1])[0]
data = packet.data[1:]
# Decoding the known packet types
# TODO: more generic decoding scheme?
decoded_data = None
if pk_type == self.RANGE_STREAM_REPORT:
if len(data) % 5 != 0:
logger.error('Wrong range stream report data lenght')
return
decoded_data = {}
raw_data = data
for i in range(int(len(data) / 5)):
anchor_id, distance = struct.unpack('<Bf', raw_data[:5])
decoded_data[anchor_id] = distance
raw_data = raw_data[5:]
elif pk_type == self.LH_PERSIST_DATA:
decoded_data = bool(data[0])
elif pk_type == self.LH_ANGLE_STREAM:
decoded_data = self._decode_lh_angle(data)
pk = LocalizationPacket(pk_type, data, decoded_data)
self.receivedLocationPacket.call(pk)
def send_extpos(self, pos):
"""
Send the current Espdrone X, Y, Z position. This is going to be
forwarded to the Espdrone's position estimator.
"""
pk = CRTPPacket()
pk.port = CRTPPort.LOCALIZATION
pk.channel = self.POSITION_CH
pk.data = struct.pack('<fff', pos[0], pos[1], pos[2])
self._ed.send_packet(pk)
def send_extpose(self, pos, quat):
"""
Send the current Espdrone pose (position [x, y, z] and
attitude quaternion [qx, qy, qz, qw]). This is going to be forwarded
to the Espdrone's position estimator.
"""
pk = CRTPPacket()
pk.port = CRTPPort.LOCALIZATION
pk.channel = self.GENERIC_CH
pk.data = struct.pack('<Bfffffff', self.EXT_POSE, pos[0], pos[1],
pos[2], quat[0], quat[1], quat[2], quat[3])
self._ed.send_packet(pk)
def send_short_lpp_packet(self, dest_id, data):
"""
Send ultra-wide-band LPP packet to dest_id
"""
pk = CRTPPacket()
pk.port = CRTPPort.LOCALIZATION
pk.channel = self.GENERIC_CH
pk.data = struct.pack('<BB', self.LPS_SHORT_LPP_PACKET, dest_id) + data
self._ed.send_packet(pk)
def send_emergency_stop(self):
"""
Send emergency stop
"""
pk = CRTPPacket()
pk.port = CRTPPort.LOCALIZATION
pk.channel = self.GENERIC_CH
pk.data = struct.pack('<B', self.EMERGENCY_STOP)
self._ed.send_packet(pk)
def send_emergency_reset(self):
"""
Send emergency reset
"""
pk = CRTPPacket()
pk.port = CRTPPort.LOCALIZATION
pk.channel = self.GENERIC_CH
pk.data = struct.pack('<B', self.EMERGENCY_RESET)
self._ed.send_packet(pk)
def send_emergency_stop_watchdog(self):
"""
Send emergency stop watchdog
"""
pk = CRTPPacket()
pk.port = CRTPPort.LOCALIZATION
pk.channel = self.GENERIC_CH
pk.data = struct.pack('<B', self.EMERGENCY_STOP_WATCHDOG)
self._ed.send_packet(pk)
@staticmethod
def quatdecompress(comp: int):
q = [0] * 4
sqrt1_2 = 0.70710678118654752440
mask = (1 << 9) - 1
i_largest = comp >> 30
sum_squares = 0
for i in range(3, -1, -1):
if (i != i_largest):
mag = comp & mask
negbit = (comp >> 9) & 0x1
comp = comp >> 10
q[i] = sqrt1_2 * mag / mask
if (negbit == 1):
q[i] = -q[i]
sum_squares += q[i] * q[i]
q[i_largest] = math.sqrt(1 - sum_squares)
return q
@staticmethod
def quatcompress(q: List[float]):
sqrt1_2 = 0.70710678118654752440
i_largest = 0
for i in range(1, 4):
if (math.fabs(q[i]) > math.fabs(q[i_largest])):
i_largest = i
negate = q[i_largest] < 0
comp = i_largest
for i in range(4):
if i != i_largest:
negbit = (q[i] < 0) ^ negate
mag = int(((1 << 9) - 1) * math.fabs(q[i]) / sqrt1_2 + 0.5)
comp = (comp << 10) | (negbit << 9) | mag
return comp
class CallerTest(unittest.TestCase):
def setUp(self):
self.callback_count = 0
self.sut = Caller()
def test_that_callback_is_added(self):
# Fixture
# Test
self.sut.add_callback(self._callback)
# Assert
self.sut.call()
self.assertEqual(1, self.callback_count)
def test_that_callback_is_added_only_one_time(self):
# Fixture
# Test
self.sut.add_callback(self._callback)
self.sut.add_callback(self._callback)
# Assert
self.sut.call()
self.assertEqual(1, self.callback_count)
def test_that_multiple_callbacks_are_added(self):
# Fixture
# Test
self.sut.add_callback(self._callback)
self.sut.add_callback(self._callback2)
# Assert
self.sut.call()
self.assertEqual(2, self.callback_count)
def test_that_callback_is_removed(self):
# Fixture
self.sut.add_callback(self._callback)
# Test
self.sut.remove_callback(self._callback)
# Assert
self.sut.call()
self.assertEqual(0, self.callback_count)
def test_that_callback_is_called_with_arguments(self):
# Fixture
self.sut.add_callback(self._callback_with_args)
# Test
self.sut.call('The token')
# Assert
self.assertEqual('The token', self.callback_token)
def _callback(self):
self.callback_count += 1
def _callback2(self):
self.callback_count += 1
def _callback_with_args(self, token):
self.callback_token = token