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 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 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 _create_log_config(self, rate_ms):
log_config = LogConfig('multiranger', rate_ms)
log_config.add_variable(self.FRONT)
log_config.add_variable(self.BACK)
log_config.add_variable(self.LEFT)
log_config.add_variable(self.RIGHT)
log_config.add_variable(self.UP)
log_config.add_variable(self.DOWN)
log_config.data_received_cb.add_callback(self._data_received)
return log_config
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, 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 _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 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 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')
log_conf.add_variable('stabilizer.yaw', 'float')
scf.cf.log.add_config(log_conf)
log_conf.data_received_cb.add_callback(position_callback)
log_conf.start()
def start_data_printing(scf):
log_conf = LogConfig(name='Yaw', period_in_ms=50)
log_conf.add_variable('stabilizer.roll', 'float')
log_conf.add_variable('stabilizer.pitch', 'float')
log_conf.add_variable('stabilizer.yaw', 'float')
scf.cf.log.add_config(log_conf)
log_conf.data_received_cb.add_callback(position_callback)
log_conf.start()
def start_attitude_printing(scf):
log_conf = LogConfig(name='Stabilizer', period_in_ms=25)
log_conf.add_variable('stabilizer.roll', 'float')
log_conf.add_variable('stabilizer.pitch', 'float')
log_conf.add_variable('stabilizer.yaw', 'float')
scf.cf.log.add_config(log_conf)
log_conf.data_received_cb.add_callback(attitude_callback)
log_conf.start()
def start_position_printing(scf):
if scf.cf.link_uri==URI1:
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_callback1)
log_conf.start()
elif scf.cf.link_uri==URI2:
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_callback2)
log_conf.start()
def waitForEstimator(scf):
print('Waiting for estimator to find position...')
log_config = LogConfig(name='Kalman Variance', period_in_ms=50)
log_config.add_variable('kalman.varPX', 'float')
log_config.add_variable('kalman.varPY', 'float')
log_config.add_variable('kalman.varPZ', 'float')
log_config.add_variable('kalman.stateX', 'float')
log_config.add_variable('kalman.stateY', 'float')
log_config.add_variable('kalman.stateZ', 'float')
var_y_history = [1000] * 10
var_x_history = [1000] * 10
var_z_history = [1000] * 10
threshold = 0.01
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)
x = data['kalman.stateX']
y = data['kalman.stateY']
z = data['kalman.stateZ']
# print("{:6.4f} {:6.4f} {:6.4f}".format(x, y, z))
if (max_x - min_x) < threshold and (
max_y - min_y) < threshold and (max_z - min_z) < threshold:
#print("-- POSITION FOUND --")
print("Found position: (", x, ",", y, ",", z, ")")
break
def start_logging(self, scf):
#with open("ugly_log.txt","a") as file:
# file.write("timestamp,roll,pitch,yaw,height\n")
log_conf = LogConfig(name='logdata', period_in_ms=10)
#-- States
log_conf.add_variable('stateEstimate.x', 'float')
log_conf.add_variable('stateEstimate.y', 'float')
log_conf.add_variable('stateEstimate.z', 'float')
log_conf.add_variable('stabilizer.roll', 'float')
log_conf.add_variable('stabilizer.pitch', 'float')
log_conf.add_variable('stabilizer.yaw', 'float')
log_conf.add_variable('range.zrange', 'uint16_t')
#-- Battery voltage
#log_conf.add_variable('pm.vbat', 'float')
#-- Command
#log_conf.add_variable('posCtl.targetVX','float')
#log_conf.add_variable('posCtl.targetVY','float')
#log_conf.add_variable('posCtl.targetVZ','float')
#param_conf.add_variable('posCtl.VZp')
#param_conf.add_variable('posCtl.VZi')
#param_conf.add_variable('posCtl.VZd')
#logz_conf = LogConfig(name='logzdata', period_in_ms=25)
#logz_conf.add_variable('range.zrange','float')
scf.cf.log.add_config(log_conf)
log_conf.data_received_cb.add_callback(self.log_callback)
#logz_conf.data_received_cb.add_callback(self.logz_callback)
log_conf.start()
def check_battery(scf, name):
"""
Checks the battery level. If the battery is too low, it prints an error
and returns false, otherwise it returns true.
Too low is considered to be 3.4 volts
:param scf: SyncCrazyflie object
:param name: The "Name" for logging
"""
print("Checking battery")
log_config = LogConfig(name='Battery', period_in_ms=500)
log_config.add_variable('pm.vbat', 'float')
with SyncLogger(scf, log_config) as logger:
for log_entry in logger:
print(name + ": battery at " + str(log_entry[1]['pm.vbat']))
if log_entry[1]['pm.vbat'] > 3.4:
print(name + ": Battery at good level")
return True
else:
print(name + ": Battery too low")
return False
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 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)
if (max_x - min_x) < threshold and (
max_y - min_y) < threshold and (max_z - min_z) < threshold:
break
print("Estimator reset.")
def start_ddmotor_printing(scf):
log_conf = LogConfig(name='DDMotor', period_in_ms=25)
log_conf.add_variable('motor.m1', 'float')
log_conf.add_variable('motor.m2', 'float')
log_conf.add_variable('motor.m3', 'float')
log_conf.add_variable('motor.m4', 'float')
log_conf.add_variable('stateEstimate.z', 'float')
scf.cf.log.add_config(log_conf)
log_conf.data_received_cb.add_callback(motor_callback)
log_conf.start()
def start_position_printing(scf):
# This function will also used to initialize
# initial position global: x_init, y_init, z_init, yaw_init
log_conf = LogConfig(name='Position', period_in_ms=500) #500
log_conf.add_variable('kalman.stateX', 'float')
log_conf.add_variable('kalman.stateY', 'float')
log_conf.add_variable('kalman.stateZ', 'float')
log_conf.add_variable('stabilizer.yaw', 'float')
scf.cf.log.add_config(log_conf)
log_conf.data_received_cb.add_callback(position_callback)
log_conf.start()
def run_sequence(scf, sequence):
try:
cf = scf.cf
lpos = LogConfig(name='position', period_in_ms=60)
lpos.add_variable('stateEstimate.x')
lpos.add_variable('stateEstimate.y')
lpos.add_variable('stateEstimate.z')
# 配置
if scf.cf.link_uri == URI:
lpos.add_variable(distance_mapper[URI_fixed])
else:
pass
scf.cf.log.add_config(lpos)
# 回调
if scf.cf.link_uri == URI:
lpos.data_received_cb.add_callback(pos_data)
else:
lpos.data_received_cb.add_callback(pos_data_fixed)
# lpos.start()
# time.sleep(500)
# lpos.stop()
with PositionHlCommander(scf) as pc:
if scf.cf.link_uri == URI:
# 等待二号无人机飞到指定地点后开始做事情
pc.go_to(sequence[0][0], sequence[0][1], sequence[0][2])
time.sleep(2)
for i in range(cyc):
for idx, ele in enumerate(sequence):
if idx == 0:
pass
elif idx == 1:
pc.set_default_velocity(0.5)
pc.go_to(ele[0], ele[1], ele[2])
else:
pc.set_default_velocity(vel)
lpos.start()
pc.go_to(ele[0], ele[1], ele[2])
lpos.stop()
else:
for ele in sequence:
pc.go_to(ele[0], ele[1], ele[2])
time.sleep(ele[3])
if scf.cf.link_uri == URI:
df = pd.DataFrame(
moving_dt,
columns=['x', 'y', 'z', 'distance_UWB', 'distance_lighthouse'])
df.to_csv('./VelocitySwarm_50ms_' + str(vel) + '_far.csv',
index=False)
except Exception as e:
print(e)
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 find_initial_position(scf):
print('Finding intial position and state')
log_config = LogConfig(name='Variance', period_in_ms=100)
log_config.add_variable('stateEstimate.x', 'float')
log_config.add_variable('stateEstimate.y', 'float')
log_config.add_variable('stateEstimate.z', 'float')
log_config.add_variable('stabilizer.yaw', 'float')
with SyncLogger(scf, log_config) as logger:
for log_entry in logger:
data = log_entry[1]
return data['stateEstimate.x'], data['stateEstimate.y'], data['stateEstimate.z'], data['stabilizer.yaw']
def start_position_printing(scf):
"""
This function will also used to initialize initial position
"""
log_conf = LogConfig(name='Position', period_in_ms=50) #500
log_conf.add_variable('kalman.stateX', 'float')
log_conf.add_variable('kalman.stateY', 'float')
log_conf.add_variable('kalman.stateZ', 'float')
log_conf.add_variable('stabilizer.yaw', 'float') # additional
#print('pos: ({}, {}, {}) yaw: ({})'.format(x, y, z, yaw))
scf.cf.log.add_config(log_conf)
log_conf.data_received_cb.add_callback(position_callback)
log_conf.start()
def add_callback_to_singlecf(uri, scf, cf_args):
cflib.crtp.init_drivers(enable_debug_driver=False)
log_conf = LogConfig(name=uri, 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')
log_conf.add_variable('pm.vbat', 'float')
scf.cf.log.add_config(log_conf)
def outer_callback(timestamp, data, logconf):
return CFDispatch.update_cfstatus(timestamp, data, logconf, cf_args, uri)
log_conf.data_received_cb.add_callback(outer_callback)
log_conf.start()
def _hover_test(self):
print("sending initial thrust of 0")
self._cf.commander.send_setpoint(0,0,0,0);
time.sleep(0.5);
print("putting in althold")
self._cf.param.set_value("flightmode.althold","True")
print("Stay in althold for 7s")
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')
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
it=0
self._cf.commander.send_setpoint(0.66,1,0,35000)
time.sleep(2)
#Bug here
#self._cf.param.set_value("flightmode.althold","True")
for log_entry in logger:
print('HI')
timestamp = log_entry[0]
data = log_entry[1]
logconf_name = log_entry[2]
print('[%d][%s]: %s' % (timestamp,logconf_name,data))
#while it<200:
#self._cf.commander.send_setpoint(roll, pitch, yawrate, thrust)
#self._cf.commander.send_setpoint(0.66,-1,0,32767)
self._cf.commander.send_setpoint(0.66,1,0,32767)
self._cf.param.set_value("flightmode.althold","True")
#self._cf.param.set_value("flightmode.poshold","False")
#time.sleep(0.01)
#it+=1
if time.time()>endTime:
print("Close connection")
self._cf.commander.send_setpoint(0,0,0,0)
self._cf.close_link()
break
def connected(self, URI):
print('We are now connected to {}'.format(URI))
# The definition of the logconfig can be made before connecting
lp = LogConfig(name='Position', period_in_ms=100)
lp.add_variable('stateEstimate.x')
lp.add_variable('stateEstimate.y')
lp.add_variable('stateEstimate.z')
lp.add_variable('stabilizer.roll')
lp.add_variable('stabilizer.pitch')
lp.add_variable('stabilizer.yaw')
try:
self.cf.log.add_config(lp)
lp.data_received_cb.add_callback(self.pos_data)
lp.start()
except KeyError as e:
print('Could not start log configuration,'
'{} not found in TOC'.format(str(e)))
except AttributeError:
print('Could not add Position log config, bad configuration.')
def _connected(self, link_uri):
lg = LogConfig("GPS", 1000)
lg.add_variable("gps.lat")
lg.add_variable("gps.lon")
lg.add_variable("gps.hMSL")
lg.add_variable("gps.hAcc")
lg.add_variable("gps.nsat")
lg.add_variable("gps.fixType")
self._cf.log.add_config(lg)
"""When heading & speed are included in the above logging variables, """
""" the CF2 logging becomes overloaded and freezes up. """
# lg.add_variable("gps.gSpeed")
# lg.add_variable("gps.heading")
if lg.valid:
lg.data_received_cb.add_callback(self._log_data_signal.emit)
lg.error_cb.add_callback(self._log_error_signal.emit)
lg.start()
else:
logger.warning("Could not setup logging block for GPS!")
self._max_speed = 0.0
def _connected(self, link_uri):
lg = LogConfig("GPS", 1000)
lg.add_variable("gps.lat")
lg.add_variable("gps.lon")
lg.add_variable("gps.hAcc")
lg.add_variable("gps.hMSL")
lg.add_variable("gps.nsat")
self._cf.log.add_config(lg)
if lg.valid:
lg.data_received_cb.add_callback(self._log_data_signal.emit)
lg.error_cb.add_callback(self._log_error_signal.emit)
lg.start()
else:
logger.warning("Could not setup logging block for GPS!")
self._max_speed = 0.0
def distance_measurement():
for uri in uris:
with SyncCrazyflie(uri, cf=Crazyflie(rw_cache='./cache')) as scf:
lpos = LogConfig(name='rxCNT', period_in_ms=500)
lpos.add_variable('rxCNT.rxcnt10')
lpos.add_variable('rxCNT.rxcnt20')
lpos.add_variable('rxCNT.rxcnt30')
lpos.add_variable('rxCNT.rxcnt40')
scf.cf.log.add_config(lpos)
lpos.data_received_cb.add_callback(pos_data)
lpos.start()
time.sleep(2)
lpos.stop()
df = pd.DataFrame(
dt, columns=['uri', 'rxcnt10', 'rxcnt20', 'rxcnt30', 'rxcnt40'])
df.to_csv('./rxcnt.csv', index=False)
dt.clear()
dt_mapper.clear()
for uri in uris:
with SyncCrazyflie(uri, cf=Crazyflie(rw_cache='./cache')) as scf:
lpos = LogConfig(name='rxCNT', period_in_ms=500)
lpos.add_variable('rangingCNT.rangingcnt10')
lpos.add_variable('rangingCNT.rangingcnt20')
lpos.add_variable('rangingCNT.rangingcnt30')
lpos.add_variable('rangingCNT.rangingcnt40')
scf.cf.log.add_config(lpos)
lpos.data_received_cb.add_callback(pos_data2)
lpos.start()
time.sleep(2)
lpos.stop()
df = pd.DataFrame(dt,
columns=[
'uri', 'rangingcnt10', 'rangingcnt20',
'rangingcnt30', 'rangingcnt40'
])
df.to_csv('./rangingcnt.csv', index=False)
print("done")
return
def startP2PPrinting(scf):
#if scf.cf.link_uri == 'radio://0/80/2M/E7E7E7E7E9':
# return
logConf = LogConfig(name='P2P', period_in_ms=200)
#logConf.add_variable('p2pmsg.vx', 'float')
#logConf.add_variable('p2pmsg.vy', 'float')
#logConf.add_variable('p2pmsg.vz', 'float')
logConf.add_variable('p2pmsg.px', 'float')
logConf.add_variable('p2pmsg.py', 'float')
logConf.add_variable('p2pmsg.pz', 'float')
#logConf.add_variable('p2pmsg.rot', 'float')
#logConf.add_variable('p2pmsg.srssi', 'uint8_t')
logConf.add_variable('p2pmsg.sid', 'uint8_t')
scf.cf.log.add_config(logConf)
logConf.data_received_cb.add_callback(lambda timestamp, data, logconf: P2PCallback(timestamp, data, logconf, scf.cf.link_uri))
logConf.start()
def start_logging(self,scf):
log_conf = LogConfig(name='logdata', period_in_ms=100)
#-- Battery voltage
log_conf.add_variable('pm.vbat', 'float')
#log_conf.add_variable('pm.vbatMV', 'unint16_t')
log_conf.add_variable('pm.state', 'int8_t')
log_conf.add_variable('pm.batteryLevel','uint8_t')
scf.cf.log.add_config(log_conf)
log_conf.data_received_cb.add_callback(self.log_callback)
#logz_conf.data_received_cb.add_callback(self.logz_callback)
log_conf.start()
def log_synchronously():
log_config = LogConfig(name='Stabilizer', period_in_ms=25)
# log_config.add_variable('stabilizer.roll', 'float')
# log_config.add_variable('ctrltarget.pitch', 'float')
# log_config.add_variable('ctrltarget.yaw', 'float')
# log_config.add_variable('stabilizer.thrust', 'float')
log_config.add_variable('stabilizer.yaw', 'float')
# log_config.add_variable('ctrltarget.roll', 'float')
log_config.add_variable('motor.m1', 'float')
log_config.add_variable('motor.m2', 'float')
log_config.add_variable('motor.m3', 'float')
log_config.add_variable('motor.m4', 'float')
print("here1")
le[0].cf.log.add_config(log_config)
log_config.data_received_cb.add_callback(stab_callback)
print("here2")
log_config.start()
def connected(self, linkURI):
# MOTOR & THRUST
lg = LogConfig("Motors", Config().get("ui_update_period"))
lg.add_variable("stabilizer.thrust", "uint16_t")
lg.add_variable("motor.m1")
lg.add_variable("motor.m2")
lg.add_variable("motor.m3")
lg.add_variable("motor.m4")
try:
self.helper.cf.log.add_config(lg)
lg.data_received_cb.add_callback(self._motor_data_signal.emit)
lg.error_cb.add_callback(self._log_error_signal.emit)
lg.start()
except KeyError as e:
logger.warning(str(e))
except AttributeError as e:
logger.warning(str(e))
def add_callback_to_singlecf(uri, scf, cf_arg):
cflib.crtp.init_drivers(enable_debug_driver=False)
log_conf = LogConfig(name=uri, period_in_ms=300)
log_conf.add_variable('kalman.stateX', 'float')
log_conf.add_variable('kalman.stateY', 'float')
log_conf.add_variable('kalman.stateZ', 'float')
log_conf.add_variable('pm.vbat', 'float')
scf.cf.log.add_config(log_conf)
#CFDispatch.ax.scatter(-100, -100, -100, c=CFDispatch.color_dic[uri], alpha=0.4, label=uri) # 散点图
#CFDispatch.ax.legend()
def outer_callback(timestamp, data, logconf):
return CFDispatch.update_cfstatus(timestamp, data, logconf, cf_arg,
uri)
log_conf.data_received_cb.add_callback(outer_callback)
#print('about to start log')
log_conf.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.stateX', 'float')
log_config.add_variable('kalman.stateY', 'float')
log_config.add_variable('kalman.stateZ', 'float')
# import pdb; pdb.set_trace()
scf.log.add_config(log_config)
log_config.data_received_cb.add_callback(position_callback)
log_config.start()
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]
log_config.add_variable('ranging.distance2', 'float')
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 distance_measurement():
with SyncCrazyflie(uri, cf=Crazyflie(rw_cache='./cache')) as scf:
lpos = LogConfig(name='distance', period_in_ms=10)
lpos.add_variable('peerdist.distance2peer10')
lpos.add_variable('peerdist.distance2peer20')
lpos.add_variable('peerdist.distance2peer30')
lpos.add_variable('peerdist.distance2peer40')
lpos.add_variable('peerdist.distance2peer50')
scf.cf.log.add_config(lpos)
lpos.data_received_cb.add_callback(pos_data)
lpos.start()
time.sleep(200)
df = pd.DataFrame(dt,
columns=[
'distance10', 'distance20', 'distance30',
'distance40', 'distance50', 'time'
])
df.to_csv('./freq_10_rd41_40_rd41.csv', index=False)
print("done")
return
class LoggingExample:
"""
Simple logging example class that logs the from a supplied
link uri and disconnects after 5s.
"""
def __init__(self, cf):
""" Initialize and run the example with the specified link_uri """
self._cf = cf
self.data = {'t': [], 'motor': [], 'z': []}
self._lg_alt = LogConfig(name='Altitude', period_in_ms=10)
self._lg_alt.add_variable('motor.m1', 'float')
self._lg_alt.add_variable('motor.m2', 'float')
self._lg_alt.add_variable('motor.m3', 'float')
self._lg_alt.add_variable('motor.m4', 'float')
self._lg_alt.add_variable('stateEstimate.z', 'float')
try:
self._cf.log.add_config(self._lg_alt)
# This callback will receive the data
self._lg_alt.data_received_cb.add_callback(self._alt_log_data)
# This callback will be called on errors
self._lg_alt.error_cb.add_callback(self._alt_log_error)
# Start the logging
self._lg_alt.start()
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.')
def _alt_log_error(self, logconf, msg):
"""Callback from the log API when an error occurs"""
print('Error when logging %s: %s' % (logconf.name, msg))
def _alt_log_data(self, timestamp, data, logconf):
"""Callback from a the log API when data arrives"""
self.data['motor'].append((data['motor.m1'] + data['motor.m2'] +
data['motor.m3'] + data['motor.m4']))
self.data['z'].append(data['stateEstimate.z'])
self.data['t'].append(timestamp)
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 Main:
"""
Class is required so that methods can access the object fields.
"""
def __init__(self):
"""
Connect to Crazyflie, initialize drivers and set up callback.
The callback takes care of logging the accelerometer values.
"""
self.crazyflie = cflib.crazyflie.Crazyflie()
print 'Init drivers'
cflib.crtp.init_drivers()
print 'Search available interfaces'
available = cflib.crtp.scan_interfaces()
radio = False
for i in available:
print "Interface with URI [%s] found and name/comment [%s]" % (i[0], i[1])
if 'radio' in i[0]:
radio = True
dev = i[0]
self.crazyflie.open_link(dev)
break
if not radio:
print 'No quadcopter detected'
exit(-1)
# Set up the callback when connected
self.crazyflie.connectSetupFinished.add_callback(self.connectSetupFinished)
def connectSetupFinished(self, linkURI):
"""
Configure the logger to log accelerometer values 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.
"""
## Set stabilizer logging config
##stab_log_conf = LogConfig("Stabalizer", 200)
##stab_log_conf.add_variable("stabilizer.roll", "float")
##stab_log_conf.add_variable("stabilizer.pitch", "float")
##stab_log_conf.add_variable("stabilizer.yaw", "float")
##stab_log_conf.add_variable("stabilizer.thrust", "uint16_t")
### Now that the connection is established, start logging
##self.crazyflie.log.add_config(stab_log_conf)
##if stab_log_conf.valid:
## stab_log_conf.data_received_cb.add_callback(self.log_stab_data)
## stab_log_conf.start()
##else:
## print("acc.x/y/z not found in log TOC")
# Log stabilizer
self.logStab = LogConfig("Stabalizer", 200)
self.logStab.add_variable("stabilizer.roll", "float")
self.logStab.add_variable("stabilizer.pitch", "float")
self.logStab.add_variable("stabilizer.yaw", "float")
self.logStab.add_variable("stabilizer.thrust", "uint16_t")
self.crazyflie.log.add_config(self.logStab)
if self.logStab.valid:
self.logStab.data_received_cb.add_callback(self.print_stab_data)
self.logStab.start()
else:
logger.warning("Could not setup log configuration for stabilizer after connection!")
# Set barometer
self.logBaro = LogConfig("Baro", 200)
self.logBaro.add_variable("baro.aslLong", "float")
self.crazyflie.log.add_config(self.logBaro)
if self.logBaro.valid:
self.logBaro.data_received_cb.add_callback(self.print_baro_data)
self.logBaro.start()
else:
logger.warning("Could not setup log configuration for barometer after connection!")
# Log Accelerometer
self.logAccel = LogConfig("Accel",200)
self.logAccel.add_variable("acc.x", "float")
self.logAccel.add_variable("acc.y", "float")
self.logAccel.add_variable("acc.z", "float")
self.crazyflie.log.add_config(self.logAccel)
if self.logAccel.valid:
self.logAccel.data_received_cb.add_callback(self.print_accel_data)
self.logAccel.start()
else:
logger.warning("Could not setup log configuration for accelerometer after connection!")
def print_baro_data(self, ident, data, logconfig):
logging.info("Id={0}, Barometer: asl={1:.4f}".format(ident, data["baro.aslLong"]))
def print_stab_data(self, ident, data, logconfig):
logging.info("Id={0}, Stabilizer: Roll={1:.2f}, Pitch={2:.2f}, Yaw={3:.2f}, Thrust={4:.2f}".format(ident, data["stabilizer.roll"], data["stabilizer.pitch"], data["stabilizer.yaw"], data["stabilizer.thrust"]))
def print_accel_data(self, ident, data, logconfig):
logging.info("Id={0}, Accelerometer: x={1:.2f}, y={2:.2f}, z={3:.2f}".format(ident, data["acc.x"], data["acc.y"], data["acc.z"]))
class FlightTab(Tab, flight_tab_class):
uiSetupReadySignal = pyqtSignal()
_mode_index = 0
_motor_data_signal = pyqtSignal(int, object, object)
_acc_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)
_mag_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)
_switch_mode_updated_signal = pyqtSignal()
_log_error_signal = pyqtSignal(object, str)
# UI_DATA_UPDATE_FPS = 10
connectionFinishedSignal = pyqtSignal(str)
disconnectedSignal = pyqtSignal(str)
def __init__(self, tabWidget, helper, *args):
super(FlightTab, self).__init__(*args)
self.setupUi(self)
self.setStyleSheet("QToolTip { color: #ffffff; background-color: #2d2d2d; border: 1px solid #f0f0f0; border-radius: 3px; }")
self.apitch = 0
self.aroll = 0
self.motor_power = 0
self.tabName = "Flight Control"
self.menuName = "Flight Control"
self.ai = AttitudeIndicator()
self.compass = CompassWidget()
self.tabWidget = tabWidget
self.helper = helper
########## Freefall related stuff
self.ffd = FFD(parent=self) # detection class
self.ffr = FFR(parent=self) # recovery class
# Connect the crash/free fall detections
self.ffd.sigFreeFall.connect(self.freefallDetected)
self.ffd.sigCrashed.connect(self.crashDetected)
self.ffr.sigRecoveryTimedOut.connect(self.recoveryAborted)
# continuously send acc, mean, var to AI
self.ffd.sigAccMeanVar.connect(self.ai.setFFAccMeanVar)
# Only set recovery on if both fall and crash detection are on as well as recovery
self.checkBox_ffd.stateChanged.connect(lambda on: self.ffGuiSync(0,on) ) # detect freefall on/off
self.checkBox_crash.stateChanged.connect(lambda on: self.ffGuiSync(1,on)) # detect crashing on/off
self.checkBox_ffr.stateChanged.connect(lambda on: self.ffGuiSync(2,on)) # recovery crashing on/off
# Use barometer for recovery (only clickedable if the checkbox activates in reaction to detecting a 10DOF flie
self.checkBox_ffbaro.clicked.connect(self.ffr.setUseBaro)
# intercept control commands
self.helper.inputDeviceReader.auto_input_updated.add_callback(self.ffr.step)
self.ffr.auto_input_updated.add_callback(self.helper.cf.commander.send_setpoint)
self.ffr.auto_input_updated.add_callback(self._input_updated_signal.emit)
self.ffr.althold_updated.add_callback(lambda param, arg: self.helper.cf.param.set_value(param, str(arg)))
# Emergency Stop
self._emergency_stop_updated_signal.connect(self.ffr.setKillSwitch)
#self._emergency_stop_updated_signal.connect(self.ai.setKillSwitch)
# Debugging Freefall
self.pushButton_ff.clicked.connect(self.ffd.sendFakeEmit)
self.pushButton_crash.clicked.connect(self.ffd.sendFakeLandingEmit)
self.doubleSpinBox_ff_falloff.valueChanged.connect(self.ffr.falloff.setWidth)
self.doubleSpinBox_ff_max.valueChanged.connect(self.ffr.falloff.setMaxThrust)
self.doubleSpinBox_ff_min.valueChanged.connect(self.ffr.falloff.setMinThrust)
self.doubleSpinBox_ff_time.valueChanged.connect(self.ffr.falloff.setTime)
self.pushButton_plot.clicked.connect(self.ffr.falloff.plot)
self.checkBox_debug.stateChanged.connect(self.toggleFFDebug)
self.checkBox_ffbaro.clicked.connect(self.toggleFFDebug)
# Slow down drawing to GUi items by keeping track of last received data time
self.lastImuTime = 0
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._switch_mode_updated_signal.connect(self.switchMode)
self.helper.inputDeviceReader.switch_mode_updated.add_callback(self._switch_mode_updated_signal.emit)
self.helper.inputDeviceReader.althold_updated.add_callback(self.changeHoldmode)
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._acc_data_signal.connect(self._acc_data_received)
self._mag_data_signal.connect(self._mag_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.maxAltitude.valueChanged.connect(self.maxAltitudeChanged)
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.clientHoldModeCheckbox.toggled.connect(self.changeHoldmode)
self.isInCrazyFlightmode = False
self.uiSetupReady()
self.viscousModeSlider.valueChanged.connect(self.setViscousThrust)
self.clientHoldModeCheckbox.setChecked(GuiConfig().get("client_side_holdmode"))
# 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.horizontalLayout_5.addWidget(self.ai)
self.horizontalLayout_5.addWidget(self.compass)
# 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 setViscousThrust(self, thrust):
self.helper.inputDeviceReader.setViscousModeThrust(thrust)
point = QtGui.QCursor.pos()
QtGui.QToolTip.showText(QtCore.QPoint(point.x(), self.viscousModeSlider.pos().y()*2-12), QtCore.QString.number(thrust) + "%")
@pyqtSlot(int)
def toggleFFDebug(self, ignored=None):
if self.checkBox_debug.isChecked():
self.pushButton_ff.show()
self.pushButton_crash.show()
if self.checkBox_ffbaro.isEnabled() and self.checkBox_ffbaro.isChecked():
self.pushButton_plot.hide()
self.doubleSpinBox_ff_falloff.hide()
self.doubleSpinBox_ff_max.hide()
self.doubleSpinBox_ff_min.hide()
self.doubleSpinBox_ff_time.hide()
self.label_ff4.hide()
self.label_ff1.hide()
self.label_ff2.hide()
self.label_ff3.hide()
else:
self.pushButton_plot.show()
self.doubleSpinBox_ff_falloff.show()
self.doubleSpinBox_ff_max.show()
self.doubleSpinBox_ff_min.show()
self.doubleSpinBox_ff_time.show()
self.label_ff4.show()
self.label_ff1.show()
self.label_ff2.show()
self.label_ff3.show()
else:
self.pushButton_ff.hide()
self.pushButton_crash.hide()
self.pushButton_plot.hide()
self.doubleSpinBox_ff_falloff.hide()
self.doubleSpinBox_ff_max.hide()
self.doubleSpinBox_ff_min.hide()
self.doubleSpinBox_ff_time.hide()
self.label_ff4.hide()
self.label_ff1.hide()
self.label_ff2.hide()
self.label_ff3.hide()
@pyqtSlot()
def freefallDetected(self):
self.ai.setFreefall()
if self.checkBox_ffr.isChecked() and self.checkBox_ffr.isEnabled():
self.ffr.startRecovery()
self.ai.setRecovery(True)
self.helper.inputDeviceReader.setAuto(True)
self.checkBox_ffr.setChecked(False)
# self.emergency_stop_label.setText("Recovering Freefall")
# self.checkBox_ffr.setEnabled(False)
@pyqtSlot(float)
def crashDetected(self, badness):
self.ai.setRecovery(False, 'Landed / Crashed')
self.ai.setCrash(badness)
self.helper.inputDeviceReader.setAuto(False)
self.ffr.setLanded()
# self.emergency_stop_label.setText("")
self.checkBox_ffr.setChecked(True)
@pyqtSlot()
def recoveryAborted(self):
self.ai.setRecovery(False, 'Recovery Aborted / Timed out')
self.helper.inputDeviceReader.setAuto(False)
# self.emergency_stop_label.setText("")
# self.checkBox_ffr.setEnabled(True)
pass
def ffGuiSync(self, id, on):
""" Keeps freefall gui elements in sync"""
if id == 0: # freefall detection click
self.ffd.setEnabled(on) # detect freefall on/off
elif id == 1: # landing detection click
self.ffd.setEnabledLanding(on) # detect crashing on/off
# elif id == 2: # recovery detection click
# Only allow recivery checkbox to be turned on if crash/freefall detection is active
self.checkBox_ffr.setEnabled(self.checkBox_ffd.isChecked() and self.checkBox_crash.isChecked())
# Only allow recovery class to be turned on if all boxes are checked
# self.ffr.setEnabled(on=self.checkBox_ffd.isChecked() and self.checkBox_crash.isChecked() and self.checkBox_ffr.isChecked() and self.checkBox_ffr.isEnabled())
def makeSpace(self):
# I wish I knew how to make the ai QWidget fullscreen....
s = QtGui.QSplitter()
if self.splitter_2.widget(0).isHidden():
self.splitter_2.widget(0).show()
self.splitter.widget(1).show()
else:
self.splitter_2.widget(0).hide()
self.splitter.widget(1).hide()
def _acc_data_received(self, timestamp, data, logconf):
self.ffd.readAcc(data["acc.zw"])
self.helper.cf.commander.setActualGravity(data)
def updateCamList(self):
"""Repopulate the combobox with available camera devices and select the last one (=usually the camera we need)"""
# Remove all
self.comboBox_camera.clear()
# Query available devices
devices = self.cam.getDevices()
for i, device in enumerate(devices):
logger.debug("Camera device [%d]: %s", i, device)
self.comboBox_camera.addItem(device)
# enable/disable the on/off button and dropdown
self.checkBox_camera.setEnabled(len(devices) > 0)
self.comboBox_camera.setEnabled(len(devices) > 0)
if len(devices) == 0:
self.comboBox_camera.addItem("None Detected")
# Select last one (eg the one we just plugged in)
self.comboBox_camera.setCurrentIndex(max(0, len(devices) - 1))
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 _motor_data_received(self, timestamp, data, logconf):
self.motor_power = data["motor.m1"] + data["motor.m2"] + data["motor.m3"] + data["motor.m4"]
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):
self.helper.inputDeviceReader.setCurrentAltitude(data["baro.aslLong"])
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):
self.aroll = data["stabilizer.roll"]
self.apitch = data["stabilizer.pitch"]
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.compass.setAngle(-data["stabilizer.yaw"])
# self.helper.cf.commander.setActualPoint(data)
def _mag_data_received(self, timestamp, data, logconf):
# hard and soft correction values generated by Processing Magnetometer_calibration script + calibrate_hardsoft.py
magn_ellipsoid_center = [1341.66, -537.690, 41.1584]
magn_ellipsoid_transform = [[0.934687, 0.0222809, 0.0151145], [0.0222809, 0.919365, -0.00622367], [0.0151145, -0.00622367, 0.996487]]
# values generated by calibrate_powered.py
qx = [0.067946222436498283, -0.25034004667098259, 8.3336994198409666, -0.17762637163222378]
qy = [-0.13945102271766135, 2.9074808469097495, 1.6764850422889934, 0.19244505046927501]
qz = [0.018800599305554239, -0.79590273035713055, -3.1033531112103478, 0.13550993988096199]
# matrix by vector multiplication
def mv(a, b):
out = [0,0,0]
for x in range(0, 3):
out[x] = a[x][0] * b[0] + a[x][1] * b[1] + a[x][2] * b[2];
return out
# calculate adjustments related to how much power is sent to the motors
def adj(qs, power):
p = float(power) / float(40000) # 10k * 4 motors
return qs[0]*p**3+qs[1]*p**2+qs[2]*p+qs[3]
x, y, z = data['mag.x'], data['mag.y'], data['mag.z']
x = x - magn_ellipsoid_center[0]
y = y - magn_ellipsoid_center[1]
z = z - magn_ellipsoid_center[2]
x, y, z = mv(magn_ellipsoid_transform, [x, y, z])
x = x + adj(qx, self.motor_power)
y = y + adj(qy, self.motor_power)
z = z + adj(qz, self.motor_power)
# correct magnetometer orientation relative to the CF orientation
x, y, z = y, x, z * -1
# calculate tilt-compensated heading angle
cosRoll = cos(math.radians(self.aroll))
sinRoll = sin(math.radians(self.aroll))
cosPitch = cos(math.radians(self.apitch))
sinPitch = sin(math.radians(self.apitch))
Xh = x * cosPitch + z * sinPitch
Yh = x * sinRoll * sinPitch + y * cosRoll - z * sinRoll * cosPitch
heading = math.atan2(Yh, Xh)
d_heading = math.degrees(heading) * -1 # for some reason getting inveted sign here
# update compass widget
#self.compass.setAngle(d_heading)
# lock heading to 0 degrees north
if d_heading > 0:
yaw_trim = -20
else:
yaw_trim = 20
self.helper.inputDeviceReader.update_trim_yaw_signal(yaw_trim)
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")
lg.add_variable("acc.x", "float")
lg.add_variable("acc.y", "float")
lg.add_variable("acc.z", "float")
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!")
# Acc for freefall
lg = LogConfig("Acc", 10) # Yes we need this at 100hz for freefall detection!!
lg.add_variable("acc.zw", "float")
self.helper.cf.log.add_config(lg)
if lg.valid:
lg.data_received_cb.add_callback(self._acc_data_signal.emit)
lg.error_cb.add_callback(self._log_error_signal.emit)
lg.start()
else:
logger.warning("Could not setup logconfiguration after connection [ACC]!")
lg = LogConfig("Magnetometer", 100)
lg.add_variable("mag.x", "int16_t")
lg.add_variable("mag.y", "int16_t")
lg.add_variable("mag.z", "int16_t")
self.helper.cf.log.add_config(lg)
if lg.valid:
lg.data_received_cb.add_callback(self._mag_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.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 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 maxAltitudeChanged(self):
self.helper.inputDeviceReader.setMaxAltitude(self.maxAltitude.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.setEnabled(True)
self.emergency_stop_label.setText(self.emergencyStopStringWithText("Kill Switch Active"))
self.ai.setKillSwitch(True)
else:
self.setMotorLabelsEnabled(True)
self.emergency_stop_label.setText("Kill Swith")
self.emergency_stop_label.setEnabled(False)
self.ai.setKillSwitch(False)
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 changeCareFreemode(self, checked):
# self.helper.cf.commander.set_client_carefreemode(checked)
if checked:
self.clientCareFreeModeRadio.setChecked(checked)
self.helper.cf.param.set_value("FlightMode.flightmode", "0")
# self.helper.cf.commander.set_client_positionmode(False)
# self.helper.cf.commander.set_client_xmode(False)
elif not checked and self._mode_index == 1:
self.clientCareFreeModeRadio.setChecked(False)
self.clientNormalModeRadio.setChecked(True)
self.helper.cf.param.set_value("FlightMode.flightmode", "1")
logger.info("CareFree-mode enabled: %s", checked)
@pyqtSlot(bool)
def changeXmode(self, checked):
self.clientXModeRadio.setChecked(checked)
# self.helper.cf.commander.set_client_xmode(checked)
if checked:
self.helper.cf.param.set_value("FlightMode.flightmode", "2")
# self.helper.cf.commander.set_client_carefreemode(False)
# self.helper.cf.commander.set_client_positionmode(False)
logger.info("X-mode enabled: %s", checked)
@pyqtSlot(bool)
def changePositionmode(self, checked):
self.clientPositionModeRadio.setChecked(checked)
# self.helper.cf.commander.set_client_positionmode(checked)
if checked:
self.helper.cf.param.set_value("FlightMode.flightmode", "3")
# self.helper.cf.commander.set_client_carefreemode(False)
# self.helper.cf.commander.set_client_xmode(False)
logger.info("Position-mode enabled: %s", checked)
@pyqtSlot(bool)
def changeHeadingmode(self, checked):
self.clientHeadingModeRadio.setChecked(checked)
# self.helper.cf.commander.set_client_positionmode(checked)
if checked:
self.helper.cf.param.set_value("FlightMode.flightmode", "4")
# self.helper.cf.commander.set_client_carefreemode(False)
# self.helper.cf.commander.set_client_xmode(False)
logger.info("Heading-mode enabled: %s", checked)
@pyqtSlot(bool)
def changeHoldmode(self, checked):
self.clientHoldModeCheckbox.setChecked(checked)
self.helper.cf.param.set_value("flightmode.althold", str(checked))
# self.helper.cf.commander.set_client_holdmode(checked)
GuiConfig().set("client_side_holdmode", checked)
logger.info("Hold-mode enabled: %s", checked)
def switchMode(self):
if self._mode_index == 4:
self._mode_index = 0
else:
self._mode_index += 1
if self._mode_index == 0:
self.changeCareFreemode(True)
self.changeXmode(False)
self.changePositionmode(False)
self.changeHeadingmode(False)
elif self._mode_index == 1:
self.changeCareFreemode(False)
self.changeXmode(False)
self.changePositionmode(False)
self.changeHeadingmode(False)
elif self._mode_index == 2:
self.changeCareFreemode(False)
self.changeXmode(True)
self.changePositionmode(False)
self.changeHeadingmode(False)
elif self._mode_index == 3:
self.changeCareFreemode(False)
self.changeXmode(False)
self.changePositionmode(True)
self.changeHeadingmode(False)
elif self._mode_index == 4:
self.changeCareFreemode(False)
self.changeXmode(False)
self.changePositionmode(False)
self.changeHeadingmode(True)
logger.info("Mode switched to index: %d", self._mode_index)
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)
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)
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):
""" 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 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
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")
lg.add_variable("acc.x", "float")
lg.add_variable("acc.y", "float")
lg.add_variable("acc.z", "float")
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!")
# Acc for freefall
lg = LogConfig("Acc", 10) # Yes we need this at 100hz for freefall detection!!
lg.add_variable("acc.zw", "float")
self.helper.cf.log.add_config(lg)
if lg.valid:
lg.data_received_cb.add_callback(self._acc_data_signal.emit)
lg.error_cb.add_callback(self._log_error_signal.emit)
lg.start()
else:
logger.warning("Could not setup logconfiguration after connection [ACC]!")
lg = LogConfig("Magnetometer", 100)
lg.add_variable("mag.x", "int16_t")
lg.add_variable("mag.y", "int16_t")
lg.add_variable("mag.z", "int16_t")
self.helper.cf.log.add_config(lg)
if lg.valid:
lg.data_received_cb.add_callback(self._mag_data_signal.emit)
lg.error_cb.add_callback(self._log_error_signal.emit)
lg.start()
else:
logger.warning("Could not setup logconfiguration after connection!")
class RollController:
"""Example that connects to a Crazyflie and ramps the motors up/down and
the disconnects"""
def __init__(self, link_uri):
self._runCouner = 0
# Set Initial values
self._thrust = 42000
self._pitch = 0
self._roll = 0
self._yawrate = 0
# Roll Trim
self._rollTrim = 0
self._yawSetPoint = 0
self._initialYawSet = False
self._rollPid = PID()
self._rollPid.SetKp(-.5); # Proportional Gain
self._rollPid.SetKi(-.01); # Integral Gain
self._rollPid.SetKd(0); # Derivative Gain
self._yawPid = PID()
self._yawPid.SetKp(-.01); # Proportional Gain
self._yawPid.SetKi(-.01); # Integral Gain
self._yawPid.SetKd(0); # Derivative Gain
""" Initialize and run the example with the specified link_uri """
print "Connecting to %s" % link_uri
self._cf = Crazyflie(ro_cache="./crazyflie-clients-python/lib/cflib/cache",
rw_cache="./crazyflie-clients-python/lib/cflib/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.open_link(link_uri)
print "Connecting to %s" % 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
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 _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)
new_dict = {logconf.name:data}
stab_roll = new_dict['Logger']['stabilizer.roll']
stab_pitch = new_dict['Logger']['stabilizer.pitch']
stab_yaw = new_dict['Logger']['stabilizer.yaw']
gyro_x = new_dict['Logger']['gyro.x']
gyro_y = new_dict['Logger']['gyro.y']
gyro_z = new_dict['Logger']['gyro.z']
if stab_roll > 15:
print "I'm out of control!!!!!"
self._cf.commander.setpoint(0, 0, 0, 0)
self._cf.close_link()
os._exit(1)
if not self._initialYawSet:
self._initialYawSet = True
self._yawSetPoint = stab_yaw
return;
prevRoll = self._roll
self._roll = self._rollPid.GenOut(stab_roll + self._rollTrim)
prevYawRate = self._yawrate
self._yawrate = self._rollPid.GenOut(stab_yaw + self._yawSetPoint)
self._cf.commander.send_setpoint(self._roll, self._pitch, self._yawrate, self._thrust)
print "Yaw %s, Old Yaw %s, Stab Yaw = %s" % (self._yawrate, prevYawRate, stab_yaw)
print "Roll %s, Old Roll %s, Stab Roll = %s" % (self._roll, prevRoll, stab_roll)
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)
self.is_connected = False
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 kill(self, signal, frame):
print "Ctrl+C pressed"
self._cf.commander.send_setpoint(0, 0, 0, 0)
self._cf.close_link()
os._exit(1)
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 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)
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!")
class Hover:
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 _connected(self, link_uri):
""" This callback is called form the Crazyflie API when a Crazyflie
has been connected and the TOCs have been downloaded."""
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.")
Thread(target=self._hover).start()
Thread(target=self._log_task).start()
Thread(target=self._exit_task).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"""
self._acc_x = float(data['acc.x'])
self._acc_y = float(data['acc.y'])
self._acc_zw = float(data['acc.zw'])
# self._acc_z = float(data['acc.z'])
self._gyro_x = float(data['gyro.x'])
self._gyro_y = float(data['gyro.y'])
self._gyro_z = float(data['gyro.z'])
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)
self._is_connected = False
def _exit_task(self):
while not self.exit:
inp = int(input('Want to exit? [NO:0/YES:1]\n'))
if inp != 0:
self.exit = True
def _log_task(self):
time.sleep(3)
print("Done log sleep!")
x = [0]
it = 0
data = {'acc.x': [0], 'acc.y': [0], 'acc.zw': [0],
'gyro.x': [0], 'gyro.y': [0], 'gyro.z': [0],
'out_pitch': [0], 'out_roll': [0], 'out_thrust': [0]}
plt.ion()
fig = plt.figure()
ax1 = fig.add_subplot(311)
line1, = ax1.plot(x, data['acc.x'], 'b-', label="acc.x")
line2, = ax1.plot(x, data['out_pitch'], 'r-', label="out pitch")
lineGX, = ax1.plot(x, data['gyro.x'], 'g-', label="gyro.x")
# ax1.legend(handles=[line1, line2, lineGX])
ax2 = fig.add_subplot(312)
line3, = ax2.plot(x, data['acc.y'], 'b-', label="acc.y")
line4, = ax2.plot(x, data['out_roll'], 'r-', label="out roll")
lineGY, = ax2.plot(x, data['gyro.y'], 'g-', label="gyro.y")
# ax1.legend(handles=[line3, line4, lineGY])
ax3 = fig.add_subplot(313)
line5, = ax3.plot(data['acc.zw'], 'b-', label="acc.zw")
line6, = ax3.plot(data['out_thrust'], 'r-', label="out thrust")
# lineGZ, = ax3.plot(data['gyro.z'], 'g-', label="gyro.z")
# ax1.legend(handles=[line5, line6])
fig.canvas.draw()
plt.show(block=False)
while not self.exit:
it += 1
x.append(it)
data['acc.x'].append(self._acc_x)
data['acc.y'].append(self._acc_y)
data['acc.zw'].append(self._acc_zw)
data['gyro.x'].append(self._gyro_x)
data['gyro.y'].append(self._gyro_y)
data['gyro.z'].append(self._gyro_z)
data['out_pitch'].append(self._output_pitch_raw)
data['out_roll'].append(self._output_roll_raw)
data['out_thrust'].append(self._output_thrust_raw)
# print(int(65000*(self._output_thrust_raw)/2))
line1.set_ydata(data['acc.x'])
line1.set_xdata(x)
line2.set_ydata(data['out_pitch'])
line2.set_xdata(x)
lineGX.set_ydata(data['gyro.x'])
lineGX.set_xdata(x)
line3.set_ydata(data['acc.y'])
line3.set_xdata(x)
line4.set_ydata(data['out_roll'])
line4.set_xdata(x)
lineGY.set_ydata(data['gyro.y'])
lineGY.set_xdata(x)
line5.set_ydata(data['acc.zw'])
line5.set_xdata(x)
# lineGZ.set_ydata(data['gyro.z'])
# lineGZ.set_xdata(x)
line6.set_ydata(data['out_thrust'])
line6.set_xdata(x)
ax1.relim()
ax1.autoscale_view(True, True, True)
ax2.relim()
ax2.autoscale_view(True, True, True)
ax3.relim()
ax3.autoscale_view(True, True, True)
fig.canvas.draw()
# time.sleep(0.5)
plt.pause(0.05)
filename = str(input('Log File Name'))
if filename != '0':
fig.savefig(filename)
self._cf.close_link()
print("Closing Log task")
def _hover(self):
print("Starting Hover")
self._cf.commander.send_setpoint(0, 0, 0, 0)
self._output_pitch_raw = 0
self._output_roll_raw = 0
self._output_thrust_raw = 0
while not self.exit:
self._output_pitch_raw = (self._acc_pid_x.compute(
self._acc_x)[0] + self._gyro_x_pid.compute(self._gyro_x)[0])/2
self._output_roll_raw = (self._acc_pid_y.compute(
self._acc_y)[0] + self._gyro_x_pid.compute(self._gyro_x)[0])/2
self._output_thrust_raw = self._acc_pid_z.compute(self._acc_zw)[0]
self._output_pitch = -(self._output_pitch_raw)*10
self._output_roll = self._output_roll_raw*10
self._output_thrust = int(60000*(self._output_thrust_raw)/2)
self._cf.commander.send_setpoint(0, 0, 0, self._output_thrust)
# self._cf.commander.send_setpoint(self._output_pitch,
# self._output_roll,
# 0,
# self._output_thrust)
time.sleep(0.02)
self._cf.commander.send_setpoint(0, 0, 0, 0)
time.sleep(0.1)
def connected(self, linkURI):
# IMU & THRUST
lg = LogConfig("Stabalizer", 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")
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", 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")
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!")
if self.helper.cf.mem.ow_search(vid=0xBC, pid=0x01):
self._led_ring_effect.setEnabled(True)
self._led_ring_headlight.setEnabled(True)
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='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')
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))
def _connected(self, link_uri):
lg = LogConfig("GPS", 100)
lg.add_variable("gps.lat")
lg.add_variable("gps.lon")
lg.add_variable("gps.hMSL")
lg.add_variable("gps.heading")
lg.add_variable("gps.gSpeed")
lg.add_variable("gps.hAcc")
lg.add_variable("gps.fixType")
try:
self._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))
self._max_speed = 0.0
# 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
def _connected(self, link_uri):
lg = LogConfig("GPS", 100)
lg.add_variable("gps.lat")
lg.add_variable("gps.lon")
lg.add_variable("gps.hMSL")
lg.add_variable("gps.heading")
lg.add_variable("gps.gSpeed")
lg.add_variable("gps.hAcc")
lg.add_variable("gps.fixType")
self._cf.log.add_config(lg)
if lg.valid:
lg.data_received_cb.add_callback(self._log_data_signal.emit)
lg.error_cb.add_callback(self._log_error_signal.emit)
lg.start()
else:
logger.warning("Could not setup logging block for GPS!")
self._max_speed = 0.0
