def _connected(self, link_uri):
""" This callback is called form the Espdrone API when a Espdrone
has been connected and the TOCs have been downloaded."""
print('Connected to %s' % link_uri)
# The definition of the logconfig can be made before connecting
self._lg_stab = LogConfig(name='Stabilizer', period_in_ms=10)
self._lg_stab.add_variable('stabilizer.roll', 'float')
self._lg_stab.add_variable('stabilizer.pitch', 'float')
self._lg_stab.add_variable('stabilizer.yaw', 'float')
# Adding the configuration cannot be done until a Espdrone is
# connected, since we need to check that the variables we
# would like to log are in the TOC.
try:
self._ed.log.add_config(self._lg_stab)
# This callback will receive the data
self._lg_stab.data_received_cb.add_callback(self._stab_log_data)
# This callback will be called on errors
self._lg_stab.error_cb.add_callback(self._stab_log_error)
# Start the logging
self._lg_stab.start()
except KeyError as e:
print('Could not start log configuration,'
'{} not found in TOC'.format(str(e)))
except AttributeError:
print('Could not add Stabilizer log config, bad configuration.')
# Start a timer to disconnect in 10s
t = Timer(10, self._ed.close_link)
t.start()
def createConfigFromSelection(self):
logconfig = LogConfig(str(self.configNameCombo.currentText()),
self.period)
for node in self.getNodeChildren(self.varTree.invisibleRootItem()):
parentName = node.text(NAME_FIELD)
for leaf in self.getNodeChildren(node):
varName = leaf.text(NAME_FIELD)
varType = str(leaf.text(CTYPE_FIELD))
completeName = "%s.%s" % (parentName, varName)
logconfig.add_variable(completeName, varType)
return logconfig
def assert_add_logging_and_get_non_zero_value(self, sed):
log_name = 'stabilizer.roll'
expected = 0.0
lg_conf = LogConfig(name='SysTest', period_in_ms=10)
lg_conf.add_variable(log_name, 'float')
with SyncLogger(sed, lg_conf) as logger:
for log_entry in logger:
actual = log_entry[1][log_name]
break
self.assertNotAlmostEqual(expected, actual, places=4)
def verify_writing_memory_data(self, mems, sed):
self.wrote_data = False
sed.ed.param.set_value('memTst.resetW', '1')
time.sleep(0.1)
mems[0].write_data(5, 1000, self._data_written)
while not self.wrote_data:
time.sleep(1)
log_conf = LogConfig(name='memtester', period_in_ms=100)
log_conf.add_variable('memTst.errCntW', 'uint32_t')
with SyncLogger(sed, log_conf) as logger:
for log_entry in logger:
errorCount = log_entry[1]['memTst.errCntW']
self.assertEqual(0, errorCount)
break
def _connected(self, link_uri):
lg = LogConfig("GPS", 1000)
lg.add_variable("gps.lat")
lg.add_variable("gps.lon")
lg.add_variable("gps.hAcc")
lg.add_variable("gps.hMSL")
lg.add_variable("gps.nsat")
self._cf.log.add_config(lg)
if lg.valid:
lg.data_received_cb.add_callback(self._log_data_signal.emit)
lg.error_cb.add_callback(self._log_error_signal.emit)
lg.start()
else:
logger.warning("Could not setup logging block for GPS!")
self._max_speed = 0.0
def start_position_printing(sed):
log_conf = LogConfig(name='Position', period_in_ms=500)
log_conf.add_variable('kalman.stateX', 'float')
log_conf.add_variable('kalman.stateY', 'float')
log_conf.add_variable('kalman.stateZ', 'float')
sed.ed.log.add_config(log_conf)
log_conf.data_received_cb.add_callback(position_callback)
log_conf.start()
def _set_available_sensors(self, name, available):
logger.info("[%s]: %s", name, available)
available = eval(available)
self.actualHeight.setEnabled(True)
self.helper.inputDeviceReader.set_alt_hold_available(available)
if not self.logBaro:
# The sensor is available, set up the logging
self.logBaro = LogConfig("Baro", 200)
self.logBaro.add_variable(LOG_NAME_ESTIMATED_Z, "float")
try:
self.helper.cf.log.add_config(self.logBaro)
self.logBaro.data_received_cb.add_callback(
self._baro_data_signal.emit)
self.logBaro.error_cb.add_callback(self._log_error_signal.emit)
self.logBaro.start()
except KeyError as e:
logger.warning(str(e))
except AttributeError as e:
logger.warning(str(e))
def _read_config_files(self):
"""Read and parse log configurations"""
configsfound = [
os.path.basename(f)
for f in glob.glob(edclient.config_path + "/log/[A-Za-z_-]*.json")
]
new_dsList = []
for conf in configsfound:
try:
logger.info("Parsing [%s]", conf)
json_data = open(edclient.config_path + "/log/%s" % conf)
self.data = json.load(json_data)
infoNode = self.data["logconfig"]["logblock"]
logConf = LogConfig(infoNode["name"], int(infoNode["period"]))
for v in self.data["logconfig"]["logblock"]["variables"]:
if v["type"] == "TOC":
logConf.add_variable(str(v["name"]), v["fetch_as"])
else:
logConf.add_variable("Mem", v["fetch_as"],
v["stored_as"],
int(v["address"], 16))
new_dsList.append(logConf)
json_data.close()
except Exception as e:
logger.warning("Exception while parsing logconfig file: %s", e)
self.dsList = new_dsList
def _handle_logging(self, data):
resp = {"version": 1}
if data["action"] == "create":
lg = LogConfig(data["name"], data["period"])
for v in data["variables"]:
lg.add_variable(v)
lg.started_cb.add_callback(self._logging_started)
lg.added_cb.add_callback(self._logging_added)
try:
lg.data_received_cb.add_callback(self._logdata_callback)
self._logging_configs[data["name"]] = lg
self._cf.log.add_config(lg)
lg.create()
self._log_added_queue.get(block=True, timeout=LOG_TIMEOUT)
resp["status"] = 0
except KeyError as e:
resp["status"] = 1
resp["msg"] = str(e)
except AttributeError as e:
resp["status"] = 2
resp["msg"] = str(e)
except queue.Empty:
resp["status"] = 3
resp["msg"] = "Log configuration did not start"
if data["action"] == "start":
try:
self._logging_configs[data["name"]].start()
self._log_started_queue.get(block=True, timeout=LOG_TIMEOUT)
resp["status"] = 0
except KeyError as e:
resp["status"] = 1
resp["msg"] = "{} config not found".format(str(e))
except queue.Empty:
resp["status"] = 2
resp["msg"] = "Log configuration did not stop"
if data["action"] == "stop":
try:
self._logging_configs[data["name"]].stop()
self._log_started_queue.get(block=True, timeout=LOG_TIMEOUT)
resp["status"] = 0
except KeyError as e:
resp["status"] = 1
resp["msg"] = "{} config not found".format(str(e))
except queue.Empty:
resp["status"] = 2
resp["msg"] = "Log configuration did not stop"
if data["action"] == "delete":
try:
self._logging_configs[data["name"]].delete()
self._log_added_queue.get(block=True, timeout=LOG_TIMEOUT)
resp["status"] = 0
except KeyError as e:
resp["status"] = 1
resp["msg"] = "{} config not found".format(str(e))
except queue.Empty:
resp["status"] = 2
resp["msg"] = "Log configuration did not stop"
return resp
def _connected(self):
self.uiState = UIState.CONNECTED
self._update_ui_state()
Config().set("link_uri", str(self._selected_interface))
lg = LogConfig("Battery", 1000)
lg.add_variable("pm.vbat", "float")
lg.add_variable("pm.state", "int8_t")
try:
self.cf.log.add_config(lg)
lg.data_received_cb.add_callback(self.batteryUpdatedSignal.emit)
lg.error_cb.add_callback(self._log_error_signal.emit)
lg.start()
except KeyError as e:
logger.warning(str(e))
mems = self.cf.mem.get_mems(MemoryElement.TYPE_DRIVER_LED)
if len(mems) > 0:
mems[0].write_data(self._led_write_done)
def _register_logblock(self,
logblock_name,
variables,
data_cb,
error_cb,
update_period=UPDATE_PERIOD_LOG):
"""Register log data to listen for. One logblock can contain a limited
number of parameters (6 for floats)."""
lg = LogConfig(logblock_name, update_period)
for variable in variables:
if self._is_in_log_toc(variable):
lg.add_variable('{}.{}'.format(variable[0], variable[1]),
variable[2])
self._helper.cf.log.add_config(lg)
lg.data_received_cb.add_callback(data_cb)
lg.error_cb.add_callback(error_cb)
lg.start()
return lg
def wait_for_position_estimator(sed):
print('Waiting for estimator to find position...')
log_config = LogConfig(name='Kalman Variance', period_in_ms=500)
log_config.add_variable('kalman.varPX', 'float')
log_config.add_variable('kalman.varPY', 'float')
log_config.add_variable('kalman.varPZ', 'float')
var_y_history = [1000] * 10
var_x_history = [1000] * 10
var_z_history = [1000] * 10
threshold = 0.001
with SyncLogger(sed, log_config) as logger:
for log_entry in logger:
data = log_entry[1]
var_x_history.append(data['kalman.varPX'])
var_x_history.pop(0)
var_y_history.append(data['kalman.varPY'])
var_y_history.pop(0)
var_z_history.append(data['kalman.varPZ'])
var_z_history.pop(0)
min_x = min(var_x_history)
max_x = max(var_x_history)
min_y = min(var_y_history)
max_y = max(var_y_history)
min_z = min(var_z_history)
max_z = max(var_z_history)
# print("{} {} {}".
# format(max_x - min_x, max_y - min_y, max_z - min_z))
if (max_x - min_x) < threshold and (
max_y - min_y) < threshold and (
max_z - min_z) < threshold:
break
def connected(self, URI):
print('We are now connected to {}'.format(URI))
# The definition of the logconfig can be made before connecting
lpos = LogConfig(name='Position', period_in_ms=100)
lpos.add_variable('stateEstimate.x')
lpos.add_variable('stateEstimate.y')
lpos.add_variable('stateEstimate.z')
try:
self.ed.log.add_config(lpos)
lpos.data_received_cb.add_callback(self.pos_data)
lpos.start()
except KeyError as e:
print('Could not start log configuration,'
'{} not found in TOC'.format(str(e)))
except AttributeError:
print('Could not add Position log config, bad configuration.')
lmeas = LogConfig(name='Meas', period_in_ms=100)
lmeas.add_variable('range.front')
lmeas.add_variable('range.back')
lmeas.add_variable('range.up')
lmeas.add_variable('range.left')
lmeas.add_variable('range.right')
lmeas.add_variable('range.zrange')
lmeas.add_variable('stabilizer.roll')
lmeas.add_variable('stabilizer.pitch')
lmeas.add_variable('stabilizer.yaw')
try:
self.ed.log.add_config(lmeas)
lmeas.data_received_cb.add_callback(self.meas_data)
lmeas.start()
except KeyError as e:
print('Could not start log configuration,'
'{} not found in TOC'.format(str(e)))
except AttributeError:
print('Could not add Measurement log config, bad configuration.')
edlib.crtp.init_drivers(enable_debug_driver=False)
parser = argparse.ArgumentParser()
parser.add_argument("--uri", help='The ip address of the drone, e.g. "192.168.0.102"')
args = parser.parse_args()
if args.uri:
uri = args.uri
else:
available = edlib.crtp.scan_interfaces()
print('Espdrones found:')
if available:
print(available[0])
uri = available[0][0]
else:
quit()
lg_stab = LogConfig(name='Stabilizer', period_in_ms=10)
lg_stab.add_variable('stabilizer.roll', 'float')
lg_stab.add_variable('stabilizer.pitch', 'float')
lg_stab.add_variable('stabilizer.yaw', 'float')
ed = Espdrone(rw_cache='./cache')
with SyncEspdrone(uri, ed=ed) as sed:
with SyncLogger(sed, lg_stab) as logger:
endTime = time.time() + 10
for log_entry in logger:
timestamp = log_entry[0]
data = log_entry[1]
logconf_name = log_entry[2]
print('[%d][%s]: %s' % (timestamp, logconf_name, data))
class FlightTab(Tab, flight_tab_class):
uiSetupReadySignal = pyqtSignal()
_motor_data_signal = pyqtSignal(int, object, object)
_imu_data_signal = pyqtSignal(int, object, object)
_baro_data_signal = pyqtSignal(int, object, object)
_input_updated_signal = pyqtSignal(float, float, float, float)
_rp_trim_updated_signal = pyqtSignal(float, float)
_emergency_stop_updated_signal = pyqtSignal(bool)
_assisted_control_updated_signal = pyqtSignal(bool)
_heighthold_input_updated_signal = pyqtSignal(float, float, float, float)
_hover_input_updated_signal = pyqtSignal(float, float, float, float)
_log_error_signal = pyqtSignal(object, str)
# UI_DATA_UPDATE_FPS = 10
connectionFinishedSignal = pyqtSignal(str)
disconnectedSignal = pyqtSignal(str)
_limiting_updated = pyqtSignal(bool, bool, bool)
def __init__(self, tabWidget, helper, *args):
super(FlightTab, self).__init__(*args)
self.setupUi(self)
self.tabName = "Flight Control"
self.menuName = "Flight Control"
self.tabWidget = tabWidget
self.helper = helper
self.disconnectedSignal.connect(self.disconnected)
self.connectionFinishedSignal.connect(self.connected)
# Incomming signals
self.helper.cf.connected.add_callback(
self.connectionFinishedSignal.emit)
self.helper.cf.disconnected.add_callback(self.disconnectedSignal.emit)
self._input_updated_signal.connect(self.updateInputControl)
self.helper.inputDeviceReader.input_updated.add_callback(
self._input_updated_signal.emit)
self._rp_trim_updated_signal.connect(self.calUpdateFromInput)
self.helper.inputDeviceReader.rp_trim_updated.add_callback(
self._rp_trim_updated_signal.emit)
self._emergency_stop_updated_signal.connect(self.updateEmergencyStop)
self.helper.inputDeviceReader.emergency_stop_updated.add_callback(
self._emergency_stop_updated_signal.emit)
self.helper.inputDeviceReader.heighthold_input_updated.add_callback(
self._heighthold_input_updated_signal.emit)
self._heighthold_input_updated_signal.connect(
self._heighthold_input_updated)
self.helper.inputDeviceReader.hover_input_updated.add_callback(
self._hover_input_updated_signal.emit)
self._hover_input_updated_signal.connect(self._hover_input_updated)
self.helper.inputDeviceReader.assisted_control_updated.add_callback(
self._assisted_control_updated_signal.emit)
self._assisted_control_updated_signal.connect(
self._assisted_control_updated)
self._imu_data_signal.connect(self._imu_data_received)
self._baro_data_signal.connect(self._baro_data_received)
self._motor_data_signal.connect(self._motor_data_received)
self._log_error_signal.connect(self._logging_error)
# Connect UI signals that are in this tab
self.flightModeCombo.currentIndexChanged.connect(self.flightmodeChange)
self.minThrust.valueChanged.connect(self.minMaxThrustChanged)
self.maxThrust.valueChanged.connect(self.minMaxThrustChanged)
self.thrustLoweringSlewRateLimit.valueChanged.connect(
self.thrustLoweringSlewRateLimitChanged)
self.slewEnableLimit.valueChanged.connect(
self.thrustLoweringSlewRateLimitChanged)
self.targetCalRoll.valueChanged.connect(self._trim_roll_changed)
self.targetCalPitch.valueChanged.connect(self._trim_pitch_changed)
self.maxAngle.valueChanged.connect(self.maxAngleChanged)
self.maxYawRate.valueChanged.connect(self.maxYawRateChanged)
self.uiSetupReadySignal.connect(self.uiSetupReady)
self.clientXModeCheckbox.toggled.connect(self.changeXmode)
self.isInCrazyFlightmode = False
self.uiSetupReady()
self.clientXModeCheckbox.setChecked(Config().get("client_side_xmode"))
self.espdroneXModeCheckbox.clicked.connect(
lambda enabled: self.helper.cf.param.set_value(
"flightmode.x", str(enabled)))
self.helper.cf.param.add_update_callback(
group="flightmode",
name="xmode",
cb=(lambda name, checked: self.espdroneXModeCheckbox.setChecked(
eval(checked))))
self.ratePidRadioButton.clicked.connect(
lambda enabled: self.helper.cf.param.set_value(
"flightmode.ratepid", str(enabled)))
self.angularPidRadioButton.clicked.connect(
lambda enabled: self.helper.cf.param.set_value(
"flightmode.ratepid", str(not enabled)))
self._led_ring_headlight.clicked.connect(
lambda enabled: self.helper.cf.param.set_value(
"ring.headlightEnable", str(enabled)))
self.helper.cf.param.add_update_callback(
group="flightmode",
name="ratepid",
cb=(lambda name, checked: self.ratePidRadioButton.setChecked(
eval(checked))))
self.helper.cf.param.add_update_callback(
group="cpu", name="flash", cb=self._set_enable_client_xmode)
self.helper.cf.param.add_update_callback(
group="ring",
name="headlightEnable",
cb=(lambda name, checked: self._led_ring_headlight.setChecked(
eval(checked))))
self._ledring_nbr_effects = 0
self.helper.cf.param.add_update_callback(group="ring",
name="effect",
cb=self._ring_effect_updated)
self.helper.cf.param.add_update_callback(
group="imu_sensors", cb=self._set_available_sensors)
self.helper.cf.param.all_updated.add_callback(self._all_params_updated)
self.logBaro = None
self.logAltHold = None
self.ai = AttitudeIndicator()
self.verticalLayout_4.addWidget(self.ai)
self.splitter.setSizes([1000, 1])
self.targetCalPitch.setValue(Config().get("trim_pitch"))
self.targetCalRoll.setValue(Config().get("trim_roll"))
self.helper.inputDeviceReader.alt1_updated.add_callback(
self.alt1_updated)
self.helper.inputDeviceReader.alt2_updated.add_callback(
self.alt2_updated)
self._tf_state = 0
self._ring_effect = 0
# Connect callbacks for input device limiting of rpöö/pitch/yaw/thust
self.helper.inputDeviceReader.limiting_updated.add_callback(
self._limiting_updated.emit)
self._limiting_updated.connect(self._set_limiting_enabled)
def _set_enable_client_xmode(self, name, value):
if eval(value) <= 128:
self.clientXModeCheckbox.setEnabled(True)
else:
self.clientXModeCheckbox.setEnabled(False)
self.clientXModeCheckbox.setChecked(False)
def _set_limiting_enabled(self, rp_limiting_enabled, yaw_limiting_enabled,
thrust_limiting_enabled):
self.maxAngle.setEnabled(rp_limiting_enabled)
self.targetCalRoll.setEnabled(rp_limiting_enabled)
self.targetCalPitch.setEnabled(rp_limiting_enabled)
self.maxYawRate.setEnabled(yaw_limiting_enabled)
self.maxThrust.setEnabled(thrust_limiting_enabled)
self.minThrust.setEnabled(thrust_limiting_enabled)
self.slewEnableLimit.setEnabled(thrust_limiting_enabled)
self.thrustLoweringSlewRateLimit.setEnabled(thrust_limiting_enabled)
def thrustToPercentage(self, thrust):
return ((thrust / MAX_THRUST) * 100.0)
def uiSetupReady(self):
flightComboIndex = self.flightModeCombo.findText(
Config().get("flightmode"), Qt.MatchFixedString)
if (flightComboIndex < 0):
self.flightModeCombo.setCurrentIndex(0)
self.flightModeCombo.currentIndexChanged.emit(0)
else:
self.flightModeCombo.setCurrentIndex(flightComboIndex)
self.flightModeCombo.currentIndexChanged.emit(flightComboIndex)
def _logging_error(self, log_conf, msg):
QMessageBox.about(
self, "Log error",
"Error when starting log config [%s]: %s" % (log_conf.name, msg))
def _motor_data_received(self, timestamp, data, logconf):
if self.isVisible():
self.actualM1.setValue(data["motor.m1"])
self.actualM2.setValue(data["motor.m2"])
self.actualM3.setValue(data["motor.m3"])
self.actualM4.setValue(data["motor.m4"])
def _baro_data_received(self, timestamp, data, logconf):
if self.isVisible():
estimated_z = data[LOG_NAME_ESTIMATED_Z]
self.actualHeight.setText(("%.2f" % estimated_z))
self.ai.setBaro(estimated_z, self.is_visible())
def _heighthold_input_updated(self, roll, pitch, yaw, height):
if (self.isVisible() and
(self.helper.inputDeviceReader.get_assisted_control()
== self.helper.inputDeviceReader.ASSISTED_CONTROL_HEIGHTHOLD)):
self.targetRoll.setText(("%0.2f deg" % roll))
self.targetPitch.setText(("%0.2f deg" % pitch))
self.targetYaw.setText(("%0.2f deg/s" % yaw))
self.targetHeight.setText(("%.2f m" % height))
self.ai.setHover(height, self.is_visible())
def _hover_input_updated(self, vx, vy, yaw, height):
if (self.isVisible()
and (self.helper.inputDeviceReader.get_assisted_control()
== self.helper.inputDeviceReader.ASSISTED_CONTROL_HOVER)):
self.targetRoll.setText(("%0.2f m/s" % vy))
self.targetPitch.setText(("%0.2f m/s" % vx))
self.targetYaw.setText(("%0.2f deg/s" % yaw))
self.targetHeight.setText(("%.2f m" % height))
self.ai.setHover(height, self.is_visible())
def _imu_data_received(self, timestamp, data, logconf):
if self.isVisible():
self.actualRoll.setText(("%.2f" % data["stabilizer.roll"]))
self.actualPitch.setText(("%.2f" % data["stabilizer.pitch"]))
self.actualYaw.setText(("%.2f" % data["stabilizer.yaw"]))
self.actualThrust.setText(
"%.2f%%" % self.thrustToPercentage(data["stabilizer.thrust"]))
self.ai.setRollPitch(-data["stabilizer.roll"],
data["stabilizer.pitch"], self.is_visible())
def connected(self, linkURI):
# IMU & THRUST
lg = LogConfig("Stabilizer", Config().get("ui_update_period"))
lg.add_variable("stabilizer.roll", "float")
lg.add_variable("stabilizer.pitch", "float")
lg.add_variable("stabilizer.yaw", "float")
lg.add_variable("stabilizer.thrust", "uint16_t")
try:
self.helper.cf.log.add_config(lg)
lg.data_received_cb.add_callback(self._imu_data_signal.emit)
lg.error_cb.add_callback(self._log_error_signal.emit)
lg.start()
except KeyError as e:
logger.warning(str(e))
except AttributeError as e:
logger.warning(str(e))
# MOTOR
lg = LogConfig("Motors", Config().get("ui_update_period"))
lg.add_variable("motor.m1")
lg.add_variable("motor.m2")
lg.add_variable("motor.m3")
lg.add_variable("motor.m4")
try:
self.helper.cf.log.add_config(lg)
lg.data_received_cb.add_callback(self._motor_data_signal.emit)
lg.error_cb.add_callback(self._log_error_signal.emit)
lg.start()
except KeyError as e:
logger.warning(str(e))
except AttributeError as e:
logger.warning(str(e))
def _set_available_sensors(self, name, available):
logger.info("[%s]: %s", name, available)
available = eval(available)
self.actualHeight.setEnabled(True)
self.helper.inputDeviceReader.set_alt_hold_available(available)
if not self.logBaro:
# The sensor is available, set up the logging
self.logBaro = LogConfig("Baro", 200)
self.logBaro.add_variable(LOG_NAME_ESTIMATED_Z, "float")
try:
self.helper.cf.log.add_config(self.logBaro)
self.logBaro.data_received_cb.add_callback(
self._baro_data_signal.emit)
self.logBaro.error_cb.add_callback(self._log_error_signal.emit)
self.logBaro.start()
except KeyError as e:
logger.warning(str(e))
except AttributeError as e:
logger.warning(str(e))
def disconnected(self, linkURI):
self.ai.setRollPitch(0, 0)
self.actualM1.setValue(0)
self.actualM2.setValue(0)
self.actualM3.setValue(0)
self.actualM4.setValue(0)
self.actualRoll.setText("")
self.actualPitch.setText("")
self.actualYaw.setText("")
self.actualThrust.setText("")
self.actualHeight.setText("")
self.targetHeight.setText("Not Set")
self.ai.setHover(0, self.is_visible())
self.targetHeight.setEnabled(False)
self.actualHeight.setEnabled(False)
self.clientXModeCheckbox.setEnabled(False)
self.logBaro = None
self.logAltHold = None
self._led_ring_effect.setEnabled(False)
self._led_ring_effect.clear()
try:
self._led_ring_effect.currentIndexChanged.disconnect(
self._ring_effect_changed)
except TypeError:
# Signal was not connected
pass
self._led_ring_effect.setCurrentIndex(-1)
self._led_ring_headlight.setEnabled(False)
try:
self._assist_mode_combo.currentIndexChanged.disconnect(
self._assist_mode_changed)
except TypeError:
# Signal was not connected
pass
self._assist_mode_combo.setEnabled(False)
self._assist_mode_combo.clear()
def minMaxThrustChanged(self):
self.helper.inputDeviceReader.min_thrust = self.minThrust.value()
self.helper.inputDeviceReader.max_thrust = self.maxThrust.value()
if (self.isInCrazyFlightmode is True):
Config().set("min_thrust", self.minThrust.value())
Config().set("max_thrust", self.maxThrust.value())
def thrustLoweringSlewRateLimitChanged(self):
self.helper.inputDeviceReader.thrust_slew_rate = (
self.thrustLoweringSlewRateLimit.value())
self.helper.inputDeviceReader.thrust_slew_limit = (
self.slewEnableLimit.value())
if (self.isInCrazyFlightmode is True):
Config().set("slew_limit", self.slewEnableLimit.value())
Config().set("slew_rate", self.thrustLoweringSlewRateLimit.value())
def maxYawRateChanged(self):
logger.debug("MaxYawrate changed to %d", self.maxYawRate.value())
self.helper.inputDeviceReader.max_yaw_rate = self.maxYawRate.value()
if (self.isInCrazyFlightmode is True):
Config().set("max_yaw", self.maxYawRate.value())
def maxAngleChanged(self):
logger.debug("MaxAngle changed to %d", self.maxAngle.value())
self.helper.inputDeviceReader.max_rp_angle = self.maxAngle.value()
if (self.isInCrazyFlightmode is True):
Config().set("max_rp", self.maxAngle.value())
def _trim_pitch_changed(self, value):
logger.debug("Pitch trim updated to [%f]" % value)
self.helper.inputDeviceReader.trim_pitch = value
Config().set("trim_pitch", value)
def _trim_roll_changed(self, value):
logger.debug("Roll trim updated to [%f]" % value)
self.helper.inputDeviceReader.trim_roll = value
Config().set("trim_roll", value)
def calUpdateFromInput(self, rollCal, pitchCal):
logger.debug("Trim changed on joystick: roll=%.2f, pitch=%.2f",
rollCal, pitchCal)
self.targetCalRoll.setValue(rollCal)
self.targetCalPitch.setValue(pitchCal)
def updateInputControl(self, roll, pitch, yaw, thrust):
self.targetRoll.setText(("%0.2f deg" % roll))
self.targetPitch.setText(("%0.2f deg" % pitch))
self.targetYaw.setText(("%0.2f deg/s" % yaw))
self.targetThrust.setText(
("%0.2f %%" % self.thrustToPercentage(thrust)))
self.thrustProgress.setValue(thrust)
def setMotorLabelsEnabled(self, enabled):
self.M1label.setEnabled(enabled)
self.M2label.setEnabled(enabled)
self.M3label.setEnabled(enabled)
self.M4label.setEnabled(enabled)
def emergencyStopStringWithText(self, text):
return ("<html><head/><body><p>"
"<span style='font-weight:600; color:#7b0005;'>{}</span>"
"</p></body></html>".format(text))
def updateEmergencyStop(self, emergencyStop):
if emergencyStop:
self.setMotorLabelsEnabled(False)
self.emergency_stop_label.setText(
self.emergencyStopStringWithText("Kill switch active"))
else:
self.setMotorLabelsEnabled(True)
self.emergency_stop_label.setText("")
def flightmodeChange(self, item):
Config().set("flightmode", str(self.flightModeCombo.itemText(item)))
logger.debug("Changed flightmode to %s",
self.flightModeCombo.itemText(item))
self.isInCrazyFlightmode = False
if (item == 0): # Normal
self.maxAngle.setValue(Config().get("normal_max_rp"))
self.maxThrust.setValue(Config().get("normal_max_thrust"))
self.minThrust.setValue(Config().get("normal_min_thrust"))
self.slewEnableLimit.setValue(Config().get("normal_slew_limit"))
self.thrustLoweringSlewRateLimit.setValue(
Config().get("normal_slew_rate"))
self.maxYawRate.setValue(Config().get("normal_max_yaw"))
if (item == 1): # Advanced
self.maxAngle.setValue(Config().get("max_rp"))
self.maxThrust.setValue(Config().get("max_thrust"))
self.minThrust.setValue(Config().get("min_thrust"))
self.slewEnableLimit.setValue(Config().get("slew_limit"))
self.thrustLoweringSlewRateLimit.setValue(
Config().get("slew_rate"))
self.maxYawRate.setValue(Config().get("max_yaw"))
self.isInCrazyFlightmode = True
if (item == 0):
newState = False
else:
newState = True
self.maxThrust.setEnabled(newState)
self.maxAngle.setEnabled(newState)
self.minThrust.setEnabled(newState)
self.thrustLoweringSlewRateLimit.setEnabled(newState)
self.slewEnableLimit.setEnabled(newState)
self.maxYawRate.setEnabled(newState)
def _assist_mode_changed(self, item):
mode = None
if (item == 0): # Altitude hold
mode = JoystickReader.ASSISTED_CONTROL_ALTHOLD
if (item == 1): # Position hold
mode = JoystickReader.ASSISTED_CONTROL_POSHOLD
if (item == 2): # Position hold
mode = JoystickReader.ASSISTED_CONTROL_HEIGHTHOLD
if (item == 3): # Position hold
mode = JoystickReader.ASSISTED_CONTROL_HOVER
self.helper.inputDeviceReader.set_assisted_control(mode)
Config().set("assistedControl", mode)
def _assisted_control_updated(self, enabled):
if self.helper.inputDeviceReader.get_assisted_control() == \
JoystickReader.ASSISTED_CONTROL_POSHOLD:
self.targetThrust.setEnabled(not enabled)
self.targetRoll.setEnabled(not enabled)
self.targetPitch.setEnabled(not enabled)
elif ((self.helper.inputDeviceReader.get_assisted_control()
== JoystickReader.ASSISTED_CONTROL_HEIGHTHOLD)
or (self.helper.inputDeviceReader.get_assisted_control()
== JoystickReader.ASSISTED_CONTROL_HOVER)):
self.targetThrust.setEnabled(not enabled)
self.targetHeight.setEnabled(enabled)
else:
self.helper.cf.param.set_value("flightmode.althold", str(enabled))
@pyqtSlot(bool)
def changeXmode(self, checked):
self.helper.cf.commander.set_client_xmode(checked)
Config().set("client_side_xmode", checked)
logger.info("Clientside X-mode enabled: %s", checked)
def alt1_updated(self, state):
if state:
new_index = (self._ring_effect + 1) % (self._ledring_nbr_effects +
1)
self.helper.cf.param.set_value("ring.effect", str(new_index))
def alt2_updated(self, state):
self.helper.cf.param.set_value("ring.headlightEnable", str(state))
def _all_params_updated(self):
#self._ring_populate_dropdown()#ESPLane2.0
self._populate_assisted_mode_dropdown()
# def _ring_populate_dropdown(self):
# try:
# nbr = int(self.helper.cf.param.values["ring"]["neffect"])
# current = int(self.helper.cf.param.values["ring"]["effect"])
# except KeyError:
# return
#
# # Used only in alt1_updated function
# self._ring_effect = current
# self._ledring_nbr_effects = nbr
#
# hardcoded_names = {0: "Off",
# 1: "White spinner",
# 2: "Color spinner",
# 3: "Tilt effect",
# 4: "Brightness effect",
# 5: "Color spinner 2",
# 6: "Double spinner",
# 7: "Solid color effect",
# 8: "Factory test",
# 9: "Battery status",
# 10: "Boat lights",
# 11: "Alert",
# 12: "Gravity",
# 13: "LED tab",
# 14: "Color fader",
# 15: "Link quality"}
#
# for i in range(nbr + 1):
# name = "{}: ".format(i)
# if i in hardcoded_names:
# name += hardcoded_names[i]
# else:
# name += "N/A"
# self._led_ring_effect.addItem(name, i)
#
# self._led_ring_effect.currentIndexChanged.connect(
# self._ring_effect_changed)
#
# self._led_ring_effect.setCurrentIndex(current)
# # if int(self.helper.cf.param.values["deck"]["bcLedRing"]) == 1:#ESPlane
# # self._led_ring_effect.setEnabled(True)
# # self._led_ring_headlight.setEnabled(True)
def _ring_effect_changed(self, index):
self._ring_effect = index
if index > -1:
i = self._led_ring_effect.itemData(index)
logger.info("Changed effect to {}".format(i))
# if i != int(self.helper.cf.param.values["ring"]["effect"]):#ESPlane
# self.helper.cf.param.set_value("ring.effect", str(i))
def _ring_effect_updated(self, name, value):
if self.helper.cf.param.is_updated:
self._led_ring_effect.setCurrentIndex(int(value))
def _populate_assisted_mode_dropdown(self):
self._assist_mode_combo.addItem("Altitude hold", 0)
self._assist_mode_combo.addItem("Position hold", 1)
self._assist_mode_combo.addItem("Height hold", 2)
self._assist_mode_combo.addItem("Hover", 3)
heightHoldPossible = False
hoverPossible = False
# if int(self.helper.cf.param.values["deck"]["bcZRanger"]) == 1:
# heightHoldPossible = True
# self.helper.inputDeviceReader.set_hover_max_height(1.0)
if int(self.helper.cf.param.values["deck"]["bcZRanger2"]) == 1:
heightHoldPossible = True
self.helper.inputDeviceReader.set_hover_max_height(2.0)
# if int(self.helper.cf.param.values["deck"]["bcFlow"]) == 1:
# heightHoldPossible = True
# hoverPossible = True
# self.helper.inputDeviceReader.set_hover_max_height(1.0)
# if int(self.helper.cf.param.values["deck"]["bcFlow2"]) == 1:
# heightHoldPossible = True
# hoverPossible = True
# self.helper.inputDeviceReader.set_hover_max_height(2.0)
if not heightHoldPossible:
self._assist_mode_combo.model().item(2).setEnabled(False)
else:
self._assist_mode_combo.model().item(0).setEnabled(False)
if not hoverPossible:
self._assist_mode_combo.model().item(3).setEnabled(False)
else:
self._assist_mode_combo.model().item(0).setEnabled(False)
self._assist_mode_combo.currentIndexChanged.connect(
self._assist_mode_changed)
self._assist_mode_combo.setEnabled(True)
try:
assistmodeComboIndex = Config().get("assistedControl")
if assistmodeComboIndex == 3 and not hoverPossible:
self._assist_mode_combo.setCurrentIndex(0)
self._assist_mode_combo.currentIndexChanged.emit(0)
elif assistmodeComboIndex == 0 and hoverPossible:
self._assist_mode_combo.setCurrentIndex(3)
self._assist_mode_combo.currentIndexChanged.emit(3)
elif assistmodeComboIndex == 2 and not heightHoldPossible:
self._assist_mode_combo.setCurrentIndex(0)
self._assist_mode_combo.currentIndexChanged.emit(0)
elif assistmodeComboIndex == 0 and heightHoldPossible:
self._assist_mode_combo.setCurrentIndex(2)
self._assist_mode_combo.currentIndexChanged.emit(2)
else:
self._assist_mode_combo.setCurrentIndex(assistmodeComboIndex)
self._assist_mode_combo.currentIndexChanged.emit(
assistmodeComboIndex)
except KeyError:
defaultOption = 0
if hoverPossible:
defaultOption = 3
elif heightHoldPossible:
defaultOption = 2
self._assist_mode_combo.setCurrentIndex(defaultOption)
self._assist_mode_combo.currentIndexChanged.emit(defaultOption)
def connected(self, linkURI):
# IMU & THRUST
lg = LogConfig("Stabilizer", Config().get("ui_update_period"))
lg.add_variable("stabilizer.roll", "float")
lg.add_variable("stabilizer.pitch", "float")
lg.add_variable("stabilizer.yaw", "float")
lg.add_variable("stabilizer.thrust", "uint16_t")
try:
self.helper.cf.log.add_config(lg)
lg.data_received_cb.add_callback(self._imu_data_signal.emit)
lg.error_cb.add_callback(self._log_error_signal.emit)
lg.start()
except KeyError as e:
logger.warning(str(e))
except AttributeError as e:
logger.warning(str(e))
# MOTOR
lg = LogConfig("Motors", Config().get("ui_update_period"))
lg.add_variable("motor.m1")
lg.add_variable("motor.m2")
lg.add_variable("motor.m3")
lg.add_variable("motor.m4")
try:
self.helper.cf.log.add_config(lg)
lg.data_received_cb.add_callback(self._motor_data_signal.emit)
lg.error_cb.add_callback(self._log_error_signal.emit)
lg.start()
except KeyError as e:
logger.warning(str(e))
except AttributeError as e:
logger.warning(str(e))
logging.basicConfig(level=logging.ERROR)
if __name__ == '__main__':
# Initialize the low-level drivers (don't list the debug drivers)
edlib.crtp.init_drivers(enable_debug_driver=False)
# Scan for Espdrones and use the first one found
print('Scanning interfaces for Espdrones...')
available = edlib.crtp.scan_interfaces()
print('Espdrones found:')
for i in available:
print(i[0])
if len(available) == 0:
print('No Espdrones found, cannot run example')
else:
lg_stab = LogConfig(name='StateEstimate', period_in_ms=10)
lg_stab.add_variable('stateEstimate.x', 'float')
lg_stab.add_variable('stateEstimate.y', 'float')
lg_stab.add_variable('stateEstimate.z', 'float')
ed = Espdrone(rw_cache='./cache')
with SyncEspdrone(available[0][0], ed=ed) as sed:
with SyncLogger(sed, lg_stab) as logger:
endTime = time.time() + 10
update_time = current_time = time.time()
for log_entry in logger:
timestamp = log_entry[0]
data = log_entry[1]
logconf_name = log_entry[2]
class LoggingExample:
"""
Simple logging example class that logs the Stabilizer from a supplied
link uri and disconnects after 10s.
"""
def __init__(self, link_uri):
""" Initialize and run the example with the specified link_uri """
self._ed = Espdrone(rw_cache='./cache')
# Connect some callbacks from the Espdrone API
self._ed.connected.add_callback(self._connected)
self._ed.disconnected.add_callback(self._disconnected)
self._ed.connection_failed.add_callback(self._connection_failed)
self._ed.connection_lost.add_callback(self._connection_lost)
print('Connecting to %s' % link_uri)
# Try to connect to the Espdrone
self._ed.open_link(link_uri)
# Variable used to keep main loop occupied until disconnect
self.is_connected = True
def _connected(self, link_uri):
""" This callback is called form the Espdrone API when a Espdrone
has been connected and the TOCs have been downloaded."""
print('Connected to %s' % link_uri)
# The definition of the logconfig can be made before connecting
self._lg_stab = LogConfig(name='Stabilizer', period_in_ms=10)
self._lg_stab.add_variable('stabilizer.roll', 'float')
self._lg_stab.add_variable('stabilizer.pitch', 'float')
self._lg_stab.add_variable('stabilizer.yaw', 'float')
# Adding the configuration cannot be done until a Espdrone is
# connected, since we need to check that the variables we
# would like to log are in the TOC.
try:
self._ed.log.add_config(self._lg_stab)
# This callback will receive the data
self._lg_stab.data_received_cb.add_callback(self._stab_log_data)
# This callback will be called on errors
self._lg_stab.error_cb.add_callback(self._stab_log_error)
# Start the logging
self._lg_stab.start()
except KeyError as e:
print('Could not start log configuration,'
'{} not found in TOC'.format(str(e)))
except AttributeError:
print('Could not add Stabilizer log config, bad configuration.')
# Start a timer to disconnect in 10s
t = Timer(10, self._ed.close_link)
t.start()
def _stab_log_error(self, logconf, msg):
"""Callback from the log API when an error occurs"""
print('Error when logging %s: %s' % (logconf.name, msg))
def _stab_log_data(self, timestamp, data, logconf):
"""Callback froma the log API when data arrives"""
print('[%d][%s]: %s' % (timestamp, logconf.name, data))
def _connection_failed(self, link_uri, msg):
"""Callback when connection initial connection fails (i.e no Espdrone
at the speficied address)"""
print('Connection to %s failed: %s' % (link_uri, msg))
self.is_connected = False
def _connection_lost(self, link_uri, msg):
"""Callback when disconnected after a connection has been made (i.e
Espdrone moves out of range)"""
print('Connection to %s lost: %s' % (link_uri, msg))
def _disconnected(self, link_uri):
"""Callback when the Espdrone is disconnected (called in all cases)"""
print('Disconnected from %s' % link_uri)
self.is_connected = False
def _create_log_config(self, rate_ms):
log_config = LogConfig('multiranger', rate_ms)
log_config.add_variable(self.FRONT)
log_config.add_variable(self.BACK)
log_config.add_variable(self.LEFT)
log_config.add_variable(self.RIGHT)
log_config.add_variable(self.UP)
log_config.add_variable(self.DOWN)
log_config.data_received_cb.add_callback(self._data_received)
return log_config