def start(self, cf):
self._lc_stab = LogConfig(name="Log-Stab", period_in_ms=50)
self._lc_stab.add_variable("stabilizer.roll", "float")
self._lc_stab.add_variable("stabilizer.pitch", "float")
self._lc_stab.add_variable("stabilizer.yaw", "float")
self._lc_stab.add_variable("stabilizer.thrust", "float")
self._lc_motor = LogConfig(name="Log-Motor", period_in_ms=50)
self._lc_motor.add_variable("pm.vbat", "float")
self._lc_motor.add_variable("motor.m1", "float") # Front (green)
self._lc_motor.add_variable("motor.m2", "float") # Right
self._lc_motor.add_variable("motor.m3", "float") # Back (red)
self._lc_motor.add_variable("motor.m4", "float") # Left
cf.log.add_config(self._lc_stab)
cf.log.add_config(self._lc_motor)
if self._lc_stab.valid and self._lc_motor.valid:
self._lc_stab.data_received_cb.add_callback(self._log_data)
self._lc_stab.error_cb.add_callback(self._log_error)
self._lc_stab.start()
self._lc_motor.data_received_cb.add_callback(self._log_data)
self._lc_motor.error_cb.add_callback(self._log_error)
self._lc_motor.start()
logger.info("Starting CFLog")
else:
logger.error("Could not add logconfig since some variables are not in TOC")
def _read_config_files(self):
"""Read and parse log configurations"""
configsfound = [os.path.basename(f) for f in
glob.glob(sys.path[1] + "/log/[A-Za-z_-]*.json")]
new_dsList = []
for conf in configsfound:
try:
logger.info("Parsing [%s]", conf)
json_data = open(sys.path[1] + "/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 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 _connected(self, linkURI):
self._update_ui_state(UIState.CONNECTED, linkURI)
Config().set("link_uri", str(linkURI))
lg = LogConfig("Battery", 1000)
lg.add_variable("pm.vbat", "float")
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))
def connectionDone(self, linkURI):
self.setUIState(UIState.CONNECTED, linkURI)
GuiConfig().set("link_uri", linkURI)
lg = LogConfig("Battery", 1000)
lg.add_variable("pm.vbat", "float")
self.cf.log.add_config(lg)
if lg.valid:
lg.data_received_cb.add_callback(self.batteryUpdatedSignal.emit)
lg.error_cb.add_callback(self._log_error_signal.emit)
lg.start()
else:
logger.warning("Could not setup loggingblock!")
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 _connected(self, link_uri):
lg = LogConfig("GPS", 1000)
lg.add_variable("gps.lat", "float")
lg.add_variable("gps.long", "float")
lg.add_variable("gps.alt", "float")
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!")
def makeLog(self):
""" Makes a log. All ative children are added """
#print " -> Making new Log with %d ms interval"
if self.lg:
print " ---> Previous Log detected, removing first"
self.lg.delete()
self.fm = FreqMonitor(window=max(10, int(1.2*float(self.text(2)))))
self.lg = LogConfig(self.name, 1000/int(float(self.text(2))))
#print " ---> Adding children to new log"
for x in range(self.childCount()):
c = self.child(x)
if c.isChecked():
name = c.log.group+"."+c.log.name
self.lg.add_variable(name, c.log.ctype)
#print " --- --> Adding child[%d]: [%s] to new log"%(x, name)
#print " ---> Checking log with TOC"
self.treeWidget().cf.log.add_config(self.lg)
if self.lg.valid:
#print " --- --> PASS"
self.lg.data_received_cb.add_callback(self.logDataCB)
self.lg.started_cb.add_callback(self.logStartedCB)
self.lg.error_cb.add_callback(self.treeWidget().sig_logError.emit)
self.lg.error_cb.add_callback(self.errorLog)
#print " --- --> callbacks added, starting new log NOW"
self.lg.start()
else:
#print " --- --> FAIL"
self.errorLog(None, "Invalid Config")
self.lg = None
def _connected(self, link_uri):
""" This callback is called form the Crazyflie API when a Crazyflie
has been connected and the TOCs have been downloaded."""
print "Connected to %s" % link_uri
self._cf.commander.send_setpoint(0, 0, 0, 20000)
# The definition of the logconfig can be made before connecting
self._lg_stab = LogConfig(name="Logger", 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")
self._lg_stab.add_variable("stabilizer.thrust", "uint16_t")
self._lg_stab.add_variable("gyro.x", "float")
self._lg_stab.add_variable("gyro.y", "float")
self._lg_stab.add_variable("gyro.z", "float")
# Adding the configuration cannot be done until a Crazyflie is
# connected, since we need to check that the variables we
# would like to log are in the TOC.
self._cf.log.add_config(self._lg_stab)
if self._lg_stab.valid:
# 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()
else:
print("Could not add logconfig since some variables are not in TOC")
def _connected(self, link_uri):
""" This callback is called form the Crazyflie API when a Crazyflie
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 Crazyflie is
# connected, since we need to check that the variables we
# would like to log are in the TOC.
try:
self._cf.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(5, self._cf.close_link)
t.start()
def _connected(self, link_uri):
#initial time to be shown in the screen
self.init_time = time.time()
#Threat 1 that will control the quadridrone
Thread(target=self._ramp_motors).start()
#Thread 2 that will show results in the screen
Thread(target=self._show_values).start()
self._lg_stab = LogConfig(name='Stabilizer', period_in_ms=30)
#self._lg_stab.addVariable(LogVariable("stabilizer.thrust", "uint16_t"))
self._lg_stab.add_variable('stabilizer.roll', 'float')
self._lg_stab.add_variable('stabilizer.pitch', 'float')
self._lg_stab.add_variable('stabilizer.yaw', 'float')
self._lg_stab.add_variable('stabilizer.thrust', "uint16_t")
# Adding the configuration cannot be done until a Crazyflie is
# connected, since we need to check that the variables we
# would like to log are in the TOC.
try:
self._cf.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)
#Thread 3 that will read from the TOC table, and 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.')
def _connected(self, linkURI):
self._update_ui_state(UIState.CONNECTED, linkURI)
Config().set("link_uri", str(linkURI))
lg = LogConfig("Battery", 1000)
lg.add_variable("pm.vbat", "float")
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))
mem = self.cf.mem.get_mems(MemoryElement.TYPE_DRIVER_LED)[0]
mem.write_data(self._led_write_done)
def __init__(self, link_uri):
""" Initialize and run the example with the specified link_uri """
self._cf = Crazyflie()
self._cf.connected.add_callback(self._connected)
self._cf.disconnected.add_callback(self._disconnected)
self._cf.connection_failed.add_callback(self._connection_failed)
self._cf.connection_lost.add_callback(self._connection_lost)
self._lg_stab = LogConfig(name="Logger", period_in_ms=10)
self._lg_stab.add_variable('acc.x', "float")
self._lg_stab.add_variable('acc.y', "float")
self._lg_stab.add_variable('acc.zw', "float")
self._lg_stab.add_variable('gyro.x', "float")
self._lg_stab.add_variable('gyro.y', "float")
self._lg_stab.add_variable('gyro.z', "float")
# PID for Z velocity??
# self._lg_stab.add_variable('acc.z', "float")
# self._lg_stab.add_variable("", "float")
self._cf.open_link(link_uri)
print("Connecting to %s" % link_uri)
self._acc_x = 0.0
self._acc_y = 0.0
self._acc_zw = 0.0
self._gyro_x = 0.0
self._gyro_y = 0.0
self._gyro_z = 0.0
# self._acc_z = 0.0
# self._vel_z = 0.0
# ROLL/PITCH
maxangle = 0.25
kpangle = 2.0
kiangle = 0.0
kdangle = 0.1
self._acc_pid_x = pid.PID(
0, 0, kpangle, kiangle, kdangle, -maxangle, maxangle)
self._acc_pid_y = pid.PID(
0, 0, kpangle, kiangle, kdangle, -maxangle, maxangle)
self._acc_pid_z = pid.PID(0, 0, 10, 0.0005, 0.1, 1/6, 2)
self._gyro_x_pid = pid.PID(
0, 0, kpangle, kiangle, kdangle, -maxangle, maxangle)
self._gyro_y_pid = pid.PID(
0, 0, kpangle, kiangle, kdangle, -maxangle, maxangle)
# self._gyro_z_pid = pid.PID(
# 0, 0, kpangle, kiangle, kdangle, -maxangle, maxangle)
self._is_connected = True
# self._acc_log = []
self.exit = False
def start_position_printing(scf):
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')
scf.cf.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)
if ("HMC5883L" in name):
if (not available):
self.actualASL.setText("N/A")
self.actualASL.setEnabled(False)
else:
self.actualASL.setEnabled(True)
self.helper.inputDeviceReader.set_alt_hold_available(available)
if (not self.logBaro and not self.logAltHold):
# The sensor is available, set up the logging
self.logBaro = LogConfig("Baro", 200)
self.logBaro.add_variable(PARAM_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))
self.logAltHold = LogConfig("AltHold", 200)
self.logAltHold.add_variable(PARAM_NAME_ALT_HOLD_TARGET,
"float")
try:
self.helper.cf.log.add_config(self.logAltHold)
self.logAltHold.data_received_cb.add_callback(
self._althold_data_signal.emit)
self.logAltHold.error_cb.add_callback(
self._log_error_signal.emit)
self.logAltHold.start()
except KeyError as e:
logger.warning(str(e))
except AttributeError:
logger.warning(str(e))
def _set_available_sensors(self, name, available):
logger.info("[%s]: %s", name, available)
available = eval(available)
if ("HMC5883L" in name):
if (not available):
self.maxAltitudeLabel.setEnabled(False)
self.maxAltitude.setEnabled(False)
self.actualASL.setText("N/A")
self.actualASL.setEnabled(False)
self.checkBox_ffbaro.setEnabled(False)
self.checkBox_ffbaro.setChecked(False)
else:
self.actualASL.setEnabled(True)
self.checkBox_ffbaro.setEnabled(True)
self.helper.inputDeviceReader.setAltHoldAvailable(available)
if (not self.logBaro and not self.logAltHold):
# The sensor is available, set up the logging
self.logBaro = LogConfig("Baro", 200)
self.logBaro.add_variable("baro.aslLong", "float")
self.helper.cf.log.add_config(self.logBaro)
if self.logBaro.valid:
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()
else:
logger.warning("Could not setup logconfiguration after "
"connection!")
self.logAltHold = LogConfig("AltHold", 200)
self.logAltHold.add_variable("altHold.target", "float")
self.helper.cf.log.add_config(self.logAltHold)
if self.logAltHold.valid:
self.logAltHold.data_received_cb.add_callback(self._althold_data_signal.emit)
self.logAltHold.error_cb.add_callback(self._log_error_signal.emit)
self.logAltHold.start()
else:
logger.warning("Could not setup logconfiguration after "
"connection!")
def wait_for_position_estimator(scf):
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(scf, 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 setup_cf(self):
"""
Sets up the CrazyFlie before running the experiment. This includes configuring some params to default values
and requesting the CrazyFlie to log certain variables back at constant intervals.
Doesn't return anything, but raises exceptions if anything goes wrong.
"""
try: # Send some default values for CF params
self.crazyflie.param.set_value('controller.tiltComp', '{:d}'.format(True))
self.crazyflie.param.set_value('flightmode.poshold', '{:d}'.format(False)) # Disable poshold and althold by default
self.crazyflie.param.set_value('flightmode.althold', '{:d}'.format(False))
self.crazyflie.param.set_value('flightmode.posSet', '{:d}'.format(False))
self.crazyflie.param.set_value('flightmode.yawMode', '0')
self.crazyflie.param.set_value('flightmode.timeoutStab', '{:d}'.format(1000*60*10)) # Stabilize (rpy=0) CF if doesn't receive a radio command in 10min
self.crazyflie.param.set_value('flightmode.timeoutShut', '{:d}'.format(1000*60*20)) # Shutdown CF if doesn't receive a radio command in 20min
self.crazyflie.param.set_value('posCtlPid.thrustBase', '{}'.format(self.TAKEOFF_THRUST))
self.crazyflie.param.set_value("ring.effect", "1") # Turn off LED ring
self.crazyflie.param.set_value("ring.headlightEnable", "0") # Turn off LED headlight
except Exception as e:
raise Exception("Couldn't initialize CrazyFlie params to their desired values. Details: {}".format(e.message))
# Create a log configuration and include all variables that want to be logged
self.cf_log = LogConfig(name="cf_log", period_in_ms=10)
self.cf_log.add_variable("stabilizer.roll", "float")
self.cf_log.add_variable("stabilizer.pitch", "float")
self.cf_log.add_variable("stabilizer.yaw", "float")
self.cf_log.add_variable("posEstimatorAlt.estimatedZ", "float")
self.cf_log.add_variable("posEstimatorAlt.velocityZ", "float")
try:
self.crazyflie.log.add_config(self.cf_log) # Validate the log configuration and attach it to our CF
except Exception as e:
raise Exception("Couldn't attach the log config to the CrazyFlie, bad configuration. Details: {}".format(e.message))
self.cf_log.data_received_cb.add_callback(self.on_cf_log_new_data) # Register appropriate callbacks
self.cf_log.error_cb.add_callback(self.on_cf_log_error)
self.cf_log.start() # Start logging!
# Get the CF's initial yaw (should be facing the camera) so we can have PID_yaw maintain that orientation
if self.ASK_FOR_TARGET_YAW: # Either ask the user to press Enter to indicate the CF's orientation is ready
raw_input("\nRotate the drone so it faces the camera, press Enter when you're ready...\n")
else: # Or automatically detect the first yaw log packet and set the current orientation as the desired yaw
while abs(self.cf_yaw) < 0.01: # Wait until first cf_yaw value is received (cf_yaw=0 by default)
time.sleep(0.1)
self.cf_PID_yaw.SetPoint = self.cf_yaw
print "Target yaw set at {:.2f}.".format(self.cf_yaw)
self.crazyflie.add_port_callback(CRTPPort.CONSOLE, self.print_cf_console)
self.crazyflie.commander.send_setpoint(0, 0, 0, 0) # New firmware version requires to send thrust=0 at least once to "unlock thrust"
def __init__(self, link_uri):
""" Initialize and run the example with the specified link_uri """
self._cf = Crazyflie()
self._cf.connected.add_callback(self._connected)
self._cf.disconnected.add_callback(self._disconnected)
self._cf.connection_failed.add_callback(self._connection_failed)
self._cf.connection_lost.add_callback(self._connection_lost)
self._asl = 0
self._thrust = 0
self.exit = False
self._logger = LogConfig(name='Logger', period_in_ms=10)
self._logger.add_variable("baro.asl", "float")
self._logger.add_variable("stabilizer.thrust", "float")
self._cf.open_link(link_uri)
print("Connecting to %s" % link_uri)
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 _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 __init__(self, link_uri):
self._cf = Crazyflie()
self._cf.connected.add_callback(self._connected)
self._cf.disconnected.add_callback(self._disconnected)
self._cf.connection_failed.add_callback(self._connection_failed)
self._cf.connection_lost.add_callback(self._connection_lost)
self._logger = LogConfig(name='Logger', period_in_ms=10)
self._logger.add_variable('acc.x', 'float')
self._logger.add_variable('acc.y', 'float')
self._logger.add_variable('acc.z', 'float')
self._logger.add_variable('acc.zw', 'float')
self._acc_x = 0
self._acc_y = 0
self._acc_z = 0
self._acc_zw = 0.0001
self.log = []
self.pidLog = []
self.acc_pid_x = None
self.acc_pid_y = None
self.acc_pid_z = None
self.vel_pid_x = None
self.vel_pid_y = None
self.vel_pid_z = None
print("Connecting to %s" % link_uri)
self._cf.open_link(link_uri)
self.is_connected = True
self.exit = False
self.init = False
Thread(target=self._exit_task).start()
Thread(target=self._run_task).start()
def wait_for_position_estimator(scf):
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.0008 #0.001
with SyncLogger(scf, 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 _wait_for_position_estimator(self):
"""Waits until the position estimator reports a consistent location after resetting."""
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(self.scf, 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)
if (max_x - min_x) < threshold and (
max_y - min_y) < threshold and (max_z -
min_z) < threshold:
break
def _connected(self, link_uri):
"""Callback when the Crazyflie has been connected"""
logger.debug("Crazyflie connected to {}".format(link_uri))
# self.pub = rospy.Publisher('chatter', String, queue_size=10)
# rospy.init_node('talker', anonymous=True, disable_signals=False)
# # rate = rospy.Rate(10)
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._log_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 crazyFlieloop(self):
#wall_follower = WallFollowerController()
#wall_follower.init(0.5)
twist = Twist()
#log_config.data_received_cb.add_callback(self.data_received)
cflib.crtp.init_drivers(enable_debug_driver=False)
cf = Crazyflie(rw_cache='./cache')
lg_states = LogConfig(name='kalman_states', period_in_ms=100)
lg_states.add_variable('stabilizer.yaw')
lg_states.add_variable('kalman_states.ox')
lg_states.add_variable('kalman_states.oy')
lg_states.add_variable('updown_laser.back_range')
lg_states.add_variable('updown_laser.front_range')
lg_states.add_variable('updown_laser.left_range')
lg_states.add_variable('updown_laser.right_range')
lg_states.data_received_cb.add_callback(self.data_received)
''' lg_states.add_variable('rssiCR.rssi')
lg_states.add_variable('rssiCR.distance')
lg_states.add_variable('rssiCR.pos_x')
lg_states.add_variable('rssiCR.pos_y')'''
fh = open("log_test.txt", "w")
with SyncCrazyflie(self.URI, cf=cf) as scf:
with MotionCommander(scf,0.3) as motion_commander:
with MultiRanger(scf) as multi_ranger:
with Stabilization(scf) as stabilization:
with SyncLogger(scf, lg_states) as logger_states:
bug_controller = WallFollowerController()
bug_controller.init(0.7,0.5)
keep_flying = True
time.sleep(1)
# param_name = "rssiCR.start"
# param_value = "1"
# cf.param.set_value(param_name, param_value)
twist.linear.x = 0.2
twist.linear.y = 0.0
twist.angular.z = 0
heading_prev = 0.0
heading = 0.0
angle_to_goal = 0.0;
kalman_x = 0.0
kalman_y = 0.0
already_reached_far_enough = False
state = "TURN_TO_GOAL"
prev_heading = stabilization.heading
angle_outbound = stabilization.heading;
state_start_time =0
rssi = 0
distance = 0
first_run = True
x_0 = 0
y_0 = 0
state_WF="FORWARD"
while keep_flying:
# crazyflie related stuff
for log_entry_1 in logger_states:
data = log_entry_1[1]
heading = math.radians(float(data["stabilizer.yaw"]));
pos_x =float(data["kalman_states.ox"])-0.5
pos_y =float(data["kalman_states.oy"])-1.5
kalman_x = float(data["kalman_states.ox"])
kalman_y = float(data["kalman_states.oy"])
#rssi = float(data["rssiCR.rssi"])
#distance = float(data["rssiCR.distance"])
if first_run is True:
x_0 = kalman_x
y_0 = kalman_y
first_run = False
if pos_x == 0:
pos_x = 0.02
if pos_y == 0:
pos_y = 0.02
rel_coord_x = self.goal_coord_x[self.coord_index]-(kalman_x-x_0);
rel_coord_y = self.goal_coord_y[self.coord_index]-(kalman_y-y_0);
self.distance_to_goal = math.sqrt(math.pow(rel_coord_x,2)+math.pow(rel_coord_y,2))
self.angle_to_goal = wraptopi(np.arctan2(rel_coord_y,rel_coord_x))
break
time.sleep(0.1)
#Handle state transitions
if state =="STATE_MACHINE":
if self.distance_to_goal<1.0 and self.coord_index is not len(self.goal_coord_x)-1:
state = self.transition("TURN_TO_GOAL")
self.coord_index = self.coord_index + 1
self.distance_to_goal = 10
self.angle_to_goal = 10
if self.distance_to_goal<1.0 and self.coord_index is len(self.goal_coord_x)-1:
state = self.transition("STOP")
if state =="TURN_TO_GOAL":
if time.time()-self.state_start_time > 1 and logicIsCloseTo(stabilization.heading,wraptopi(self.angle_to_goal),0.1):
state = self.transition("STATE_MACHINE")
#Handle state actions
if state=="STATE_MACHINE":
#twist = bug_controller.stateMachine(multi_ranger.front,multi_ranger.right,multi_ranger.left,stabilization.heading,wraptopi(self.angle_to_goal),self.distance_to_goal)
print("front_range",self.front_range)
twist,state_WF = bug_controller.stateMachine(self.front_range,self.left_range,self.right_range,stabilization.heading,wraptopi(self.angle_to_goal),self.distance_to_goal)
twist.linear.x = 0.5;
twist.linear.y = 0.0;
circumference = 2 * 0.5 * math.pi
rate = 2*math.pi * 0.5 / circumference
twist.angular.z = rate;
if state =="TURN_TO_GOAL":
twist.linear.x = 0.0;
twist.linear.y = 0.0;
twist.angular.z = 0.6;
if state =="STOP":
twist.linear.x = 0.0;
twist.linear.y = 0.0;
twist.angular.z = 0.0;
keep_flying = False
if self.front_range == None:
self.front_range = 8
if self.back_range == None:
self.back_range = 8
if self.left_range == None:
self.left_range = 8
if self.right_range == None:
self.right_range = 8
fh.write("%f, %f, %f, %f, %s\n"% (self.front_range,self.back_range,self.left_range,self.right_range,state_WF))
''' if len(pos)>0:
fh.write("%f, %f, %f, %f, %f, %f, %f, %f, %f, %f, %f, %f, %f\n"% (twist.linear.x, -1*math.degrees(twist.angular.z), self.distance_to_goal,self.angle_to_goal, stabilization.heading, kalman_x, kalman_y, pos_x, pos_y,pos[0],pos[2],rssi,distance))
else:
fh.write("%f, %f, %f, %f, %f, %f, %f, %f, %f, 0, 0, %f, %f\n"% (twist.linear.x, -1*math.degrees(twist.angular.z), self.distance_to_goal,self.angle_to_goal, stabilization.heading, kalman_x, kalman_y, pos_x, pos_y,rssi,distance))'''
print(state)
motion_commander._set_vel_setpoint(twist.linear.x,twist.linear.y,0,-1*math.degrees(twist.angular.z))
'''
if self.back_range is not None :
if self.back_range < 0.2:
print("up range is activated")
keep_flying = False'''
motion_commander.stop()
print("demo terminated")
fh.close()
class SwarmTab(Tab, example_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)
_plotter_log_error_signal = pyqtSignal(object, str)
_log_data_signal = pyqtSignal(int, object, object)
_disconnected_signal = pyqtSignal(str)
_connected_signal = pyqtSignal(str)
colors = [
(60, 200, 60), # green
(40, 100, 255), # blue
(255, 130, 240), # magenta
(255, 26, 28), # red
(255, 170, 0), # orange
(40, 180, 240), # cyan
(153, 153, 153), # grey
(176, 96, 50), # brown
(180, 60, 240), # purple
]
# UI_DATA_UPDATE_FPS = 10
connectionFinishedSignal = pyqtSignal(str)
disconnectedSignal = pyqtSignal(str)
_limiting_updated = pyqtSignal(bool, bool, bool)
def __init__(self, tabWidget, helper, *args):
super(SwarmTab, self).__init__(*args)
self.setupUi(self)
self.tabName = "Swarm"
self.menuName = "Swarm"
self.tabWidget = tabWidget
self.helper = helper
self.disconnectedSignal.connect(self.disconnected)
self.connectionFinishedSignal.connect(self.connected)
# Incoming 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)
# PlotWidget Stuff
self._plot = PlotWidget(fps=30)
# Check if we could find the PyQtImport. If not, then
# set this tab as disabled
self.enabled = self._plot.can_enable
self._model = LogConfigModel()
self.dataSelector.setModel(self._model)
self.plotLayout.addWidget(self._plot)
self._log_data_signal.connect(self._log_data_received)
self._plotter_log_error_signal.connect(self._plotter_logging_error)
if self.enabled:
self._disconnected_signal.connect(self._disconnected)
self.helper.cf.disconnected.add_callback(
self._disconnected_signal.emit)
self._connected_signal.connect(self._connected)
self.helper.cf.connected.add_callback(
self._connected_signal.emit)
self.helper.cf.log.block_added_cb.add_callback(self._config_added)
self.dataSelector.currentIndexChanged.connect(
self._selection_changed)
self._previous_config = None
self._started_previous = False
# 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.inputTypeComboBox.currentIndexChanged.connect(self.inputtypeChange)
self.stepHeightCombo.valueChanged.connect(self._step_height_changed)
self.frequencyCombo.valueChanged.connect(self._sine_frequency_changed)
self.referenceHeightCheckbox.toggled.connect(lambda: self._use_ref_input(self.referenceHeightCheckbox.isChecked()))
self.targetCalPitch.valueChanged.connect(self._trim_pitch_changed)
self.updateGainsBtn.clicked.connect(self._controller_gain_changed)
self.k1Combo.valueChanged.connect(self._k123_gain_changed)
self.k2Combo.valueChanged.connect(self._k123_gain_changed)
self.k3Combo.valueChanged.connect(self._k123_gain_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.crazyflieXModeCheckbox.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.crazyflieXModeCheckbox.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._ring_populate_dropdown)
self.logBaro = None
self.logAltHold = None
self.ai = AttitudeIndicator()
self.verticalLayout_2.addWidget(self.ai)
# self.logo = QLabel(self)
# pixmap = QPixmap('C:\dev\crazyflie-clients-python\src\cfclient/SUTDLogo.png')
# self.logo.setPixmap(pixmap.scaled(150, 150, Qt.KeepAspectRatio))
# self.verticalLayout.addWidget(self.logo)
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):
# Reset Gains
self.helper.cf.param.set_value("posCtlPid.zKd", str(0.0))
self.helper.cf.param.set_value("posCtlPid.zKi", str(0.5))
self.helper.cf.param.set_value("posCtlPid.zKp", str(40.0))
self.k1Combo.setValue(40.0)
self.helper.cf.param.set_value("posCtlPid.zVelMax", str(15.0))
self.helper.cf.param.set_value("velCtlPid.vzKd", str(0.0))
self.helper.cf.param.set_value("velCtlPid.vzKi", str(0.0))
self.helper.cf.param.set_value("velCtlPid.vzKp", str(15.0))
self.k2Combo.setValue(15.0)
self.k3Combo.setValue(0.0)
# 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))
self._populate_assisted_mode_dropdown()
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 _k123_gain_changed(self):
logger.debug("controller gains updated")
self.updateGainsBtn.setStyleSheet("background-color:#0078d7; color:#ffffff;")
self.resetGainsBtn.setStyleSheet("background-color:#0078d7; color:#ffffff;")
def _controller_gain_changed(self):
self.updateGainsBtn.setStyleSheet("background-color:#f0f0f0; color:#000000;")
self.helper.cf.param.set_value("posCtlPid.zKi", str(self.k3Combo.value()))
self.helper.cf.param.set_value("posCtlPid.zKp", str(self.k1Combo.value()))
self.helper.cf.param.set_value("velCtlPid.vzKp", str(self.k2Combo.value()))
def _reset_gain_changed(self):
# Reset Gains
self.k1Combo.value(40.0)
self.k2Combo.setValue(15.0)
self.k3Combo.setValue(0.0)
self.helper.cf.param.set_value("posCtlPid.zKi", str(self.k3Combo.value()))
self.helper.cf.param.set_value("posCtlPid.zKp", str(self.k1Combo.value()))
self.helper.cf.param.set_value("velCtlPid.vzKp", str(self.k2Combo.value()))
self.resetGainsBtn.setStyleSheet("background-color:#f0f0f0; color:#000000;")
def _step_height_changed(self, reference):
logger.debug("Reference height updated to [%f]" % reference)
self.targetHeight.setText(("%.2f m" % reference))
self.ai.setHover(reference, self.is_visible())
self.helper.referenceHeight = reference
self.helper.inputTime = 0.0
def _sine_frequency_changed(self, freq):
logger.debug("Sine-wave frequency updated to [%f]" % freq)
self.helper.sinewaveFrequency = freq
self.helper.inputTime = 0.0
def _use_ref_input(self, enabled):
logger.debug("Using reference input")
self.helper.useReferenceHeight = enabled
self.helper.inputTime = 0.0
def inputtypeChange(self, item):
logger.debug("Changed input type to %s",
self.inputTypeComboBox.itemText(item))
self.helper.inputType = item
self.helper.inputTime = 0.0
# item is a number, #0 = 'Step Input', #1 = 'Sine Wave', #2 = 'Ramp'
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 _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"}
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 self.helper.cf.mem.ow_search(vid=0xBC, pid=0x01):
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"]):
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 self.helper.cf.mem.ow_search(vid=0xBC, pid=0x09):
heightHoldPossible = True
if self.helper.cf.mem.ow_search(vid=0xBC, pid=0x0A):
heightHoldPossible = True
hoverPossible = True
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, link_uri):
"""Callback when the Crazyflie has been connected"""
self._plot.removeAllDatasets()
self._plot.set_title("")
def _disconnected(self, link_uri):
"""Callback for when the Crazyflie has been disconnected"""
self._model.beginResetModel()
self._model.reset()
self._model.endResetModel()
self.dataSelector.setCurrentIndex(-1)
self._previous_config = None
self._started_previous = False
def _log_data_signal_wrapper(self, ts, data, logconf):
"""Wrapper for signal"""
# For some reason the *.emit functions are not
# the same over time (?!) so they cannot be registered and then
# removed as callbacks.
self._log_data_signal.emit(ts, data, logconf)
def _log_error_signal_wrapper(self, config, msg):
"""Wrapper for signal"""
# For some reason the *.emit functions are not
# the same over time (?!) so they cannot be registered and then
# removed as callbacks.
self._plotter_log_error_signal.emit(config, msg)
def _selection_changed(self, i):
"""Callback from ComboBox when a new item has been selected"""
# Check if we have disconnected
if i < 0:
return
# First check if we need to stop the old block
if self._started_previous and self._previous_config:
logger.debug("Should stop config [%s], stopping!",
self._previous_config.name)
self._previous_config.delete()
# Remove our callback for the previous config
if self._previous_config:
self._previous_config.data_received_cb.remove_callback(
self._log_data_signal_wrapper)
self._previous_config.error_cb.remove_callback(
self._log_error_signal_wrapper)
lg = self._model.get_config(i)
if not lg.started:
logger.debug("Config [%s] not started, starting!", lg.name)
self._started_previous = True
lg.start()
else:
self._started_previous = False
self._plot.removeAllDatasets()
color_selector = 0
self._plot.set_title(lg.name)
for d in lg.variables:
self._plot.add_curve(d.name, self.colors[
color_selector % len(self.colors)])
color_selector += 1
lg.data_received_cb.add_callback(self._log_data_signal_wrapper)
lg.error_cb.add_callback(self._log_error_signal_wrapper)
self._previous_config = lg
def _config_added(self, logconfig):
"""Callback from the log layer when a new config has been added"""
logger.debug("Callback for new config [%s]", logconfig.name)
self._model.add_block(logconfig)
def _plotter_logging_error(self, log_conf, msg):
"""Callback from the log layer when an error occurs"""
QMessageBox.about(
self, "Plot error", "Error when starting log config [%s]: %s" % (
log_conf.name, msg))
def _log_data_received(self, timestamp, data, logconf):
"""Callback when the log layer receives new data"""
# Check so that the incoming data belongs to what we are currently
# logging
if self._previous_config:
if self._previous_config.name == logconf.name:
self._plot.add_data(data, timestamp)
def connected(self, linkURI):
# Reset Gains
self.helper.cf.param.set_value("posCtlPid.zKd", str(0.0))
self.helper.cf.param.set_value("posCtlPid.zKi", str(0.5))
self.helper.cf.param.set_value("posCtlPid.zKp", str(40.0))
self.k1Combo.setValue(40.0)
self.helper.cf.param.set_value("posCtlPid.zVelMax", str(15.0))
self.helper.cf.param.set_value("velCtlPid.vzKd", str(0.0))
self.helper.cf.param.set_value("velCtlPid.vzKi", str(0.0))
self.helper.cf.param.set_value("velCtlPid.vzKp", str(15.0))
self.k2Combo.setValue(15.0)
self.k3Combo.setValue(0.0)
# 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))
self._populate_assisted_mode_dropdown()
from cflib.crazyflie import Crazyflie
from cflib.crazyflie.log import LogConfig
from cflib.crazyflie.syncCrazyflie import SyncCrazyflie
from cflib.crazyflie.syncLogger import SyncLogger
URI = 'radio://0/40/2M/E7E7E7E7E7'
def position_callback(timestamp, data, logconf):
print(data)
if __name__ == '__main__':
# Initialize the low-level drivers (don't list the debug drivers)
cflib.crtp.init_drivers(enable_debug_driver=False)
log_conf = LogConfig(name='Position', period_in_ms=500)
log_conf.add_variable('ranging.distance2', 'float')
#log_conf.add_variable('kalman.stateX', 'float')
#log_conf.add_variable('kalman.stateZ', 'float')
#log_conf.add_variable('stabilizer.roll', 'float')
#log_conf.add_variable('stabilizer.pitch', 'float')
#log_conf.add_variable('stabilizer.yaw', 'float')
with SyncCrazyflie(URI, cf=Crazyflie(rw_cache='./cache')) as scf:
scf.cf.log.add_config(log_conf)
log_conf.data_received_cb.add_callback(position_callback)
log_conf.start()
for i in range(60):
time.sleep(1)
def main():
# Initialize the low-level drivers (don't list the debug drivers)
cflib.crtp.init_drivers(enable_debug_driver=False)
# Scan for Crazyflies and use the first one found
print('Scanning interfaces for Crazyflies...')
available = cflib.crtp.scan_interfaces()
print('Crazyflies found:')
for i in available:
print(i[0])
if len(available) == 0:
print('No Crazyflies found, cannot run example')
else:
if (small_log):
logConfig_stab = LogConfig(name='AttitudeBeforePosCtrl',
period_in_ms=log_intervall)
logConfig_stab.add_variable('bposatt.roll', 'float')
logConfig_stab.add_variable('bposatt.pitch', 'float')
logConfig_stab.add_variable('bposatt.yaw', 'float')
logConfig_stab.add_variable('bposatt.actuatorThrust', 'float')
logConfig_state = LogConfig(name='AttitudeAfterPosCtrl',
period_in_ms=log_intervall)
logConfig_state.add_variable('aposatt.roll', 'float')
logConfig_state.add_variable('aposatt.pitch', 'float')
logConfig_state.add_variable('aposatt.yaw', 'float')
logConfig_state.add_variable('aposatt.actuatorThrust', 'float')
logConfig_all = LogConfig(name='PositionLog',
period_in_ms=log_intervall)
logConfig_all.add_variable('stateEstimate.x', 'float')
logConfig_all.add_variable('stateEstimate.y', 'float')
logConfig_all.add_variable('stateEstimate.z', 'float')
with SyncCrazyflie(URI) as scf:
cf = scf.cf
if (small_log):
logger_stab = SyncLogger(scf, logConfig_stab)
logger_state = SyncLogger(scf, logConfig_state)
logger_stab.connect()
logger_state.connect()
logger_all = SyncLogger(scf, logConfig_all)
logger_all.connect()
cf.param.set_value('kalman.resetEstimation', '1')
time.sleep(0.1)
cf.param.set_value('kalman.resetEstimation', '0')
time.sleep(2)
for y in range(maneuver_iterations):
cf.commander.send_hover_setpoint(0, 0, 0, y * maneuver_step)
time.sleep(time_step)
for i in range(balance_iterations):
print_message("Balancing out " + str(i))
cf.commander.send_hover_setpoint(0, 0, 0, height)
time.sleep(time_step)
log_start = logger_all._queue.qsize()
for i in range(log_iterations):
print_message("Logging " + str(i))
cf.commander.send_hover_setpoint(0, 0, 0, height)
time.sleep(time_step)
log_end = logger_all._queue.qsize()
log_count = log_end - log_start
print_message("Start:{}, end:{}".format(log_start, log_end))
print_message("To log {} lines.".format(log_end - log_start + 1))
for y in range(maneuver_iterations - 1):
cf.commander.send_hover_setpoint(0, 0, 0,
height - (y * maneuver_step))
time.sleep(time_step)
cf.commander.send_hover_setpoint(0, 0, 0, 0.1)
time.sleep(time_step)
time.sleep(time_step)
cf.commander.send_stop_setpoint() #land
time.sleep(1.5)
print_message("Landed, writing out log...")
if (small_log):
with open(log_file_name(logConfig_stab), "w") as f:
log_count = write_out_log(logger_stab, f, log_start,
log_end)
print_message("logged stabilizer")
with open(log_file_name(logConfig_state), "w") as f:
log_count = write_out_log(logger_state, f, log_start,
log_end)
print_message("logged state")
logger_stab.disconnect()
logger_state.disconnect()
with open(log_file_name(logConfig_all), "w") as f:
log_count = write_out_log(logger_all, f, log_start, log_end)
logger_all.disconnect()
print_message("Done logging.")
class FlightTab(Tab, flight_tab_class):
uiSetupReadySignal = pyqtSignal()
_accel_data_signal = pyqtSignal(int, object, object)
_gyro_data_signal = pyqtSignal(int, object, object)
_motor_data_signal = pyqtSignal(int, object, object)
_imu_data_signal = pyqtSignal(int, object, object)
_althold_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)
_log_error_signal = pyqtSignal(object, str)
#UI_DATA_UPDATE_FPS = 10
connectionFinishedSignal = pyqtSignal(str)
disconnectedSignal = pyqtSignal(str)
def __init__(self, tabWidget, helper, *args):
global D
D.thrust = 0
D.pitch = 0
D.yawrate = 0
D.roll = 0
rospy.init_node("cf_flightTab")
self.motor_pub = rospy.Publisher("cf_motorData", MotorData)
self.stab_pub = rospy.Publisher("cf_stabData", StabData)
self.acc_pub = rospy.Publisher("cf_accData", AccelData)
self.gyro_pub = rospy.Publisher("cf_gyroData", GyroData)
rospy.Subscriber("cf_textcmd", String, self._cmdCB)
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.althold_updated.add_callback(
lambda enabled: self.helper.cf.param.set_value("flightmode.althold", enabled))
self._gyro_data_signal.connect(self._gyro_data_received)
self._accel_data_signal.connect(self._accel_data_received)
self._imu_data_signal.connect(self._imu_data_received)
self._baro_data_signal.connect(self._baro_data_received)
self._althold_data_signal.connect(self._althold_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(GuiConfig().get("client_side_xmode"))
self.crazyflieXModeCheckbox.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.crazyflieXModeCheckbox.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.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="flightmode", name="althold",
cb=(lambda name, enabled:
self.helper.inputDeviceReader.setAltHold(eval(enabled))))
self.helper.cf.param.add_update_callback(
group="imu_sensors",
cb=self._set_available_sensors)
self.logBaro = None
self.logAltHold = None
self.ai = AttitudeIndicator()
self.verticalLayout_4.addWidget(self.ai)
self.splitter.setSizes([1000,1])
self.targetCalPitch.setValue(GuiConfig().get("trim_pitch"))
self.targetCalRoll.setValue(GuiConfig().get("trim_roll"))
def thrustToPercentage(self, thrust):
return ((thrust / MAX_THRUST) * 100.0)
def uiSetupReady(self):
flightComboIndex = self.flightModeCombo.findText(
GuiConfig().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 _cmdCB(self, data):
m = data.data
print ("Recieved command: " + m)
if m is 'q':
self.helper.cf.commander.send_setpoint(0, 0, 0, 0)
elif m.count(" ")>0:
hpr,value=m.split(" ")
if(hpr is "pitch" or hpr is "p"):
D.pitch= float(value)
if(hpr=="yawrate" or hpr is "y"):
D.yawrate= float(value)
if(hpr=="roll" or hpr is "r"):
D.roll=float(value)
if(hpr=="thrust" or hpr is "t"):
D.thrust= int(value)
print (D.thrust)
if D.thrust <= 10000:
D.thrust = 10001
elif D.thrust > 60000:
D.thrust = 60000
self.helper.cf.commander.send_setpoint(D.roll, D.pitch, D.yawrate, D.thrust)
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"])
d = MotorData()
d.m1 = data["motor.m1"]
d.m2 = data["motor.m2"]
d.m3 = data["motor.m3"]
d.m4 = data["motor.m4"]
self.motor_pub.publish(d)
def _gyro_data_received(self, timestamp, data, logconf):
if self.isVisible():
d = GyroData()
d.x = data["gyro.x"]
d.y = data["gyro.y"]
d.z = data["gyro.z"]
self.gyro_pub.publish(d)
def _accel_data_received(self, timestamp, data, logconf):
if self.isVisible():
d = AccelData()
d.x = data["acc.x"]
d.y = data["acc.y"]
d.z = data["acc.z"]
self.acc_pub.publish(d)
def _baro_data_received(self, timestamp, data, logconf):
if self.isVisible():
self.actualASL.setText(("%.2f" % data["baro.aslLong"]))
self.ai.setBaro(data["baro.aslLong"])
def _althold_data_received(self, timestamp, data, logconf):
if self.isVisible():
target = data["altHold.target"]
if target>0:
if not self.targetASL.isEnabled():
self.targetASL.setEnabled(True)
self.targetASL.setText(("%.2f" % target))
self.ai.setHover(target)
elif self.targetASL.isEnabled():
self.targetASL.setEnabled(False)
self.targetASL.setText("Not set")
self.ai.setHover(0)
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"])
d = StabData()
d.pitch = data["stabilizer.pitch"]
d.roll = data["stabilizer.roll"]
d.yaw = data["stabilizer.yaw"]
self.stab_pub.publish(d)
def connected(self, linkURI):
# IMU & THRUST
lg = LogConfig("Stabalizer", GuiConfig().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")
self.helper.cf.log.add_config(lg)
if (lg.valid):
lg.data_received_cb.add_callback(self._imu_data_signal.emit)
lg.error_cb.add_callback(self._log_error_signal.emit)
lg.start()
else:
logger.warning("Could not setup logconfiguration after "
"connection!")
# MOTOR
lg = LogConfig("Motors", GuiConfig().get("ui_update_period"))
lg.add_variable("motor.m1")
lg.add_variable("motor.m2")
lg.add_variable("motor.m3")
lg.add_variable("motor.m4")
self.helper.cf.log.add_config(lg)
if lg.valid:
lg.data_received_cb.add_callback(self._motor_data_signal.emit)
lg.error_cb.add_callback(self._log_error_signal.emit)
lg.start()
else:
logger.warning("Could not setup logconfiguration after "
"connection!")
#Accel
lg = LogConfig("Accelerometer", GuiConfig().get("ui_update_period"))
lg.add_variable("acc.x")
lg.add_variable("acc.y")
lg.add_variable("acc.z")
self.helper.cf.log.add_config(lg)
if lg.valid:
lg.data_received_cb.add_callback(self._accel_data_signal.emit)
lg.error_cb.add_callback(self._log_error_signal.emit)
lg.start()
else:
logger.warning("Could not setup logconfiguration after "
"connection!")
#Gyroscope
lg = LogConfig("Gyroscope", GuiConfig().get("ui_update_period"))
lg.add_variable("gyro.x")
lg.add_variable("gyro.y")
lg.add_variable("gyro.z")
self.helper.cf.log.add_config(lg)
if lg.valid:
print ("Gyro logging started correctly")
lg.data_received_cb.add_callback(self._gyro_data_signal.emit)
lg.error_cb.add_callback(self._log_error_signal.emit)
lg.start()
else:
logger.warning("Could not setup logconfiguration after "
"connection!")
def _set_available_sensors(self, name, available):
logger.info("[%s]: %s", name, available)
available = eval(available)
if ("HMC5883L" in name):
if (not available):
self.actualASL.setText("N/A")
self.actualASL.setEnabled(False)
else:
self.actualASL.setEnabled(True)
self.helper.inputDeviceReader.setAltHoldAvailable(available)
if (not self.logBaro and not self.logAltHold):
# The sensor is available, set up the logging
self.logBaro = LogConfig("Baro", 200)
self.logBaro.add_variable("baro.aslLong", "float")
self.helper.cf.log.add_config(self.logBaro)
if self.logBaro.valid:
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()
else:
logger.warning("Could not setup logconfiguration after "
"connection!")
self.logAltHold = LogConfig("AltHold", 200)
self.logAltHold.add_variable("altHold.target", "float")
self.helper.cf.log.add_config(self.logAltHold)
if self.logAltHold.valid:
self.logAltHold.data_received_cb.add_callback(
self._althold_data_signal.emit)
self.logAltHold.error_cb.add_callback(
self._log_error_signal.emit)
self.logAltHold.start()
else:
logger.warning("Could not setup logconfiguration after "
"connection!")
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.actualASL.setText("")
self.targetASL.setText("Not Set")
self.targetASL.setEnabled(False)
self.actualASL.setEnabled(False)
self.logBaro = None
self.logAltHold = None
def minMaxThrustChanged(self):
self.helper.inputDeviceReader.set_thrust_limits(
self.minThrust.value(), self.maxThrust.value())
if (self.isInCrazyFlightmode == True):
GuiConfig().set("min_thrust", self.minThrust.value())
GuiConfig().set("max_thrust", self.maxThrust.value())
def thrustLoweringSlewRateLimitChanged(self):
self.helper.inputDeviceReader.set_thrust_slew_limiting(
self.thrustLoweringSlewRateLimit.value(),
self.slewEnableLimit.value())
if (self.isInCrazyFlightmode == True):
GuiConfig().set("slew_limit", self.slewEnableLimit.value())
GuiConfig().set("slew_rate", self.thrustLoweringSlewRateLimit.value())
def maxYawRateChanged(self):
logger.debug("MaxYawrate changed to %d", self.maxYawRate.value())
self.helper.inputDeviceReader.set_yaw_limit(self.maxYawRate.value())
if (self.isInCrazyFlightmode == True):
GuiConfig().set("max_yaw", self.maxYawRate.value())
def maxAngleChanged(self):
logger.debug("MaxAngle changed to %d", self.maxAngle.value())
self.helper.inputDeviceReader.set_rp_limit(self.maxAngle.value())
if (self.isInCrazyFlightmode == True):
GuiConfig().set("max_rp", self.maxAngle.value())
def _trim_pitch_changed(self, value):
logger.debug("Pitch trim updated to [%f]" % value)
self.helper.inputDeviceReader.set_trim_pitch(value)
GuiConfig().set("trim_pitch", value)
def _trim_roll_changed(self, value):
logger.debug("Roll trim updated to [%f]" % value)
self.helper.inputDeviceReader.set_trim_roll(value)
GuiConfig().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" % roll))
self.targetPitch.setText(("%0.2f" % pitch))
self.targetYaw.setText(("%0.2f" % 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):
GuiConfig().set("flightmode", self.flightModeCombo.itemText(item))
logger.info("Changed flightmode to %s",
self.flightModeCombo.itemText(item))
self.isInCrazyFlightmode = False
if (item == 0): # Normal
self.maxAngle.setValue(GuiConfig().get("normal_max_rp"))
self.maxThrust.setValue(GuiConfig().get("normal_max_thrust"))
self.minThrust.setValue(GuiConfig().get("normal_min_thrust"))
self.slewEnableLimit.setValue(GuiConfig().get("normal_slew_limit"))
self.thrustLoweringSlewRateLimit.setValue(
GuiConfig().get("normal_slew_rate"))
self.maxYawRate.setValue(GuiConfig().get("normal_max_yaw"))
if (item == 1): # Advanced
self.maxAngle.setValue(GuiConfig().get("max_rp"))
self.maxThrust.setValue(GuiConfig().get("max_thrust"))
self.minThrust.setValue(GuiConfig().get("min_thrust"))
self.slewEnableLimit.setValue(GuiConfig().get("slew_limit"))
self.thrustLoweringSlewRateLimit.setValue(
GuiConfig().get("slew_rate"))
self.maxYawRate.setValue(GuiConfig().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)
@pyqtSlot(bool)
def changeXmode(self, checked):
self.helper.cf.commander.set_client_xmode(checked)
GuiConfig().set("client_side_xmode", checked)
logger.info("Clientside X-mode enabled: %s", checked)
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.crazyflieXModeCheckbox.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.crazyflieXModeCheckbox.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._ring_populate_dropdown)
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))
self._populate_assisted_mode_dropdown()
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 _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 self.helper.cf.mem.ow_search(vid=0xBC, pid=0x01):
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"]):
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 self.helper.cf.mem.ow_search(vid=0xBC, pid=0x09):
heightHoldPossible = True
self.helper.inputDeviceReader.set_hover_max_height(1.0)
if self.helper.cf.mem.ow_search(vid=0xBC, pid=0x0E):
heightHoldPossible = True
self.helper.inputDeviceReader.set_hover_max_height(2.0)
if self.helper.cf.mem.ow_search(vid=0xBC, pid=0x0A):
heightHoldPossible = True
hoverPossible = True
self.helper.inputDeviceReader.set_hover_max_height(1.0)
if self.helper.cf.mem.ow_search(vid=0xBC, pid=0x0F):
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)
time.sleep(0.1)
if __name__ == '__main__':
# # initializing the queue and event object
q = queue.Queue(maxsize=0)
e1 = threading.Event()
e2 = threading.Event()
# # initializing Crazyflie
cflib.crtp.init_drivers(enable_debug_driver=False)
with SyncCrazyflie(uri_3, cf=Crazyflie(rw_cache='./cache')) as scf_3:
logconf_3 = LogConfig(name='Position', period_in_ms=500)
logconf_3.add_variable('kalman.stateX', 'float')
logconf_3.add_variable('kalman.stateY', 'float')
logconf_3.add_variable('kalman.stateZ', 'float')
scf_3.cf.log.add_config(logconf_3)
logconf_3.data_received_cb.add_callback(
lambda timestamp, data, logconf_3: log_pos_callback_2(
uri_3, timestamp, data, logconf_3))
with SyncCrazyflie(uri_2, cf=Crazyflie(rw_cache='./cache')) as scf_2:
logconf_2 = LogConfig(name='Position', period_in_ms=500)
logconf_2.add_variable('kalman.stateX', 'float')
logconf_2.add_variable('kalman.stateY', 'float')
logconf_2.add_variable('kalman.stateZ', 'float')
scf_2.cf.log.add_config(logconf_2)
logconf_2.data_received_cb.add_callback(
def run_sequence(scf, params):
cf = scf.cf
base = params['base']
z = params['h']
#sets variables for liftoff, beginning, and end heights
end = 0.3
base = params['base']
h = params['h']
num = params['num']
# lifts drones off and sends to beginning heights
poshold(cf, 2, base)
poshold(cf, 5, h)
# The definition of the logconfig can be made before connecting
log_config = LogConfig(name='Height', period_in_ms=50)
log_config.add_variable('stateEstimate.z', 'float')
with SyncLogger(scf, log_config) as logger:
#sets the runtime
endTime = time.time() + 45
j = 0
time_elapsed = float(0)
for log_entry in logger:
timestamp = log_entry[0]
data = log_entry[1]
currentPos[num] = data['stateEstimate.z']
#first runthrough sets begin time of drone, and holds it in
#current position for another second
if j == 0:
begin_T = timestamp
j = 1
poshold(cf, 1, h)
print(0)
#concurrent runthroughs run the consensus algorithm every
#three seconds and sends the desired position every second
else:
time_elapsed = float((timestamp - begin_T)) / 1000
#every three seconds runs consensus alg
if time_elapsed.is_integer() and (int(time_elapsed) % 3) == 0:
#ensures all three drones have sent their positions before continuuing
while currentPos[0] == 0 or currentPos[
1] == 0 or currentPos[2] == 0 or currentPos[
3] == 0 or currentPos[4] == 0:
time.sleep(0.001)
#runs consensus algorithm only on one of the 3 parallel programs running
if num == 0:
print('running consensus')
consensus(currentPos)
#(nextPos)
#waits till the algorithm runs to continue
while (nextPos[0] == 0 or nextPos[1] == 0
or nextPos[2] == 0 or nextPos[3] == 0
or nextPos[4] == 0):
time.sleep(0.001)
#sends the update positions to the drones
poshold(cf, 1, nextPos[num])
#print(nextPos)
#sends position for drones every second
elif time_elapsed.is_integer():
if time_elapsed < 3:
poshold(cf, 1, h)
else:
poshold(cf, 1, nextPos[num])
#if time_elapsed.is_integer():
#print(time_elapsed)
#print(currentPos)
#appends the time and height data to a log
savelog[num].append([time_elapsed, currentPos[num]])
#ends program after endTime seconds
if time.time() > endTime:
break
#poshold(cf,2, end)
# # Base altitude in meters
#print(currentPos)
#sends drones to base height
poshold(cf, 2, base)
#saves the data for future use
str1 = 'testing'
str2 = '.csv'
with open(str1 + str(num) + str2, 'w') as f:
writer = csv.writer(f, delimiter=',')
writer.writerows(savelog[num])
cf.commander.send_stop_setpoint()
class LogGroup(QTreeWidgetItem):
""" Group
If turned on and no children are active, activate all children
if hz changed, create new log (and start if on)
Everytime the children change, we stop, delete the old log and create a new one, optionally starting it
Everytime the HZ changes, we stop, delete the old log and create a new one, optionally starting it
Everytime we change, we start/stop the log
We request a start, use partially checked
Once started, use totally checked
"""
def __init__(self, parent, name, children, hz=50):
super(LogGroup, self).__init__(parent)
self.name = name
self.setFlags(self.flags() | Qt.ItemIsEditable | Qt.ItemIsUserCheckable)
# Keep track of if all/none of the children are active
self.cAll = True
self.cNone = True
self.validHZ = list(OrderedDict.fromkeys([round(100/floor(100/x),2) for x in range(1,100)]))
self.hzTarget = hz
# Monitor the incoming frequency
self.fm = None
self.fmOn = True
# log config
self.lg = None
# Show text
QtGui.QTreeWidgetItem.setData(self, 0, Qt.DisplayRole, QVariant(name))
QtGui.QTreeWidgetItem.setData(self, 0, Qt.CheckStateRole, Qt.Unchecked)
QtGui.QTreeWidgetItem.setData(self, 1, Qt.DisplayRole, "Off")
QtGui.QTreeWidgetItem.setData(self, 3, Qt.DisplayRole, QVariant(0))
#QtGui.QTreeWidgetItem.setData(self, 4, Qt.CheckStateRole, Qt.Unchecked)
self.readSettings()
# Initialise Children
for c in sorted(children.keys()):
self.addChild(LogItem(self, children[c]))
self.c = 0
# Now everything is initialised except the logging
# Start logging if active
if self.checkState(0) == Qt.Checked:
self.requestLog()
def setEstimateHzOn(self, on):
""" If on then we esstiamte the HZ """
self.fmOn = on
def getValidHZ(self, hz):
# zip(list(OrderedDict.fromkeys([round(100/floor(100/x),2) for x in range(1,100)])),list(OrderedDict.fromkeys([1000/round(100/floor(100/x),2) for x in range(1,100)])))
i = bisect.bisect_left(self.validHZ, hz)
vHZ = min(self.validHZ[max(0, i-1): i+2], key=lambda t: abs(hz - t))
if hz!=vHZ:
rospy.loginfo("Could not set HZ to specific value [%d], rounded to valid HZ [%.4f]", hz, vHZ)
return vHZ
def readSettings(self):
""" Read the HZ and selected things to log from previous session """
settings = QSettings("omwdunkley", "flieROS")
# Read target HZ
hzQv = settings.value("log_"+self.name+"_hz",QVariant(20))
QtGui.QTreeWidgetItem.setData(self, 2, Qt.DisplayRole, hzQv)
self.hzTarget = self.getValidHZ(hzQv.toFloat()[0])
# Read if checked. If partially checked, uncheck
onQv = settings.value("log_"+self.name+"_on", QVariant(Qt.Unchecked))
QtGui.QTreeWidgetItem.setData(self, 0, Qt.CheckStateRole, onQv if onQv.toInt()[0]!=Qt.PartiallyChecked else Qt.Unchecked)
#onRos = settings.value("ros_"+self.name+"_on", QVariant(Qt.Unchecked))
#QtGui.QTreeWidgetItem.setData(self, 0, Qt.CheckStateRole, onRos)
def writeSettings(self):
""" Save the HZ and selected things to log between sessions"""
settings = QSettings("omwdunkley", "flieROS")
# Save target HZ
settings.setValue("log_"+self.name+"_hz", self.data(2, Qt.DisplayRole))
# Save checked state
settings.setValue("log_"+self.name+"_on", self.data(0, Qt.CheckStateRole))
#settings.setValue("ros_"+self.name+"_on", self.data(4, Qt.CheckStateRole))
# Save children state too
for x in range(self.childCount()):
self.child(x).writeSettings()
def updateFM(self):
# if not self.isActive()
if self.fm:
hz = self.fm.get_hz()
QtGui.QTreeWidgetItem.setData(self, 3, Qt.DisplayRole, round(hz,2))
return self.name, hz#, -1.0 if self.lg is None else self.hzTarget
else:
return self.name, -1.0#, -1.0 if self.lg is None else self.hzTarget
def logDataCB(self, ts, data, lg):
""" Our data from the log """
#print "LogCB data:", data.keys()
# Update out HZ monitor
if self.fmOn:
self.fm.count()
#TODO: add a checkbox in the row if we wanan send or not
self.treeWidget().incomingData(data, ts)
#TODO: there is bug here where the callbacks are called twice!! Holds for this + deleted CB
def logStartedCB(self, on):
""" Called when we actually start logging """
self.c+=1
#print "counter[%s]"%self.name, self.c
#if self.c % 2 == 1:
# return
if on:
rospy.loginfo( "[%d: %s @ %.2fhz] LogCB STARTED" % (self.lg.id, self.name, self.hzTarget))
QtGui.QTreeWidgetItem.setData(self, 0, Qt.CheckStateRole, Qt.Checked)
QtGui.QTreeWidgetItem.setData(self, 1, Qt.DisplayRole, "On")
self.setAllState(Qt.Checked, activeOnly=True)
else:
rospy.loginfo( "[%s] LogCB STOPPED" % ( self.name))
#print "[%s]LogCB ENDED(%d) " % (self.name, self.c)
#if self.allSelected():
# self.setAllState(Qt.Unchecked)
##else:
## self.setAllState(Qt.Checked, activeOnly=True)
#QtGui.QTreeWidgetItem.setData(self, 0, Qt.CheckStateRole, Qt.Unchecked)
#QtGui.QTreeWidgetItem.setData(self, 1, Qt.DisplayRole, "Off")
# def logAddedCB(self, l):
# """ Called when log added """
# print "LogCB added"
# QtGui.QTreeWidgetItem.setData(self, 0, Qt.CheckStateRole, Qt.Checked)
# QtGui.QTreeWidgetItem.setData(self, 1, Qt.DisplayRole, "Added")
def requestLog(self):
""" user requested start """
#print "Requested log start"
#print "None selected:",self.noneSelected()
self.setAllState(Qt.PartiallyChecked, activeOnly=not self.noneSelected())
QtGui.QTreeWidgetItem.setData(self, 0, Qt.CheckStateRole, Qt.PartiallyChecked)
QtGui.QTreeWidgetItem.setData(self, 1, Qt.DisplayRole, "Requested")
# Make and start log config
self.makeLog()
def stopLog(self):
""" User request halt """
#print "Requested log stop"
if self.lg:
self.lg.delete() #Invokes logStarted(False)
self.lg = None
if self.allSelected():
self.setAllState(Qt.Unchecked)
#else:
# self.setAllState(Qt.Checked, activeOnly=True)
QtGui.QTreeWidgetItem.setData(self, 0, Qt.CheckStateRole, Qt.Unchecked)
QtGui.QTreeWidgetItem.setData(self, 1, Qt.DisplayRole, "Off")
self.treeWidget().sig_logStatus.emit(self.name, -1)
self.fm = None
def errorLog(self, block, msg):
""" When a log error occurs """
#print "Log error: %s:", msg
QtGui.QTreeWidgetItem.setData(self, 1, Qt.DisplayRole, "Err: %s"%msg)
QtGui.QTreeWidgetItem.setData(self, 0, Qt.CheckStateRole, Qt.Unchecked)
if self.allSelected():
self.setAllState(Qt.Unchecked)
else:
self.setAllState(Qt.Checked, activeOnly=True)
self.fm = None
def setData(self, column, role, value):
""" Only allow changing of the check state and the hz """
# GROUP ON/OFF
preValue = self.checkState(0)
if role == Qt.CheckStateRole and column == 0 and preValue != value:
# Check box changed, allow and trigger
#print "\n\nCONFIG CHANGED\n"
if preValue==Qt.PartiallyChecked:
#print "User clicked: Waiting for log callback, cannot change config now"
pass
elif value==Qt.Checked:
#print "User clicked: request logging"
self.requestLog()
elif value==Qt.Unchecked:
#print "User clicked: request delete logging"
self.stopLog()
# HZ CHANGED
if column == 2:
#hz = min(max(value.toInt()[0],1),100)
hz = self.getValidHZ(value.toFloat()[0])
preHz = self.data(column, Qt.DisplayRole).toFloat()[0]
#print "\n\nHZ CHANGED\n"
if hz!=preHz:
if preValue==Qt.PartiallyChecked:
pass
#print "Waiting for log callback, cannot change hz now"
elif preValue==Qt.Checked:
#print "hz changed from %.2f to %.2f while logging, replace log" % (preHz, hz)
QtGui.QTreeWidgetItem.setData(self, column, role, hz)
self.hzTarget = hz
self.requestLog()
elif preValue==Qt.Unchecked:
#print "hz changed from %.2f to %.2f, not logging yet" % (preHz, hz)
QtGui.QTreeWidgetItem.setData(self, column, role, hz)
self.hzTarget = hz
def childStateChanged(self, child, newState):
if self.checkState(0) == Qt.Unchecked:
#print "Child Changed while not logging"
QtGui.QTreeWidgetItem.setData(child, 0, Qt.CheckStateRole, newState)
elif self.checkState(0) == Qt.PartiallyChecked:
pass
#print "Waiting for log callback"
elif self.checkState(0) == Qt.Checked:
QtGui.QTreeWidgetItem.setData(child, 0, Qt.CheckStateRole, newState)
if self.noneSelected():
#print "Last child unselected, remove logger"
self.stopLog()
elif self.allSelected():
#print "All children selected while logging, update logger"
self.requestLog()
else:
#print "Child Changed while logging, update logger"
self.requestLog()
def makeLog(self):
""" Makes a log. All ative children are added """
#print " -> Making new Log with %d ms interval"
if self.lg:
#print " ---> Previous Log detected, removing first"
self.lg.delete()
self.fm = FreqMonitor(window=max(10, int(1.2*self.hzTarget)))
self.lg = LogConfig(self.name, int(round(1000./self.hzTarget)))
#print " ---> Adding children to new log"
for x in range(self.childCount()):
c = self.child(x)
if c.isChecked():
name = c.log.group+"."+c.log.name
self.lg.add_variable(name, c.log.ctype)
#print " --- --> Adding child[%d]: [%s] to new log"%(x, name)
#print " ---> Checking log with TOC"
self.treeWidget().cf.log.add_config(self.lg)
if self.lg.valid:
#print " --- --> PASS"
self.lg.data_received_cb.add_callback(self.logDataCB)
self.lg.started_cb.add_callback(self.logStartedCB)
self.lg.error_cb.add_callback(self.treeWidget().sig_logError.emit)
self.lg.error_cb.add_callback(self.errorLog)
self.treeWidget().sig_logStatus.emit(self.name, self.hzTarget)
#print " --- --> callbacks added, starting new log NOW"
self.lg.start()
else:
#print " --- --> FAIL"
self.errorLog(None, "Invalid Config")
self.lg = None
self.fm = None
def setAllState(self, chkState, activeOnly=False):
""" Set all children on or off without their setData function. If activeOnly only set ones who are not off """
if activeOnly:
#print " -> Setting all ACTIVE children to checkstate [%d]", chkState
for x in range(self.childCount()):
if self.child(x).isChecked():
self.child(x).setState(chkState)
#print " --->", self.child(x).log.name
else:
#print " -> Setting all children to checkstate [%d]", chkState
for x in range(self.childCount()):
self.child(x).setState(chkState)
#print " --->", self.child(x).log.name
def allSelected(self):
""" Returns true if all children are selected """
for x in range(self.childCount()):
if not self.child(x).isChecked():
return False
return True
def noneSelected(self):
""" Returns true if no children are selected """
for x in range(self.childCount()):
if self.child(x).isChecked():
return False
return True
class TrafficController:
CS_DISCONNECTED = 0
CS_CONNECTING = 1
CS_CONNECTED = 2
STATE_UNKNOWN = -1
STATE_IDLE = 0
STATE_WAIT_FOR_POSITION_LOCK = 1
STATE_WAIT_FOR_TAKE_OFF = 2 # Charging
STATE_TAKING_OFF = 3
STATE_HOVERING = 4
STATE_WAITING_TO_GO_TO_INITIAL_POSITION = 5
STATE_GOING_TO_INITIAL_POSITION = 6
STATE_RUNNING_TRAJECTORY = 7
STATE_GOING_TO_PAD = 8
STATE_WAITING_AT_PAD = 9
STATE_LANDING = 10
STATE_CHECK_CHARGING = 11
STATE_REPOSITION_ON_PAD = 12
STATE_CRASHED = 13
NO_PROGRESS = -1000.0
PRE_STATE_TIMEOUT = 3
def __init__(self, uri):
self.uri = uri
self.stay_alive = True
self.reset_internal()
self.connection_thread = threading.Thread(target=self.process)
self.connection_thread.start()
def reset_internal(self):
self.connection_state = self.CS_DISCONNECTED
self._cf = None
self._log_conf = None
self.copter_state = self.STATE_UNKNOWN
self.vbat = -1.0
self._time_for_next_connection_attempt = 0
self.traj_cycles = None
self.est_x = 0.0
self.est_y = 0.0
self.est_z = 0.0
self.up_time_ms = 0
self.flight_time_ms = 0
# Pre states are used to prevent multiple calls to a copter
# when waiting for the remote state to change
self._pre_state_taking_off_end_time = 0
self._pre_state_going_to_initial_position_end_time = 0
def _pre_state_taking_off(self):
return self._pre_state_taking_off_end_time > time.time()
def _pre_state_going_to_initial_position(self):
return self._pre_state_going_to_initial_position_end_time > time.time()
def is_connected(self):
return self.connection_state == self.CS_CONNECTED
def has_found_position(self):
return self.copter_state > self.STATE_WAIT_FOR_POSITION_LOCK
def is_taking_off(self):
return self.copter_state == self.STATE_TAKING_OFF or self._pre_state_taking_off()
def is_ready_for_flight(self):
return self.copter_state == self.STATE_HOVERING and not self._pre_state_going_to_initial_position()
def is_flying(self):
return self.copter_state == self.STATE_RUNNING_TRAJECTORY or \
self.copter_state == self.STATE_WAITING_TO_GO_TO_INITIAL_POSITION or \
self.copter_state == self.STATE_GOING_TO_INITIAL_POSITION or \
self._pre_state_going_to_initial_position()
def is_landing(self):
return self.copter_state == self.STATE_GOING_TO_PAD or \
self.copter_state == self.STATE_WAITING_AT_PAD or \
self.copter_state == self.STATE_LANDING or \
self.copter_state == self.STATE_CHECK_CHARGING or \
self.copter_state == self.STATE_REPOSITION_ON_PAD
def is_charging(self):
return self.copter_state == self.STATE_WAIT_FOR_TAKE_OFF and not self._pre_state_taking_off()
def is_crashed(self):
return self.copter_state == self.STATE_CRASHED
def take_off(self):
if self.is_charging():
if self._cf:
self._pre_state_taking_off_end_time = time.time() + self.PRE_STATE_TIMEOUT
self._cf.param.set_value('app.takeoff', 1)
def start_trajectory(self, trajectory_delay, offset_x=0.0, offset_y=0.0, offset_z=0.0):
if self.is_ready_for_flight():
if self._cf:
self._cf.param.set_value('app.offsx', offset_x)
self._cf.param.set_value('app.offsy', offset_y)
self._cf.param.set_value('app.offsz', offset_z)
self._pre_state_going_to_initial_position_end_time = time.time() + self.PRE_STATE_TIMEOUT
self._cf.param.set_value('app.start', trajectory_delay)
def force_land(self):
if self.connection_state == self.CS_CONNECTED:
self._cf.param.set_value('app.stop', 1)
def set_trajectory_count(self, count):
if self.connection_state == self.CS_CONNECTED:
self._cf.param.set_value('app.trajcount', count)
def get_charge_level(self):
return self.vbat
def is_charged_for_flight(self):
return self.vbat > 4.10
def get_traj_cycles(self):
return self.traj_cycles
def process(self):
while self.stay_alive:
if self.connection_state == self.CS_DISCONNECTED:
if time.time() > self._time_for_next_connection_attempt:
self._connect()
time.sleep(1)
self._cf.close_link()
def _connected(self, link_uri):
self.connection_state = self.CS_CONNECTED
print('Connected to %s' % link_uri)
def _all_updated(self):
"""Callback that is called when all parameters have been updated"""
self.set_trajectory_count(2)
self._setup_logging()
def _connection_failed(self, link_uri, msg):
print('Connection to %s failed: %s' % (link_uri, msg))
self._set_disconnected(5)
def _connection_lost(self, link_uri, msg):
print('Connection to %s lost: %s' % (link_uri, msg))
def _disconnected(self, link_uri):
print('Disconnected from %s' % link_uri)
self._set_disconnected()
def _set_disconnected(self, hold_back_time=5):
self.reset_internal()
self._time_for_next_connection_attempt = time.time() + hold_back_time
def _connect(self):
if self.connection_state != self.CS_DISCONNECTED:
print("Can only connect when disconnected")
return
self.connection_state = self.CS_CONNECTING
self._cf = Crazyflie(rw_cache='./cache')
self._cf.connected.add_callback(self._connected)
self._cf.disconnected.add_callback(self._disconnected)
self._cf.connection_failed.add_callback(self._connection_failed)
self._cf.connection_lost.add_callback(self._connection_lost)
self._cf.param.all_updated.add_callback(self._all_updated)
# self._cf.console.receivedChar.add_callback(self._console_incoming) #print debug messages from Crazyflie
print("Connecting to " + self.uri)
self._cf.open_link(self.uri)
def _console_incoming(self, console_text):
print("CF {} DEBUG:".format( self.uri[-2:] ),console_text, end='')
def _setup_logging(self):
# print("Setting up logging")
self._log_conf = LogConfig(name='Tower', period_in_ms=100)
self._log_conf.add_variable('app.state', 'uint8_t')
self._log_conf.add_variable('app.prgr', 'float')
self._log_conf.add_variable('app.uptime', 'uint32_t')
self._log_conf.add_variable('app.flighttime', 'uint32_t')
self._log_conf.add_variable('pm.vbat', 'float')
self._log_conf.add_variable('stateEstimate.x', 'float')
self._log_conf.add_variable('stateEstimate.y', 'float')
self._cf.log.add_config(self._log_conf)
self._log_conf.data_received_cb.add_callback(self._log_data)
self._log_conf.start()
def _log_data(self, timestamp, data, logconf):
self.copter_state = data['app.state']
if self.copter_state != self.STATE_WAIT_FOR_TAKE_OFF:
self._pre_state_taking_off_end_time = 0
if self.copter_state != self.STATE_HOVERING:
self._pre_state_going_to_initial_position_end_time = 0
self.vbat = data['pm.vbat']
self.up_time_ms = data['app.uptime']
self.flight_time_ms = data['app.flighttime']
self.traj_cycles = data['app.prgr']
if self.traj_cycles <= self.NO_PROGRESS:
self.traj_cycles = None
self.est_x = data['stateEstimate.x']
self.est_y = data['stateEstimate.y']
def dump(self):
print("***", self.uri)
print(" Connection state:", self.connection_state)
print(" Copter state:", self.copter_state)
print(" Bat:", self.vbat)
print(" Up time:", self.up_time_ms / 1000)
print(" Flight time:", self.flight_time_ms / 1000)
print(" _pre_state_taking_off:", self._pre_state_taking_off())
print(" _pre_state_going_to_initial_position:", self._pre_state_going_to_initial_position())
def terminate(self):
self.stay_alive = False
if __name__ == '__main__':
cflib.crtp.init_drivers(enable_debug_driver=False)
# Set these to the position and yaw based on how your Crazyflie is placed
# on the floor
initial_x = 0.0
initial_y = 0.0
initial_z = 0.0
initial_yaw = 0 # In degrees
# 0: positive X direction
# 90: positive Y direction
# 180: negative X direction
# 270: negative Y direction
# define logging parameters
lg_posEst = LogConfig(name='stateEstimate', period_in_ms=20)
lg_posEst.add_variable('stateEstimate.x', 'float')
lg_posEst.add_variable('stateEstimate.y', 'float')
lg_posEst.add_variable('stateEstimate.z', 'float')
lg_posEst.add_variable('stateEstimate.roll', 'float')
lg_posEst.add_variable('stateEstimate.pitch', 'float')
lg_posEst.add_variable('stateEstimate.yaw', 'float')
# lg_posSet = LogConfig(name='posCtl', period_in_ms=20)
# lg_posSet.add_variable('posCtl.targetX', 'float')
# lg_posSet.add_variable('posCtl.targetY', 'float')
# lg_posSet.add_variable('posCtl.targetZ', 'float')
lg_pm = LogConfig(name='pm', period_in_ms=20)
lg_pm.add_variable('pm.vbat', 'float')
def _connected(self, link_uri):
""" This callback is called from the Crazyflie API when a Crazyflie
has been connected adn TOCs have been downloaded """
print('connected to: %s' % link_uri)
# Open text file for recording data
self.datalog_Pos = open("log_data/Cf2log_Pos", "w")
#self.datalog_Vel = open("log_data/Cf2log_Vel","w")
#self.datalog_Att = open("log_data/Cf2log_Att","w")
self.datalog_Att = open("log_data/Cf2log_Mtr", "w")
# Reset Kalman filter
self._cf.param.set_value('kalman.resetEstimation', '1')
time.sleep(0.1)
self._cf.param.set_value('kalman.resetEstimation', '0')
time.sleep(2)
print('Wait for Kalamn filter')
# Feedback position estimated by KF
logPos = LogConfig(name='Kalman Position',
period_in_ms=self.statefb_rate)
logPos.add_variable('kalman.stateX', 'float')
logPos.add_variable('kalman.stateY', 'float')
logPos.add_variable('kalman.stateZ', 'float')
# Feedback velocity estimated by KF
logVel = LogConfig(name='Kalman Velocity',
period_in_ms=self.statefb_rate)
logVel.add_variable('kalman.statePX', 'float')
logVel.add_variable('kalman.statePY', 'float')
logVel.add_variable('kalman.statePZ', 'float')
# Feedback Quaternion attitude estimated by KF
logAtt = LogConfig(name='Kalman Attitude',
period_in_ms=self.statefb_rate)
logAtt.add_variable('kalman.q0', 'float')
logAtt.add_variable('kalman.q1', 'float')
logAtt.add_variable('kalman.q2', 'float')
logAtt.add_variable('kalman.q3', 'float')
# Feedback Multi ranger
logMultiRa = LogConfig(name='Range', period_in_ms=self.statefb_rate)
logMultiRa.add_variable('range.front', 'uint16_t')
logMultiRa.add_variable('range.back', 'uint16_t')
logMultiRa.add_variable('range.left', 'uint16_t')
logMultiRa.add_variable('range.right', 'uint16_t')
logMultiRa.add_variable('range.up', 'uint16_t')
# Invoke logged states
self._cf.log.add_config(logPos)
self._cf.log.add_config(logVel)
self._cf.log.add_config(logAtt)
self._cf.log.add_config(logMultiRa)
# Start invoking logged states
if logPos.valid and logVel.valid:
# Position
logPos.data_received_cb.add_callback(self.log_pos_callback)
logPos.error_cb.add_callback(logging_error)
logPos.start()
# Velocity
logVel.data_received_cb.add_callback(self.log_vel_callback)
logVel.error_cb.add_callback(logging_error)
logVel.start()
# Quadternion attitude
logAtt.data_received_cb.add_callback(self.log_att_callback)
logAtt.error_cb.add_callback(logging_error)
logAtt.start()
# Multi-Ranger
logMultiRa.data_received_cb.add_callback(self.log_mtr_callback)
logMultiRa.error_cb.add_callback(logging_error)
logMultiRa.start()
else:
print(
"One or more of the variables in the configuration was not found"
+ "in log TOC. No logging will be possible")
# Start in a thread
threading.Thread(target=self._run_controller).start()
class Odometry(threading.Thread):
def __init__(self, cf):
self.cf = cf
super(Odometry, self).__init__()
self.lg_pose = LogConfig(name='Stabilizer1', period_in_ms=100)
self.lg_pose.add_variable('stateEstimate.x', 'float')
self.lg_pose.add_variable('stateEstimate.y', 'float')
self.lg_pose.add_variable('stateEstimate.z', 'float')
self.lg_pose.add_variable('stateEstimate.roll', 'float')
self.lg_pose.add_variable('stateEstimate.pitch', 'float')
self.lg_pose.add_variable('stateEstimate.yaw', 'float')
self.lg_velociy = LogConfig(name='Stabilizer2', period_in_ms=100)
self.lg_velociy.add_variable('stateEstimate.vx', 'float')
self.lg_velociy.add_variable('stateEstimate.vy', 'float')
self.lg_velociy.add_variable('stateEstimate.vz', 'float')
self.lg_velociy.add_variable('stateEstimateZ.ratePitch', 'float')
self.lg_velociy.add_variable('stateEstimateZ.rateRoll', 'float')
self.lg_velociy.add_variable('stateEstimateZ.rateYaw', 'float')
self.x = 0
self.y = 0
self.z = 0
self.roll = 0
self.pitch = 0
self.yaw = 0
self.vz = 0
self.vx = 0
self.vy = 0
self.ratePitch = 0
self.rateRoll = 0
self.rateYaw = 0
def run(self):
try:
self.cf.log.add_config(self.lg_pose)
self.cf.log.add_config(self.lg_velociy)
self.is_connected = True
self.lg_pose.data_received_cb.add_callback(self._pose_log_data)
self.lg_velociy.data_received_cb.add_callback(
self._velocity_log_data)
self.lg_pose.start()
self.lg_velociy.start()
except KeyError as e:
print('Could not start log configuration,'
'{} not found in TOC'.format(str(e)))
def _pose_log_data(self, timestamp, data, logconf):
"""Callback from a the log API when data arrives"""
#print('[%d][%s]: %s' % (timestamp, logconf.name, data))
self.x = data['stateEstimate.x']
self.y = data['stateEstimate.y']
self.z = data['stateEstimate.z']
self.roll = data['stateEstimate.roll']
self.pitch = data['stateEstimate.y']
self.yaw = data['stateEstimate.y']
# self.vx=data['stateEstimate.vx']
# self.vy=data['stateEstimate.vy']
# self.vz=data['stateEstimate.vz']
# self.wx=data['stateEstimateZ.rateRoll']
# self.wy=data['stateEstimateZ.ratePitch']
# self.wz=data['stateEstimateZ.rateYaw']
# print("odometry z=",self.z)
def _velocity_log_data(self, timestamp, data, logconf):
self.vx = data['stateEstimate.vx']
self.vy = data['stateEstimate.vy']
self.vz = data['stateEstimate.vz']
self.wx = data['stateEstimateZ.rateRoll']
self.wy = data['stateEstimateZ.ratePitch']
self.wz = data['stateEstimateZ.rateYaw']
class LoggingExample:
"""
Simple logging example class that logs the Stabilizer from a supplied
link uri and disconnects after 5s.
"""
def __init__(self, link_uri):
""" Initialize and run the example with the specified link_uri """
# Create a Crazyflie object without specifying any cache dirs
self._cf = Crazyflie()
#functions used for motors
self._cf.connected.add_callback(self._connected)
self._cf.disconnected.add_callback(self._disconnected)
self._cf.connection_failed.add_callback(self._connection_failed)
self._cf.connection_lost.add_callback(self._connection_lost)
# Connect some callbacks from the Crazyflie API
self._cf.connected.add_callback(self._connected)
self._cf.disconnected.add_callback(self._disconnected)
self._cf.connection_failed.add_callback(self._connection_failed)
self._cf.connection_lost.add_callback(self._connection_lost)
print('Connecting to %s' % link_uri)
# Try to connect to the Crazyflie
self._cf.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 Crazyflie API when a Crazyflie
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_acc = LogConfig(name='Accel', period_in_ms=10)
#self._lg_acc.add_variable('mag.x', 'float')
#self._lg_acc.add_variable('mag.y', 'float')
self._lg_acc.add_variable('test.TESTVARIABLE', 'float')
#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 Crazyflie is
# connected, since we need to check that the variables we
# would like to log are in the TOC.
try:
self._cf.log.add_config(self._lg_acc)
# This callback will receive the data
self._lg_acc.data_received_cb.add_callback(self._stab_log_data)
# This callback will be called on errors
self._lg_acc.error_cb.add_callback(self._stab_log_error)
# Start the logging
self._lg_acc.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(20, self._cf.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"""
global maxi
#print('[%s]: %s\n' % (logconf.name, data))
#if data > maxi:
maxi = data
print('[%s]: %s\n' % (logconf.name, data))
#Thread(target=self._ramp_motors).start()
def _connection_failed(self, link_uri, msg):
"""Callback when connection initial connection fails (i.e no Crazyflie
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
Crazyflie moves out of range)"""
print('Connection to %s lost: %s' % (link_uri, msg))
def _disconnected(self, link_uri):
"""Callback when the Crazyflie is disconnected (called in all cases)"""
print('Disconnected from %s' % link_uri)
self.is_connected = False
#def _connected(self, link_uri):
def _ramp_motors(self):
global maxi
#j = int(maxi[0])*5000
j = int(maxi['mag.z']) * 4000
if j < 0:
j = 0
elif j > 60000:
j = 60000
#for item in maxi:
# j = int(item)
thrust_mult = 1
thrust_step = 500
thrust = 20000
pitch = 0
roll = 0
yawrate = 0
# Unlock startup thrust protection
self._cf.commander.send_setpoint(0, 0, 0, 0)
print j
#time.sleep(0.1)
self._cf.commander.send_setpoint(roll, pitch, yawrate, j)
time.sleep(0.1)
def default_log_config(sample_time_ms=10):
config = LogConfig(name='Kalman Position and Velocity',
period_in_ms=sample_time_ms)
config.add_variable(CFUtil.KEY_X, 'float')
config.add_variable(CFUtil.KEY_Y, 'float')
config.add_variable(CFUtil.KEY_Z, 'float')
config.add_variable(CFUtil.KEY_DX, 'float')
config.add_variable(CFUtil.KEY_DY, 'float')
config.add_variable(CFUtil.KEY_DZ, 'float')
config.add_variable(CFUtil.KEY_BAT, 'uint16_t')
return config
class UpDown:
"""docstring for UpDown"""
def __init__(self, link_uri):
self._cf = Crazyflie()
self._cf.connected.add_callback(self._connected)
self._cf.disconnected.add_callback(self._disconnected)
self._cf.connection_failed.add_callback(self._connection_failed)
self._cf.connection_lost.add_callback(self._connection_lost)
self._logger = LogConfig(name='Logger', period_in_ms=10)
self._logger.add_variable('acc.x', 'float')
self._logger.add_variable('acc.y', 'float')
self._logger.add_variable('acc.z', 'float')
self._logger.add_variable('acc.zw', 'float')
self._acc_x = 0
self._acc_y = 0
self._acc_z = 0
self._acc_zw = 0.0001
self.log = []
self.pidLog = []
self.acc_pid_x = None
self.acc_pid_y = None
self.acc_pid_z = None
self.vel_pid_x = None
self.vel_pid_y = None
self.vel_pid_z = None
print("Connecting to %s" % link_uri)
self._cf.open_link(link_uri)
self.is_connected = True
self.exit = False
self.init = False
Thread(target=self._exit_task).start()
Thread(target=self._run_task).start()
def _connected(self, link_uri):
""" This callback is called form the Crazyflie API when a Crazyflie
has been connected and the TOCs have been downloaded."""
self.initTime = time.clock()
# self.log_file = open('log.txt', 'w')
# self.pid_log = open('pid.txt', 'w')
# self.log_file.write('[\n')
# self.pid_log.write('[\n')
try:
self._cf.log.add_config(self._logger)
# This callback will receive the data
self._logger.data_received_cb.add_callback(self._acc_log_data)
# This callback will be called on errors
self._logger.error_cb.add_callback(self._acc_log_error)
# Start the logging
self._logger.start()
except KeyError as e:
print('Could not start log configuration,'
'{} not found in TOC'.format(str(e)))
except AttributeError:
print('Could not add Logger log config, bad configuration.')
def _acc_log_error(self, logconf, msg):
"""Callback from the log API when an error occurs"""
print('Error when logging %s: %s' % (logconf.name, msg))
def _acc_log_data(self, timestamp, data, logconf):
"""Callback from the log API when data arrives"""
self._acc_x = float(data['acc.x'])
self._acc_y = float(data['acc.y'])
self._acc_z = float(data['acc.z'])
self._acc_zw = data['acc.zw']
# self.log_file.write(json.dumps(data, sort_keys=True) + ",\n")
self.log.append(data)
self.init = True
def _connection_failed(self, link_uri, msg):
"""Callback when connection initial connection fails (i.e no Crazyflie
at the specified address)"""
print('Connection to %s failed: %s' % (link_uri, msg))
self.is_connected = False
# self.log_file.write(']')
# self.pid_log.write(']')
# self.log_file.close()
# self.pid_log.close()
with open('log.txt', 'w') as logFile:
json.dump(self.log, logFile)
with open('pid.txt', 'w') as pidFile:
json.dump(self.pidLog, pidFile)
def _connection_lost(self, link_uri, msg):
"""Callback when disconnected after a connection has been made (i.e
Crazyflie moves out of range)"""
print('Connection to %s lost: %s' % (link_uri, msg))
# self.log_file.write(']')
# self.pid_log.write(']')
# self.log_file.close()
# self.pid_log.close()
with open('log.txt', 'w') as logFile:
json.dump(self.log, logFile)
with open('pid.txt', 'w') as pidFile:
json.dump(self.pidLog, pidFile)
def _disconnected(self, link_uri):
"""Callback when the Crazyflie is disconnected (called in all cases)"""
print('Disconnected from %s' % link_uri)
self.is_connected = False
with open('log.txt', 'w') as logFile:
json.dump(self.log, logFile)
with open('pid.txt', 'w') as pidFile:
json.dump(self.pidLog, pidFile)
def _exit_task(self):
while not self.exit:
inp = int(input('Want to exit? [NO:0/YES:1]\n'))
if inp == 1:
self.exit = 1
def _run_task(self):
print("Running thread")
self._cf.commander.send_setpoint(0, 0, 0, 0)
while not self.exit:
if not self.init:
self.acc_pid_x = pid.PID(0, 0, 4.4, 0.05, 0.1, -1, 1)
self.acc_pid_y = pid.PID(0, 0, 4.4, 0.05, 0.1, -1, 1)
self.acc_pid_z = pid.PID(0, 0, 100, 0.0005, 0.1, 0.4, 2)
# Thread(target=self._log_pid).start()
continue
inp_acc_x = self._acc_x
self.out_acc_x, self.err_acc_x = self.acc_pid_x.compute(inp_acc_x)
self.setPitch = -self.out_acc_x*10
inp_acc_y = self._acc_y
self.out_acc_y, self.err_acc_y = self.acc_pid_y.compute(inp_acc_y)
self.setRoll = self.out_acc_y*10
inp_acc_z = self._acc_zw
self.out_acc_z, self.err_acc_z = self.acc_pid_z.compute(self._acc_zw)
self.setThrust = int(60000*self.out_acc_z/2)
self._thrust = self.setThrust
# data = {"IN_X": inp_acc_x, "OUT_X": out_acc_x,
# "IN_Y": inp_acc_y, "OUT_Y": out_acc_y,
# "IN_Z": self._acc_zw, "OUT_Z": out_acc_z,
# "OUT_Roll": setRoll, "OUT_Pitch": setPitch,
# "OUT_Thrust": setThrust}
# self.pidLog.append(data)
# self.pid_log.write(json.dumps(data, sort_keys=True) + ",\n")
self._cf.commander.send_setpoint(
self.setRoll, self.setPitch, 0, self._thrust)
time.sleep(0.01)
# self.log_file.write(']')
# self.pid_log.write(']')
# self.log_file.close()
# self.pid_log.close()
self._cf.commander.send_setpoint(0, 0, 0, 0)
time.sleep(1)
self._cf.close_link()
def _log_pid(self):
data = {"IN_X": self._acc_x, "OUT_X": self.out_acc_x,
"IN_Y": self._acc_y, "OUT_Y": self.out_acc_y,
"IN_Z": self._acc_zw, "OUT_Z": self.out_acc_z,
"OUT_Roll": self.setRoll, "OUT_Pitch": self.setPitch,
"OUT_Thrust": self.setThrust}
self.pidLog.append(data)
class Comm:
def __init__(self, link_uri):
""" Initialize and run the test with the specified link_uri """
self._cf = Crazyflie()
self.mc = MotionCommander(self._cf, default_height=1.05)
self._cf.connected.add_callback(self._connected)
self._cf.disconnected.add_callback(self._disconnected)
self._cf.connection_failed.add_callback(self._connection_failed)
self._cf.connection_lost.add_callback(self._connection_lost)
self._cf.open_link(link_uri)
print('Connecting to %s' % link_uri)
self.is_connected = True
def _connected(self, link_uri):
""" This callback is called from the Crazyflie API when a Crazyflie
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')
self._log_conf = LogConfig(name="Accel", period_in_ms=10)
self._log_conf.add_variable('acc.x', 'float')
self._log_conf.add_variable('acc.y', 'float')
self._log_conf.add_variable('acc.z', 'float')
# Adding the configuration cannot be done until a Crazyflie is
# connected, since we need to check that the variables we
# would like to log are in the TOC.
try:
self._cf.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()
self._log = self._cf.log.add_config(self._log_conf)
if self._log_conf is not None:
self._log_conf.data_received_cb.add_callback(
self._log_accel_data)
self._log_conf.start()
else:
print("acc.x/y/z not found in log TOC")
except KeyError as e:
print('Could not start log configuration,'
'{} not found in TOC'.format(str(e)))
except AttributeError:
print('Could not add log config, bad configuration.')
# Start a separate thread to do the motor test.
# Do not hijack the calling thread!
self.is_connected = True
worker = Thread(target=self.flip_test)
worker.start()
def _log_accel_data(self, timestamp, data, logconf):
#print('[%d] Accelerometer: x=%.2f, y=%.2f, z=%.2f' %
# (timestamp, data['acc.x'], data['acc.y'], data['acc.z']))
"""This is from the basiclog file"""
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 Crazyflie
at the specified address)"""
print('Connection to %s failed: %s' % (link_uri, msg))
def _connection_lost(self, link_uri, msg):
"""Callback when disconnected after a connection has been made (i.e
Crazyflie moves out of range)"""
print('Connection to %s lost: %s' % (link_uri, msg))
def _disconnected(self, link_uri):
"""Callback when the Crazyflie is disconnected (called in all cases)"""
print('Disconnected from %s' % link_uri)
def flip_test(self):
# Below is attempt to implememnt algorithm found in https://www.research-collection.ethz.ch/bitstream/handle/20.500.11850/151399/eth-165-01.pdf?sequence=1
# Currently unknown how the provided initial values can make physical sense given the parameters of the Crazyflie (or any drone?)
# U1 = 0.9 * beta_max
# U2 = 0.5 * (beta_max + beta_min)
# U3 = beta_min
# U4 = U2
# U5 = U1
# theta_dd1 = -(beta_max - U1) / (alpha * L)
# theta_dd2 = (beta_max - beta_min) / (2.0 * alpha * L)
# theta_dd3 = 0.0
# theta_dd4 = -theta_dd2
# theta_dd5 = (beta_max - U5) / (alpha * L)
# T3 = (2 * pi - (theta_d_max ** 2) / (2 * theta_dd2))
# T2 = (theta_d_max - theta_dd1 * T1)
print 'Enter to take off'
c = raw_input()
self.mc.take_off()
print 'Enter to flip'
c = raw_input()
print 'Beginning test...'
# Accelerate up
tic = time.time()
while time.time() - tic < 0.1:
self.mc._cf.commander.send_setpoint(0, 0, 0, max_thrust)
time.sleep(0.01)
# Max differential thrust
tic = time.time()
while time.time() - tic < 0.14:
self.mc._cf.commander.send_setpoint(360 * 8, 0, 0, max_thrust)
time.sleep(0.01)
# Coast
time.sleep(0.01)
# Stop rotate
tic = time.time()
while time.time() - tic < 0.15:
self.mc._cf.commander.send_setpoint(-360 * 8, 0, 0, max_thrust)
time.sleep(0.01)
# Accelerate up
tic = time.time()
while time.time() - tic < 0.9:
self.mc._cf.commander.send_setpoint(0, 0, 0, max_thrust)
time.sleep(0.01)
while True:
self.mc.stop()
time.sleep(0.01)
def main():
# Initialize the low-level drivers (don't list the debug drivers)
cflib.crtp.init_drivers(enable_debug_driver=False)
# Scan for Crazyflies and use the first one found
print('Scanning interfaces for Crazyflies...')
available = cflib.crtp.scan_interfaces()
print('Crazyflies found:')
for i in available:
print(i[0])
if len(available) == 0:
print('No Crazyflies found, cannot run example')
else:
#create log config
if (small_log):
logConfig_stab = LogConfig(name='Stabilizer',
period_in_ms=log_period)
logConfig_stab.add_variable('stabilizer.roll', 'float')
logConfig_stab.add_variable('stabilizer.pitch', 'float')
logConfig_stab.add_variable('stabilizer.yaw', 'float')
logConfig_stab.add_variable('stabilizer.thrust', 'float')
logConfig_state = LogConfig(name='State', period_in_ms=log_period)
logConfig_state.add_variable('stateEstimate.x', 'float')
logConfig_state.add_variable('stateEstimate.y', 'float')
logConfig_state.add_variable('stateEstimate.z', 'float')
"""
logConfig_target = LogConfig(name='ControlTarget', period_in_ms=log_period)
logConfig_target.add_variable('ctrltarget.roll', 'float')
logConfig_target.add_variable('ctrltarget.pitch', 'float')
logConfig_target.add_variable('ctrltarget.yaw', 'float')
"""
logConfig_mpow = LogConfig(name='MotorPower',
period_in_ms=log_period)
logConfig_mpow.add_variable('motor.m1', 'int32_t')
logConfig_mpow.add_variable('motor.m2', 'int32_t')
logConfig_mpow.add_variable('motor.m3', 'int32_t')
logConfig_mpow.add_variable('motor.m4', 'int32_t')
if (big_log):
logConfig_all = LogConfig(name='all', period_in_ms=log_period)
logConfig_all.add_variable('stabilizer.thrust', 'float')
logConfig_all.add_variable('motor.m1', 'int32_t')
logConfig_all.add_variable('motor.m2', 'int32_t')
logConfig_all.add_variable('motor.m3', 'int32_t')
logConfig_all.add_variable('motor.m4', 'int32_t')
with SyncCrazyflie(URI) as scf:
cf = scf.cf
#create logger
if (small_log):
logger_stab = SyncLogger(scf, logConfig_stab)
logger_state = SyncLogger(scf, logConfig_state)
logger_mpow = SyncLogger(scf, logConfig_mpow)
logger_stab.connect()
logger_state.connect()
logger_mpow.connect()
#logger_target = SyncLogger(scf, logConfig_target)
#logger_target.connect()
if (big_log):
logger_all = SyncLogger(scf, logConfig_all)
logger_all.connect()
cf.param.set_value('kalman.resetEstimation', '1')
time.sleep(0.1)
cf.param.set_value('kalman.resetEstimation', '0')
time.sleep(2)
for y in range(10):
cf.commander.send_hover_setpoint(0, 0, 0, y / 20)
time.sleep(0.1)
#start point logging
if (small_log):
log_start = logger_stab.qsize()
else:
log_start = logger_all.qsize()
for i in range(50):
print_message("Going " + str(i))
cf.commander.send_hover_setpoint(0, 0, 0, 0.3)
time.sleep(0.1)
#end point logging
if (small_log):
log_end = logger_stab.qsize()
else:
log_end = logger_all.qsize()
log_count = log_end - log_start
print_message("Start:{}, end:{}".format(log_start, log_end))
print_message("To log {} lines.".format(log_end - log_start + 1))
for _ in range(10):
cf.commander.send_hover_setpoint(0, 0, 0, 0.2)
time.sleep(0.1)
cf.commander.send_hover_setpoint(0, 0, 0, 0.1)
time.sleep(0.2)
cf.commander.send_stop_setpoint() #land
time.sleep(1.5)
print_message("Landed, start logging...")
#write log files
if (small_log):
with open(log_file_name(logConfig_stab), "w") as f:
log_count = write_out_log(logger_stab, f, log_start,
log_end)
print_message("logged stabilizer")
with open(log_file_name(logConfig_state), "w") as f:
log_count = write_out_log(logger_state, f, log_start,
log_end)
print_message("logged state")
"""
with open(log_file_name(logConfig_target), "w") as f:
log_count = write_out_log(logger_target, f, log_start, log_end)
print_message("logged control target")
"""
with open(log_file_name(logConfig_mpow), "w") as f:
log_count = write_out_log(logger_mpow, f, log_start,
log_end)
print_message("logged motor power")
logger_stab.disconnect()
logger_state.disconnect()
logger_mpow.disconnect()
#logger_target.disconnect()
if (big_log):
with open(log_file_name(logConfig_all), "w") as f:
log_count = write_out_log(logger_all, f, log_start,
log_end)
logger_all.disconnect()
print_message("Done logging")
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))
if self.helper.cf.mem.ow_search(vid=0xBC, pid=0x01):
self._led_ring_effect.setEnabled(True)
self._led_ring_headlight.setEnabled(True)
def connected(self, linkURI):
self.packetsSinceConnection = 0
self.link_status = "Connected"
print("Connected") #DEBUG
"""
Configure the logger and start recording.
The logging variables are added one after another to the logging
configuration. Then the configuration is used to create a log packet
which is cached on the Crazyflie. If the log packet is None, the
program exits. Otherwise the logging packet receives a callback when
it receives data, which prints the data from the logging packet's
data dictionary as logging info.
"""
lgM = LogConfig("logM", LOGVARS_MOTOR_INTERVALL)
for f in LOGVARS_MOTOR:
lgM.add_variable(f)
self.crazyflie.log.add_config(lgM)
lgSt = LogConfig("logSt", LOGVARS_STABILIZER_INTERVALL)
for f in LOGVARS_STABILIZER:
lgSt.add_variable(f)
self.crazyflie.log.add_config(lgSt)
lgAcc = LogConfig("logAcc", LOGVARS_ACC_INTERVALL)
for f in LOGVARS_ACC:
lgAcc.add_variable(f)
self.crazyflie.log.add_config(lgAcc)
lgBa = LogConfig("logBa", LOGVARS_BARO_INTERVALL)
for f in LOGVARS_BARO:
lgBa.add_variable(f)
self.crazyflie.log.add_config(lgBa)
lgSy = LogConfig("logSy", LOGVARS_SYSTEM_INTERVALL)
for f in LOGVARS_SYSTEM:
lgSy.add_variable(f)
self.crazyflie.log.add_config(lgSy)
if (lgM.valid and lgSt.valid and lgAcc.valid and lgBa.valid and lgSy.valid):
lgM.data_received_cb.add_callback(self.on_log_data_motor)
lgSt.data_received_cb.add_callback(self.on_log_data_stabilizer)
lgAcc.data_received_cb.add_callback(self.on_log_data_acc)
lgBa.data_received_cb.add_callback(self.on_log_data_baro)
lgSy.data_received_cb.add_callback(self.on_log_data_system)
lgM.error_cb.add_callback(self.onLogError)
lgSt.error_cb.add_callback(self.onLogError)
lgAcc.error_cb.add_callback(self.onLogError)
lgBa.error_cb.add_callback(self.onLogError)
lgSy.error_cb.add_callback(self.onLogError)
lgM.start()
lgSt.start()
lgAcc.start()
lgBa.start()
lgSy.start()
print("Crazyflie sensor log successfully started")
rospy.loginfo("Crazyflie sensor log successfully started")
else:
rospy.logerror("Error while starting crazyflie sensr logs")
logger.warning("Could not setup logconfiguration after connection!")
print("Logging failed")
class FlightTab(Tab, flight_tab_class):
uiSetupReadySignal = pyqtSignal()
_motor_data_signal = pyqtSignal(int, object, object)
_imu_data_signal = pyqtSignal(int, object, object)
_althold_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)
_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.althold_updated.add_callback(
lambda enabled: self.helper.cf.param.set_value("flightmode.althold", enabled))
self._imu_data_signal.connect(self._imu_data_received)
self._baro_data_signal.connect(self._baro_data_received)
self._althold_data_signal.connect(self._althold_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.crazyflieXModeCheckbox.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.crazyflieXModeCheckbox.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.helper.cf.param.add_update_callback(
group="flightmode", name="althold",
cb=(lambda name, enabled:
self.helper.inputDeviceReader.enable_alt_hold(eval(enabled))))
self._ledring_nbr_effects = 0
self.helper.cf.param.add_update_callback(
group="ring",
name="neffect",
cb=(lambda name, value: self._set_neffect(eval(value))))
self.helper.cf.param.add_update_callback(
group="imu_sensors",
cb=self._set_available_sensors)
self.helper.cf.param.all_updated.add_callback(self._ring_populate_dropdown)
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 _set_neffect(self, n):
self._ledring_nbr_effects = n
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():
self.actualASL.setText(("%.2f" % data["baro.aslLong"]))
self.ai.setBaro(data["baro.aslLong"])
def _althold_data_received(self, timestamp, data, logconf):
if self.isVisible():
target = data["altHold.target"]
if target>0:
if not self.targetASL.isEnabled():
self.targetASL.setEnabled(True)
self.targetASL.setText(("%.2f" % target))
self.ai.setHover(target)
elif self.targetASL.isEnabled():
self.targetASL.setEnabled(False)
self.targetASL.setText("Not set")
self.ai.setHover(0)
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"])
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))
if self.helper.cf.mem.ow_search(vid=0xBC, pid=0x01):
self._led_ring_effect.setEnabled(True)
self._led_ring_headlight.setEnabled(True)
def _set_available_sensors(self, name, available):
logger.info("[%s]: %s", name, available)
available = eval(available)
if ("HMC5883L" in name):
if (not available):
self.actualASL.setText("N/A")
self.actualASL.setEnabled(False)
else:
self.actualASL.setEnabled(True)
self.helper.inputDeviceReader.set_alt_hold_available(available)
if (not self.logBaro and not self.logAltHold):
# The sensor is available, set up the logging
self.logBaro = LogConfig("Baro", 200)
self.logBaro.add_variable("baro.aslLong", "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))
self.logAltHold = LogConfig("AltHold", 200)
self.logAltHold.add_variable("altHold.target", "float")
try:
self.helper.cf.log.add_config(self.logAltHold)
self.logAltHold.data_received_cb.add_callback(
self._althold_data_signal.emit)
self.logAltHold.error_cb.add_callback(
self._log_error_signal.emit)
self.logAltHold.start()
except KeyError as e:
logger.warning(str(e))
except AttributeError:
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.actualASL.setText("")
self.targetASL.setText("Not Set")
self.targetASL.setEnabled(False)
self.actualASL.setEnabled(False)
self.clientXModeCheckbox.setEnabled(False)
self.logBaro = None
self.logAltHold = None
self._led_ring_effect.setEnabled(False)
self._led_ring_headlight.setEnabled(False)
def minMaxThrustChanged(self):
self.helper.inputDeviceReader.min_thrust = self.minThrust.value()
self.helper.inputDeviceReader.max_thrust = self.maxThrust.value()
if (self.isInCrazyFlightmode == 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 == 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 == 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 == 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" % roll))
self.targetPitch.setText(("%0.2f" % pitch))
self.targetYaw.setText(("%0.2f" % 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)
@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:
self._ring_effect += 1
if self._ring_effect > self._ledring_nbr_effects:
self._ring_effect = 0
self.helper.cf.param.set_value("ring.effect", str(self._ring_effect))
def alt2_updated(self, state):
self.helper.cf.param.set_value("ring.headlightEnable", str(state))
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
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"}
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, QVariant(i))
self._led_ring_effect.setCurrentIndex(current)
self._led_ring_effect.currentIndexChanged.connect(self._ring_effect_changed)
self.helper.cf.param.add_update_callback(group="ring",
name="effect",
cb=self._ring_effect_updated)
def _ring_effect_changed(self, index):
i = self._led_ring_effect.itemData(index).toInt()[0]
logger.info("Changed effect to {}".format(i))
if i != self.helper.cf.param.values["ring"]["effect"]:
self.helper.cf.param.set_value("ring.effect", str(i))
def _ring_effect_updated(self, name, value):
self._led_ring_effect.setCurrentIndex(int(value))
def _connected(self, link_uri):
print('Connected to %s' % link_uri)
lg_pose = LogConfig(name='test', period_in_ms=50)
lg_pose.add_variable('stabilizer.roll', 'float')
lg_pose.add_variable('stabilizer.pitch', 'float')
lg_pose.add_variable('stabilizer.yaw', 'float')
lg_pose.add_variable('stateEstimate.x', 'float')
lg_pose.add_variable('stateEstimate.y', 'float')
lg_pose.add_variable('stateEstimate.z', 'float')
lg_lh_master = LogConfig(name='master', period_in_ms=50)
lg_lh_master.add_variable('lighthouse.angle0x', 'float')
lg_lh_master.add_variable('lighthouse.angle0y', 'float')
lg_lh_master.add_variable('lighthouse.angle0x_3', 'float')
lg_lh_master.add_variable('lighthouse.angle0y_3', 'float')
lg_lh_master2 = LogConfig(name='master2', period_in_ms=50)
lg_lh_master2.add_variable('lighthouse.angle0x_1', 'float')
lg_lh_master2.add_variable('lighthouse.angle0y_1', 'float')
lg_lh_master2.add_variable('lighthouse.angle0x_2', 'float')
lg_lh_master2.add_variable('lighthouse.angle0y_2', 'float')
lg_lh_slave = LogConfig(name='slave', period_in_ms=50)
lg_lh_slave.add_variable('lighthouse.angle1x', 'float')
lg_lh_slave.add_variable('lighthouse.angle1y', 'float')
lg_lh_slave.add_variable('lighthouse.angle1x_3', 'float')
lg_lh_slave.add_variable('lighthouse.angle1y_3', 'float')
lg_lh_slave2 = LogConfig(name='slave2', period_in_ms=50)
lg_lh_slave2.add_variable('lighthouse.angle1x_1', 'float')
lg_lh_slave2.add_variable('lighthouse.angle1y_1', 'float')
lg_lh_slave2.add_variable('lighthouse.angle1x_2', 'float')
lg_lh_slave2.add_variable('lighthouse.angle1y_2', 'float')
self._cf.log.add_config(lg_pose)
lg_pose.data_received_cb.add_callback(self.data_receivedPose)
lg_pose.start()
self._cf.log.add_config(lg_lh_master)
lg_lh_master.data_received_cb.add_callback(self.data_receivedSensor)
lg_lh_master.start()
self._cf.log.add_config(lg_lh_master2)
lg_lh_master2.data_received_cb.add_callback(self.data_receivedSensor)
lg_lh_master2.start()
self._cf.log.add_config(lg_lh_slave)
lg_lh_slave.data_received_cb.add_callback(self.data_receivedSensor)
lg_lh_slave.start()
self._cf.log.add_config(lg_lh_slave2)
lg_lh_slave2.data_received_cb.add_callback(self.data_receivedSensor)
lg_lh_slave2.start()
self.read_geo_data()
if __name__ == '__main__':
# Initialize the low-level drivers (don't list the debug drivers)
cflib.crtp.init_drivers(enable_debug_driver=False)
# Scan for Crazyflies and use the first one found
print('Scanning interfaces for Crazyflies...')
available = cflib.crtp.scan_interfaces()
print('Crazyflies found:')
for i in available:
print(i[0])
if len(available) == 0:
print('No Crazyflies found, cannot run example')
else:
lg_stab = LogConfig(name='stateEstimator', 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')
cf = Crazyflie(rw_cache='./cache')
with SyncCrazyflie(available[0][0], cf=cf) as scf:
with SyncLogger(scf, 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))
T = 5
pontos = [0, 20, -15, -20, 45, -37, 10]
# Only output errors from the logging framework
logging.basicConfig(level=logging.ERROR)
if __name__ == '__main__':
# Initialize the low-level drivers (don't list the debug drivers)
cflib.crtp.init_drivers(enable_debug_driver=False)
available = cflib.crtp.scan_interfaces()
if len(available) == 0:
print('No Crazyflies found, cannot run example')
else:
period_in_ms = 10
lg_stab = LogConfig(name='Stabilizer', period_in_ms=10)
lg_stab.add_variable('pm.batteryLevel', 'uint8_t')
#Realimentação PID rate
lg_stab.add_variable('gyro.xRaw', 'float')
lg_stab.add_variable('gyro.yRaw', 'float')
#lg_stab.add_variable('gyro.zRaw', 'float')
#lg_stab.add_variable('stateEstimateZ.rateRoll', 'int16_t')
lg_stab.add_variable('stateEstimateZ.ratePitch', 'int16_t')
#lg_stab.add_variable('stateEstimateZ.rateYaw', 'int16_t')
lg_stab.add_variable('controller.cmd_roll', 'float')
lg_stab.add_variable('controller.cmd_pitch', 'float')
#lg_stab.add_variable('controller.cmd_yaw', 'float')
i = 0
#print("antes")
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))
self._populate_assisted_mode_dropdown()
class LoggingExample:
"""
Simple logging example class that logs the Stabilizer from a supplied
link uri and disconnects after 5s.
"""
def __init__(self, link_uri):
""" Initialize and run the example with the specified link_uri """
# Create a Crazyflie object without specifying any cache dirs
self._cf = Crazyflie()
# Connect some callbacks from the Crazyflie API
self._cf.connected.add_callback(self._connected)
self._cf.disconnected.add_callback(self._disconnected)
self._cf.connection_failed.add_callback(self._connection_failed)
self._cf.connection_lost.add_callback(self._connection_lost)
print('Connecting to %s' % link_uri)
# Try to connect to the Crazyflie
self._cf.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 Crazyflie API when a Crazyflie
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 Crazyflie is
# connected, since we need to check that the variables we
# would like to log are in the TOC.
try:
self._cf.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(5, self._cf.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 Crazyflie
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
Crazyflie moves out of range)"""
print('Connection to %s lost: %s' % (link_uri, msg))
def _disconnected(self, link_uri):
"""Callback when the Crazyflie is disconnected (called in all cases)"""
print('Disconnected from %s' % link_uri)
self.is_connected = False
class LoggingExample:
"""
Simple logging example class that logs the Stabilizer from a supplied
link uri and disconnects after 5s.
"""
def __init__(self, link_uri):
""" Initialize and run the example with the specified link_uri """
self._cf = Crazyflie(rw_cache='./cache')
# Connect some callbacks from the Crazyflie API
self._cf.connected.add_callback(self._connected)
self._cf.disconnected.add_callback(self._disconnected)
self._cf.connection_failed.add_callback(self._connection_failed)
self._cf.connection_lost.add_callback(self._connection_lost)
self.pubLS = rospy.Publisher('cf_tof_ranges', LaserScan, queue_size=10)
self.pubRup = rospy.Publisher('cf_tof_up', Range, queue_size=10)
rospy.init_node('talker', anonymous=True)
self.pubLS_msg = LaserScan()
self.pubLS_msg.angle_min = 0.0
self.pubLS_msg.angle_max = 3.14159 / 2 * 3
self.pubLS_msg.angle_increment = 3.14159 / 2
self.pubLS_msg.range_min = 0.0
self.pubLS_msg.range_max = 100.0
self.pubLS_msg.header.frame_id = 'cf_base'
self.range_msg_up = Range()
self.range_msg_up.header.frame_id = 'cf_base'
self.range_msg_up.min_range = 0.0
self.range_msg_up.max_range = 100.0
self.range_msg_up.field_of_view = 0.131 # 7.5 degrees
print('Connecting to %s' % link_uri)
# Try to connect to the Crazyflie
self._cf.open_link(link_uri)
# Variable used to keep main loop occupied until disconnect
self.is_connected = True
signal.signal(signal.SIGINT, self._signal_handler)
def _connected(self, link_uri):
""" This callback is called form the Crazyflie API when a Crazyflie
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='RANGES', period_in_ms=10)
self._lg_stab.add_variable('range.back', 'uint16_t')
self._lg_stab.add_variable('range.left', 'uint16_t')
self._lg_stab.add_variable('range.front', 'uint16_t')
self._lg_stab.add_variable('range.right', 'uint16_t')
self._lg_stab.add_variable('range.up', 'uint16_t')
# Adding the configuration cannot be done until a Crazyflie is
# connected, since we need to check that the variables we
# would like to log are in the TOC.
try:
self._cf.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.')
# except KeyboardInterrupt:
# rethrow()
# Start a timer to disconnect in 10s
# t = Timer(5, self._cf.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))
#rangeStr = '[%d][%s]: %s' % (timestamp, logconf.name, data)
#self.pub.publish(rangeStr)
now = rospy.get_rostime()
self.pubLS_msg.header.stamp.secs = now.secs
self.pubLS_msg.header.stamp.nsecs = now.nsecs
self.pubLS_msg.ranges = [
data['range.front'] * 0.001, data['range.left'] * 0.001,
data['range.back'] * 0.001, data['range.right'] * 0.001
]
#self.pubLS.publish(self.pubLS_msg)
self.range_msg_up.range = data['range.up'] * 0.001
self.range_msg_up.header.stamp.secs = now.secs
self.range_msg_up.header.stamp.nsecs = now.nsecs
#self.pubRup.publish(self.range_msg_up)
def _connection_failed(self, link_uri, msg):
"""Callback when connection initial connection fails (i.e no Crazyflie
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
Crazyflie moves out of range)"""
print('Connection to %s lost: %s' % (link_uri, msg))
def _disconnected(self, link_uri):
"""Callback when the Crazyflie is disconnected (called in all cases)"""
print('Disconnected from %s' % link_uri)
self.is_connected = False
def _signal_handler(self, sig, frame):
print('You pressed Ctrl+C!')
self.is_connected = False
self._cf.close_link()
sys.exit(0)
class LoggingExample:
"""
Simple logging example class that logs the Stabilizer from a supplied
link uri and disconnects after 5s.
"""
def __init__(self, link_uri):
""" Initialize and run the example with the specified link_uri """
self._cf = Crazyflie(rw_cache='./cache')
# Connect some callbacks from the Crazyflie API
self._cf.connected.add_callback(self._connected)
self._cf.disconnected.add_callback(self._disconnected)
self._cf.connection_failed.add_callback(self._connection_failed)
self._cf.connection_lost.add_callback(self._connection_lost)
print('Connecting to %s' % link_uri)
# Try to connect to the Crazyflie
self._cf.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 Crazyflie API when a Crazyflie
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='relative_pos', period_in_ms=200)
self._lg_stab.add_variable('relative_pos.rlX0', 'float')
self._lg_stab.add_variable('relative_pos.rlY0', 'float')
self._lg_stab.add_variable('relative_pos.rlYaw0', 'float')
# Adding the configuration cannot be done until a Crazyflie is
# connected, since we need to check that the variables we
# would like to log are in the TOC.
try:
self._cf.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(1000, self._cf.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"""
global rlxCF, rlyCF, rlyawCF
rlxCF = data['relative_pos.rlX0']
rlyCF = data['relative_pos.rlY0']
rlyawCF = data['relative_pos.rlYaw0']
def _connection_failed(self, link_uri, msg):
"""Callback when connection initial connection fails (i.e no Crazyflie
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
Crazyflie moves out of range)"""
print('Connection to %s lost: %s' % (link_uri, msg))
def _disconnected(self, link_uri):
"""Callback when the Crazyflie is disconnected (called in all cases)"""
print('Disconnected from %s' % link_uri)
self.is_connected = False
class cf_logging:
def __init__(self, uri):
self._cf = Crazyflie(rw_cache='./cache')
# Define callback function variable
self._cf.connected.add_callback(self._connected)
self._cf.disconnected.add_callback(self._disconnected)
self._cf.connection_failed.add_callback(self._connection_failed)
self._cf.connection_lost.add_callback(self._connection_lost)
print('Connecting to %s' % uri)
self._cf.open_link(uri)
self.is_connected = True
def _connected(self, uri):
print(f"Connected to {uri}")
self._lg_stateEstimate = LogConfig(name="StateEstimation",
period_in_ms=10)
self._lg_stateEstimate.add_variable('stateEstimate.x', 'float')
self._lg_stateEstimate.add_variable('stateEstimate.y', 'float')
self._lg_stateEstimate.add_variable('stateEstimate.z', 'float')
self._lg_stateEstimate.add_variable('stateEstimate.vx', 'float')
self._lg_stateEstimate.add_variable('stateEstimate.vy', 'float')
self._lg_stateEstimate.add_variable('stateEstimate.vz', 'float')
try:
self._cf.log.add_config(self._lg_stateEstimate)
self._lg_stateEstimate.data_received_cb.add_callback(
self.dataReceived_callback)
self._lg_stateEstimate.error_cb.add_callback(self.error_callback)
self._lg_stateEstimate.start()
except KeyError as e:
print(f"Could not start log config, {e} not found in TOC")
except AttributeError:
print('Could not add Stabilizer log config, bad configuration.')
# Start a timer to disconnect in 5 seconds
# Timer(5, self._cf.close_link).start()
# Connection callback
def _connection_failed(self, uri, msg):
print(f"Connection to {uri} failed: {msg}.")
def _connection_lost(self, uri, msg):
print(f"Connection to {uri} lost: {msg}.")
def _disconnected(self, uri):
print(f"Disconnect from {uri}.")
self.is_connected = False
# Logging data connect
def error_callback(self, logconf, msg):
print(f"Error when logging {logconf.name}: {msg}")
def dataReceived_callback(self, timestamp, data, logconf):
x = data["stateEstimate.x"]
y = data["stateEstimate.y"]
z = data["stateEstimate.z"]
print(f"\n\nX: {x}")
print(f"\nY: {y}")
print(f"\nZ: {z}")
streamingClient.rigidBodyListener = receiveRigidBodyFrame
streamingClient.run()
URI = 'radio://0/80/250K'
# Only output errors from the logging framework
logging.basicConfig(level=logging.ERROR)
if __name__ == '__main__':
# Initialize the low-level drivers (don't list the debug drivers)
cflib.crtp.init_drivers(enable_debug_driver=False)
cf = Crazyflie(rw_cache='./cache')
lg_states = LogConfig(name='kalman_states', period_in_ms=10)
lg_states.add_variable('kalman_states.ox')
lg_states.add_variable('kalman_states.oy')
with SyncCrazyflie(URI, cf=cf) as scf:
with SyncLogger(scf, lg_states) as logger_states:
with MotionCommander(scf) as motion_commander:
keep_flying = True
char = readchar.readchar()
motion_commander.up(0.5)
time.sleep(1)
while (keep_flying):
if len(pos) > 0:
cf.extpos.send_extpos(pos[0], pos[1], pos[2])
#print("external",pos[1])
for i in available:
print(i[0])
if len(available) == 0:
print('No Crazyflies found, cannot run example')
else:
#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')
#lg_motion = LogConfig(name='Motion', period_in_ms=1000)
#lg_motion.add_variable('motion.deltaX', 'float')
#lg_motion.add_variable('motion.deltaY', 'float')
log_rssi = LogConfig(name='RSSI', period_in_ms=1000)
log_rssi.add_variable('radio.rssi', 'float')
cf = Crazyflie(rw_cache='./cache')
with SyncCrazyflie(available[0][0], cf=cf) as scf:
with SyncLogger(scf, log_rssi) 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))
print(data["radio.rssi"])
logging.basicConfig(level=logging.ERROR)
# Initialize the low-level drivers (don't list the debug drivers)
cflib.crtp.init_drivers(enable_debug_driver=False)
# Scan for Crazyflies and use the first one found
print('Scanning interfaces for Crazyflies...')
available = cflib.crtp.scan_interfaces()
print('Crazyflies found:')
for i in available:
print(i[0])
if len(available) == 0:
print(err_dict["2"])
else:
cf = Crazyflie(rw_cache='./cache')
# Add logging config to logger
lg_stab = LogConfig(name='Stabilizer', period_in_ms=12)
lg_stab.add_variable('pm.vbat', 'float')
lg_stab.add_variable('pm.batteryLevel', 'float')
lg_stab.add_variable('pwm.m1_pwm', 'float')
lg_stab.add_variable('baro.temp', 'float')
with SyncCrazyflie(URI) as scf:
# Define motion commander to crazyflie as mc:
cf2 = CF(scf, available)
cf2_psi = 0 # Get current yaw-angle of cf
cf2_bat = cf2.vbat # Get current battery level of cf
cf2_blevel = cf2.blevel
cf2_pwm = cf2.m1_pwm
cf2_temp = cf2.temp
cf2_phi = 0
cf2_theta = 0
marker_psi = 0 # Set marker angle to zero initially
class LoggingExample:
"""
Simple logging example class that logs the Stabilizer from a supplied
link uri and disconnects after 5s.
"""
def __init__(self, link_uri):
""" Initialize and run the example with the specified link_uri """
self._cf = Crazyflie(rw_cache='./cache')
# Connect some callbacks from the Crazyflie API
self._cf.connected.add_callback(self._connected)
self._cf.disconnected.add_callback(self._disconnected)
self._cf.connection_failed.add_callback(self._connection_failed)
self._cf.connection_lost.add_callback(self._connection_lost)
print('Connecting to %s' % link_uri)
# Try to connect to the Crazyflie
# Lock the connection part to speed up
lock = Lock()
lock.acquire()
self._cf.open_link(link_uri)
lock.release()
# Variable used to keep main loop occupied until disconnect
self.my_connected = False
def _connected(self, link_uri):
""" This callback is called form the Crazyflie API when a Crazyflie
has been connected and the TOCs have been downloaded."""
print('Connected to %s' % link_uri)
self.my_connected = True
# The definition of the logconfig can be made before connecting
# self._lg_stab = LogConfig(name='Stabilizer', period_in_ms=10)
# set acceleration log config
self._lg_acc = LogConfig(name="acc", period_in_ms=10)
self._lg_acc.add_variable('acc.x', 'float')
self._lg_acc.add_variable('acc.y', 'float')
self._lg_acc.add_variable('acc.z', 'float')
# vec, theta = euler2axangle(0, 1.5, 9, 'szyx')
# open file in log directory
# save_path = './log'
curr_time = time.strftime("%Y%m%d-%H%M%S")
# file_name = os.path.join(save_path, 'acc_'+curr_time+'.txt')
file_name = 'acc_' + curr_time + '.txt'
self.f = open(file_name, 'w')
# Adding the configuration cannot be done until a Crazyflie is
# connected, since we need to check that the variables we
# would like to log are in the TOC.
try:
self._cf.log.add_config(self._lg_acc)
# This callback will receive the data
self._lg_acc.data_received_cb.add_callback(self._stab_log_data)
# This callback will be called on errors
self._lg_acc.error_cb.add_callback(self._stab_log_error)
# Start the logging
self._lg_acc.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.')
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"""
self.f.write('[%d][%s]: %s' % (timestamp, logconf.name, data) + "\n")
print('[%d][%s]: %s' % (timestamp, logconf.name, data))
def _connection_failed(self, link_uri, msg):
"""Callback when connection initial connection fails (i.e no Crazyflie
at the speficied address)"""
print('Connection to %s failed: %s' % (link_uri, msg))
self.my_connected = False
# self.f.close()
def _connection_lost(self, link_uri, msg):
"""Callback when disconnected after a connection has been made (i.e
Crazyflie moves out of range)"""
print('Connection to %s lost: %s' % (link_uri, msg))
self.my_connected = False
# self.f.close()
def _disconnected(self, link_uri):
"""Callback when the Crazyflie is disconnected (called in all cases)"""
print('Disconnected from %s' % link_uri)
self.my_connected = False
self.f.close()
def disconnect(self):
"""Disconnect everything and stop logging when called"""
print('Disconnect in 1 second')
t = Timer(1, self._cf.close_link)
t.start()
