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")
lg.add_variable("sys.canfly")
self._hlCommander = PositionHlCommander(
self.helper.cf,
x=0.0, y=0.0, z=0.0,
default_velocity=0.3,
default_height=0.5,
controller=PositionHlCommander.CONTROLLER_PID
)
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 connected(self, linkURI):
# IMU & THRUST
lg = LogConfig("Stabilizer", Config().get("ui_update_period"))
lg.add_variable("stabilizer.roll", "float")
lg.add_variable("stabilizer.pitch", "float")
lg.add_variable("stabilizer.yaw", "float")
lg.add_variable("stabilizer.thrust", "uint16_t")
try:
self.helper.cf.log.add_config(lg)
lg.data_received_cb.add_callback(self._imu_data_signal.emit)
lg.error_cb.add_callback(self._log_error_signal.emit)
lg.start()
except KeyError as e:
logger.warning(str(e))
except AttributeError as e:
logger.warning(str(e))
# MOTOR
lg = LogConfig("Motors", Config().get("ui_update_period"))
lg.add_variable("motor.m1")
lg.add_variable("motor.m2")
lg.add_variable("motor.m3")
lg.add_variable("motor.m4")
try:
self.helper.cf.log.add_config(lg)
lg.data_received_cb.add_callback(self._motor_data_signal.emit)
lg.error_cb.add_callback(self._log_error_signal.emit)
lg.start()
except KeyError as e:
logger.warning(str(e))
except AttributeError as e:
logger.warning(str(e))
def add_callback_to_cf(uri, scf):
cflib.crtp.init_drivers(enable_debug_driver=False)
log_conf = LogConfig(name=uri, period_in_ms=3000)
log_conf.add_variable('pm.vbat', 'float')
scf.cf.log.add_config(log_conf)
log_conf.data_received_cb.add_callback(get_current_battery_callback)
log_conf.start()
def line_sequence(velocity):
global pos_fixed
#先记录固定点的坐标用于计算距离
with SyncCrazyflie(uri_fixed, cf=Crazyflie(rw_cache='./cache')) as scf:
lpos = LogConfig(name='position', period_in_ms=100)
lpos.add_variable('stateEstimate.x')
lpos.add_variable('stateEstimate.y')
lpos.add_variable('stateEstimate.z')
scf.cf.log.add_config(lpos)
lpos.data_received_cb.add_callback(pos_data_fixed)
lpos.start()
time.sleep(2)
lpos.stop()
pos_fixed = np.mean(fixed_dt, axis=0).tolist()
print(pos_fixed)
with SyncCrazyflie(uri, cf=Crazyflie(rw_cache='./cache')) as scf:
lpos = LogConfig(name='position', period_in_ms=100)
lpos.add_variable('stateEstimate.x')
lpos.add_variable('stateEstimate.y')
lpos.add_variable('stateEstimate.z')
lpos.add_variable(distance_mapper[uri_fixed])
scf.cf.log.add_config(lpos)
lpos.data_received_cb.add_callback(pos_data)
with PositionHlCommander(scf, default_velocity=velocity) as pc:
time.sleep(2)
pc.go_to(2.0, 0.0, 0.5)
lpos.start()
pc.go_to(-0.5, 0, 0.5)
lpos.stop()
df = pd.DataFrame(
moving_dt,
columns=['x', 'y', 'z', 'distance_UWB', 'distance_lighthouse'])
df.to_csv('./antenna_' + str(velocity) + '_154.65_8' + '.csv', index=False)
def thre(self, cf):
print("start")
fly = True
t = 0
logPos = LogConfig(name='position', period_in_ms=10)
logPos.add_variable('ext_pos.X', 'float')
logPos.add_variable('ext_pos.Y', 'float')
logPos.add_variable('ext_pos.Z', 'float')
cf.log.add_config(logPos)
logPos.data_received_cb.add_callback(self.position_callback)
logPos.start()
while fly == True:
try:
loca = pos[-1]
## loca = self.pos[-1]
## self.pos = self.pos[1:]
## print('loca\n')
cf.loc.send_extpos([loca[2], loca[0], loca[1]])
## print('loca ',loca[0],loca[1],loca[2],t)
## print(logger.next())
## t+=0.1
time.sleep(0.01)
except:
pass
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!")
def connected(self, linkURI):
# IMU & THRUST
lg = LogConfig("Stabilizer", Config().get("ui_update_period"))
lg.add_variable("stabilizer.roll", "float")
lg.add_variable("stabilizer.pitch", "float")
lg.add_variable("stabilizer.yaw", "float")
lg.add_variable("stabilizer.thrust", "uint16_t")
try:
self.helper.cf.log.add_config(lg)
lg.data_received_cb.add_callback(self._imu_data_signal.emit)
lg.error_cb.add_callback(self._log_error_signal.emit)
lg.start()
except KeyError as e:
logger.warning(str(e))
except AttributeError as e:
logger.warning(str(e))
# MOTOR
lg = LogConfig("Motors", Config().get("ui_update_period"))
lg.add_variable("motor.m1")
lg.add_variable("motor.m2")
lg.add_variable("motor.m3")
lg.add_variable("motor.m4")
try:
self.helper.cf.log.add_config(lg)
lg.data_received_cb.add_callback(self._motor_data_signal.emit)
lg.error_cb.add_callback(self._log_error_signal.emit)
lg.start()
except KeyError as e:
logger.warning(str(e))
except AttributeError as e:
logger.warning(str(e))
def start_zranger_reading(self):
log_conf = LogConfig(name='Z-ranger',
period_in_ms=100) # read position with 10 Hz rate
log_conf.add_variable('range.zrange', 'float')
self.scf.cf.log.add_config(log_conf)
log_conf.data_received_cb.add_callback(self.zranger_callback)
log_conf.start()
def init_log_conf(scf, callback, data_dir):
"""Initialize and start the logging
Args:
scf: Synced Crazyflie
callback: Function called when new data is received
data_dir: Directory where the log file will be saved
"""
log_conf = LogConfig(name='MainLog', period_in_ms=100)
# Logged variables
# Multiranger
log_conf.add_variable('range.zrange', 'uint16_t')
log_conf.add_variable('range.up', 'uint16_t')
log_conf.add_variable('range.front', 'uint16_t')
log_conf.add_variable('range.back', 'uint16_t')
log_conf.add_variable('range.left', 'uint16_t')
log_conf.add_variable('range.right', 'uint16_t')
# State estimate (x, y, yaw)
log_conf.add_variable('stateEstimate.x', 'float')
log_conf.add_variable('stateEstimate.y', 'float')
log_conf.add_variable('stabilizer.yaw', 'float')
scf.cf.log.add_config(log_conf)
log_conf.data_received_cb.add_callback(callback)
log_conf.name = start_file_manager(data_dir)
log_conf.start()
def simple_sequence():
with SyncCrazyflie(URI, cf=Crazyflie(rw_cache='./cache')) as scf:
scf.cf.param.add_update_callback(group="deck",
name="bcLighthouse4",
cb=param_deck_flow)
time.sleep(1)
logconf = LogConfig(name='Parameters', period_in_ms=SAMPLE_PERIOD_MS)
for param in log_parameters:
logconf.add_variable(param[0], param[1])
scf.cf.log.add_config(logconf)
logconf.data_received_cb.add_callback(logconf_callback)
if not is_deck_attached:
return
logconf.start()
with PositionHlCommander(
scf,
x=0.0,
y=0.0,
z=0.0,
default_velocity=VELOCITY,
default_height=DEFAULT_HEIGHT,
controller=PositionHlCommander.CONTROLLER_MELLINGER) as pc:
for position in SEQUENCE:
pc.go_to(position[0], position[1])
time.sleep(5)
logconf.stop()
write_log_history()
def start_battery_status_reading(self):
log_conf = LogConfig(
name='Battery',
period_in_ms=100) # read battery status with 10 Hz rate
log_conf.add_variable('pm.vbat', 'float')
self.scf.cf.log.add_config(log_conf)
log_conf.data_received_cb.add_callback(self.battery_callback)
log_conf.start()
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 start_position_printing3(scf):
log_conf3 = LogConfig(name='Gyro', period_in_ms=10)
log_conf3.add_variable('gyro.x', 'float')
log_conf3.add_variable('gyro.y', 'float')
log_conf3.add_variable('gyro.z', 'float')
scf.cf.log.add_config(log_conf3)
log_conf3.data_received_cb.add_callback(position_callback3)
log_conf3.start()
def start_position_printing2(scf):
log_conf2 = LogConfig(name='Stabilizer', period_in_ms=10)
log_conf2.add_variable('stabilizer.roll', 'float')
log_conf2.add_variable('stabilizer.pitch', 'float')
log_conf2.add_variable('stabilizer.yaw', 'float')
scf.cf.log.add_config(log_conf2)
log_conf2.data_received_cb.add_callback(position_callback2)
log_conf2.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_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 start_position_reading(self):
log_conf = LogConfig(name='Position',
period_in_ms=100) # read position with 10 Hz rate
log_conf.add_variable('kalman.stateX', 'float')
log_conf.add_variable('kalman.stateY', 'float')
log_conf.add_variable('kalman.stateZ', 'float')
self.scf.cf.log.add_config(log_conf)
log_conf.data_received_cb.add_callback(self.position_callback)
log_conf.start()
def start_position_printing4(scf):
log_conf4 = LogConfig(name='Motor', period_in_ms=10)
log_conf4.add_variable('motor.m1', 'float')
log_conf4.add_variable('motor.m2', 'float')
log_conf4.add_variable('motor.m3', 'float')
log_conf4.add_variable('motor.m4', 'float')
scf.cf.log.add_config(log_conf4)
log_conf4.data_received_cb.add_callback(position_callback4)
log_conf4.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 startDebugPrinting(scf):
#if scf.cf.link_uri == 'radio://0/80/2M/E7E7E7E7E9':
# return
logConf = LogConfig(name='Debug', period_in_ms=100)
logConf.add_variable('debu.stop', 'uint8_t')
scf.cf.log.add_config(logConf)
logConf.data_received_cb.add_callback(lambda timestamp, data, logconf: debugCallback(timestamp, data, logconf, scf.cf.link_uri))
logConf.start()
def consumer(queue):
with SyncCrazyflie(uriDron, cf=Crazyflie(rw_cache='./cache2')) as scf2:
fileQueue = open(filenameQueue,'w')
lg_stab = LogConfig(name='Stabilizer', period_in_ms=100)
lg_stab.add_variable('stateEstimate.x', 'float')
lg_stab.add_variable('stateEstimate.y', 'float')
lg_stab.add_variable('stateEstimate.z', 'float')
scf2.cf.log.add_config(lg_stab)
lg_stab.data_received_cb.add_callback(log_stab_callback)
lg_stab.start()
scf2.cf.commander.send_position_setpoint(0, 0, 0.4, 0)
message = queue.get()
p = [0, 0]
q = [message[0], message[1]]
d = math.dist(p, q)
x = message[0]
y = message[1]
z = 0.5
if d > 0.15:
n = int(d/0.1)
for g in range(1, n):
x_i = x*(g/n)
y_i = y*(g/n)
s = '{}, {}, {}\n'.format{x_i, y_i, z}
fileQueue.write(s)
print('Step {} ({}, {}, {})'.format(g, x_i, y_i, z))
scf2.cf.commander.send_position_setpoint(x_i, y_i, z, 0)
time.sleep(1)
else:
print('Dron Inital pos ({}, {}, {})'.format(x, y, z))
scf2.cf.commander.send_position_setpoint(x, y, z, 0)
s = '{}, {}, {}\n'.format{x, y, z}
fileQueue.write(s)
time.sleep(1)
while (is_finished == False) or (not(queue.empty())):
message = queue.get()
x = message[0]
y = message[1]
z = 0.5
s = '{}, {}, {}\n'.format(x, y, z)
fileQueue.write(s)
print('Move dron to ({}, {}, {})'.format(x, y, z))
scf2.cf.commander.send_position_setpoint(x, y, z, 0)
time.sleep(0.2)
scf2.cf.commander.send_stop_setpoint()
time.sleep(1)
lg_stab.stop()
fileQueue.close()
with open(filenameDron, 'w') as f:
for q in dronPositions:
a = '{}, {}, {}\n'.format(q[0], q[1], q[2])
f.write(q)
print('Consumer ended')
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(self):
"""Activate logging of the position of the real drone."""
log_conf = LogConfig(name='Position', period_in_ms=50)
log_conf.add_variable('kalman.stateX', 'float')
log_conf.add_variable('kalman.stateY', 'float')
log_conf.add_variable('kalman.stateZ', 'float')
self.scf.cf.log.add_config(log_conf)
log_conf.data_received_cb.add_callback(self.position_callback)
log_conf.start()
def startPrinting(scf):
print("\n-- STARTING PRINT LOOP --\n\n")
log_conf = LogConfig(name='Position', period_in_ms=15)
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(positionCallback)
log_conf.start()
def _connected(self, link_uri):
print('Connected to %s' % link_uri)
lg_pose = LogConfig(name='test', period_in_ms=50)
lg_pose.add_variable('stabilizer.roll', 'float')
lg_pose.add_variable('stabilizer.pitch', 'float')
lg_pose.add_variable('stabilizer.yaw', 'float')
lg_pose.add_variable('stateEstimate.x', 'float')
lg_pose.add_variable('stateEstimate.y', 'float')
lg_pose.add_variable('stateEstimate.z', 'float')
lg_lh_master = LogConfig(name='master', period_in_ms=50)
lg_lh_master.add_variable('lighthouse.angle0x', 'float')
lg_lh_master.add_variable('lighthouse.angle0y', 'float')
lg_lh_master.add_variable('lighthouse.angle0x_3', 'float')
lg_lh_master.add_variable('lighthouse.angle0y_3', 'float')
lg_lh_master2 = LogConfig(name='master2', period_in_ms=50)
lg_lh_master2.add_variable('lighthouse.angle0x_1', 'float')
lg_lh_master2.add_variable('lighthouse.angle0y_1', 'float')
lg_lh_master2.add_variable('lighthouse.angle0x_2', 'float')
lg_lh_master2.add_variable('lighthouse.angle0y_2', 'float')
lg_lh_slave = LogConfig(name='slave', period_in_ms=50)
lg_lh_slave.add_variable('lighthouse.angle1x', 'float')
lg_lh_slave.add_variable('lighthouse.angle1y', 'float')
lg_lh_slave.add_variable('lighthouse.angle1x_3', 'float')
lg_lh_slave.add_variable('lighthouse.angle1y_3', 'float')
lg_lh_slave2 = LogConfig(name='slave2', period_in_ms=50)
lg_lh_slave2.add_variable('lighthouse.angle1x_1', 'float')
lg_lh_slave2.add_variable('lighthouse.angle1y_1', 'float')
lg_lh_slave2.add_variable('lighthouse.angle1x_2', 'float')
lg_lh_slave2.add_variable('lighthouse.angle1y_2', 'float')
self._cf.log.add_config(lg_pose)
lg_pose.data_received_cb.add_callback(self.data_receivedPose)
lg_pose.start()
self._cf.log.add_config(lg_lh_master)
lg_lh_master.data_received_cb.add_callback(self.data_receivedSensor)
lg_lh_master.start()
self._cf.log.add_config(lg_lh_master2)
lg_lh_master2.data_received_cb.add_callback(self.data_receivedSensor)
lg_lh_master2.start()
self._cf.log.add_config(lg_lh_slave)
lg_lh_slave.data_received_cb.add_callback(self.data_receivedSensor)
lg_lh_slave.start()
self._cf.log.add_config(lg_lh_slave2)
lg_lh_slave2.data_received_cb.add_callback(self.data_receivedSensor)
lg_lh_slave2.start()
self.read_geo_data()
def start_position_printing(scf):
log_conf = LogConfig(name='Position', period_in_ms=100)
log_conf.add_variable('kalman.stateX', 'float')
log_conf.add_variable('kalman.stateY', 'float')
log_conf.add_variable('kalman.stateZ', 'float')
uri= scf.cf.link_uri
scf.cf.log.add_config(log_conf)
# log_conf.data_received_cb.add_callback(position_callback)
log_conf.data_received_cb.add_callback( lambda timestamp, data, logconf: position_callback(uri, timestamp, data, logconf) )
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()
class CFLog():
def __init__(self):
self.status = CFStatus()
def start(self, cf):
self._lc_stab = LogConfig(name="Log-Stab", period_in_ms=50)
self._lc_stab.add_variable("stabilizer.roll", "float")
self._lc_stab.add_variable("stabilizer.pitch", "float")
self._lc_stab.add_variable("stabilizer.yaw", "float")
self._lc_stab.add_variable("stabilizer.thrust", "float")
self._lc_motor = LogConfig(name="Log-Motor", period_in_ms=50)
self._lc_motor.add_variable("pm.vbat", "float")
self._lc_motor.add_variable("motor.m1", "float") # Front (green)
self._lc_motor.add_variable("motor.m2", "float") # Right
self._lc_motor.add_variable("motor.m3", "float") # Back (red)
self._lc_motor.add_variable("motor.m4", "float") # Left
cf.log.add_config(self._lc_stab)
cf.log.add_config(self._lc_motor)
if self._lc_stab.valid and self._lc_motor.valid:
self._lc_stab.data_received_cb.add_callback(self._log_data)
self._lc_stab.error_cb.add_callback(self._log_error)
self._lc_stab.start()
self._lc_motor.data_received_cb.add_callback(self._log_data)
self._lc_motor.error_cb.add_callback(self._log_error)
self._lc_motor.start()
logger.info("Starting CFLog")
else:
logger.error(
"Could not add logconfig since some variables are not in TOC")
def stop(self):
self._lc_stab.stop()
def _log_error(self, logconf, msg):
"""Callback from the log API when an error occurs"""
logger.info("Error when logging %s: %s" % (logconf.name, msg))
def _log_data(self, timestamp, data, logconf):
"""Callback froma the log API when data arrives"""
if logconf.name == "Log-Stab":
self.status.pitch = data["stabilizer.pitch"]
self.status.roll = data["stabilizer.roll"]
self.status.yaw = data["stabilizer.yaw"]
self.status.thrust = data["stabilizer.thrust"]
else:
self.status.bat = data["pm.vbat"]
self.status.motor_1 = data["motor.m1"]
self.status.motor_2 = data["motor.m2"]
self.status.motor_3 = data["motor.m3"]
self.status.motor_4 = data["motor.m4"]
logger.info("%s" % self.status)
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 start_position_printing(self, scf):
log_conf = LogConfig(name='Position', period_in_ms=100)
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('lighthouse.x', 'float')
# log_conf.add_variable('lighthouse.y', 'float')
# log_conf.add_variable('lighthouse.z', 'float')
scf.cf.log.add_config(log_conf)
log_conf.data_received_cb.add_callback(self.position_callback)
log_conf.start()
class CFLog:
def __init__(self):
self.status = CFStatus()
def start(self, cf):
self._lc_stab = LogConfig(name="Log-Stab", period_in_ms=50)
self._lc_stab.add_variable("stabilizer.roll", "float")
self._lc_stab.add_variable("stabilizer.pitch", "float")
self._lc_stab.add_variable("stabilizer.yaw", "float")
self._lc_stab.add_variable("stabilizer.thrust", "float")
self._lc_motor = LogConfig(name="Log-Motor", period_in_ms=50)
self._lc_motor.add_variable("pm.vbat", "float")
self._lc_motor.add_variable("motor.m1", "float") # Front (green)
self._lc_motor.add_variable("motor.m2", "float") # Right
self._lc_motor.add_variable("motor.m3", "float") # Back (red)
self._lc_motor.add_variable("motor.m4", "float") # Left
cf.log.add_config(self._lc_stab)
cf.log.add_config(self._lc_motor)
if self._lc_stab.valid and self._lc_motor.valid:
self._lc_stab.data_received_cb.add_callback(self._log_data)
self._lc_stab.error_cb.add_callback(self._log_error)
self._lc_stab.start()
self._lc_motor.data_received_cb.add_callback(self._log_data)
self._lc_motor.error_cb.add_callback(self._log_error)
self._lc_motor.start()
logger.info("Starting CFLog")
else:
logger.error("Could not add logconfig since some variables are not in TOC")
def stop(self):
self._lc_stab.stop()
def _log_error(self, logconf, msg):
"""Callback from the log API when an error occurs"""
logger.info("Error when logging %s: %s" % (logconf.name, msg))
def _log_data(self, timestamp, data, logconf):
"""Callback froma the log API when data arrives"""
if logconf.name == "Log-Stab":
self.status.pitch = data["stabilizer.pitch"]
self.status.roll = data["stabilizer.roll"]
self.status.yaw = data["stabilizer.yaw"]
self.status.thrust = data["stabilizer.thrust"]
else:
self.status.bat = data["pm.vbat"]
self.status.motor_1 = data["motor.m1"]
self.status.motor_2 = data["motor.m2"]
self.status.motor_3 = data["motor.m3"]
self.status.motor_4 = data["motor.m4"]
logger.info("%s" % self.status)
def distance_measurement():
with SyncCrazyflie(uri, cf=Crazyflie(rw_cache='./cache')) as scf:
lpos = LogConfig(name='distance', period_in_ms=100)
lpos.add_variable('peerdist.distance2peer20')
scf.cf.log.add_config(lpos)
lpos.data_received_cb.add_callback(pos_data)
lpos.start()
time.sleep(40 + 2) #siample time
df = pd.DataFrame(dt, columns=['distance', 'time'])
df.to_csv('./distForEstVeloBy100ms.csv', index=False)
print("done")
return
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 connected(self, URI):
print('We are now connected to {}'.format(URI))
# The definition of the logconfig can be made before connecting
lpos = LogConfig(name='Position', period_in_ms=100)
lpos.add_variable('lighthouse.x')
lpos.add_variable('lighthouse.y')
lpos.add_variable('lighthouse.z')
lpos.add_variable('stabilizer.roll')
lpos.add_variable('stabilizer.pitch')
lpos.add_variable('stabilizer.yaw')
try:
self.cf.log.add_config(lpos)
lpos.data_received_cb.add_callback(self.pos_data)
lpos.start()
except KeyError as e:
print('Could not start log configuration,'
'{} not found in TOC'.format(str(e)))
except AttributeError:
print('Could not add Position log config, bad configuration.')
# lmeas = LogConfig(name='Meas', period_in_ms=100)
# lmeas.add_variable('range.front')
# lmeas.add_variable('range.back')
# lmeas.add_variable('range.up')
# lmeas.add_variable('range.left')
# lmeas.add_variable('range.right')
# lmeas.add_variable('range.zrange')
# lmeas.add_variable('stabilizer.roll')
# lmeas.add_variable('stabilizer.pitch')
# lmeas.add_variable('stabilizer.yaw')
# try:
# self.cf.log.add_config(lmeas)
# lmeas.data_received_cb.add_callback(self.meas_data)
# lmeas.start()
# except KeyError as e:
# print('Could not start log configuration,'
# '{} not found in TOC'.format(str(e)))
# except AttributeError:
# print('Could not add Measurement log config, bad configuration.')
lbat = LogConfig(name='Battery', period_in_ms=500) # read battery status with 2 Hz rate
lbat.add_variable('pm.vbat', 'float')
try:
self.cf.log.add_config(lbat)
lbat.data_received_cb.add_callback(self.battery_data)
lbat.start()
except KeyError as e:
print('Could not start log configuration,'
'{} not found in TOC'.format(str(e)))
except AttributeError:
print('Could not add Measurement log config, bad configuration.')
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 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 _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 _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, 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
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
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 _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, URI):
print('We are now connected to {}'.format(URI))
# The definition of the logconfig can be made before connecting
lpos = LogConfig(name='Position', period_in_ms=100)
lpos.add_variable('stateEstimate.x')
lpos.add_variable('stateEstimate.y')
lpos.add_variable('stateEstimate.z')
try:
self.cf.log.add_config(lpos)
lpos.data_received_cb.add_callback(self.pos_data)
lpos.start()
except KeyError as e:
print('Could not start log configuration,'
'{} not found in TOC'.format(str(e)))
except AttributeError:
print('Could not add Position log config, bad configuration.')
lmeas = LogConfig(name='Meas', period_in_ms=100)
lmeas.add_variable('range.front')
lmeas.add_variable('range.back')
lmeas.add_variable('range.up')
lmeas.add_variable('range.left')
lmeas.add_variable('range.right')
lmeas.add_variable('range.zrange')
lmeas.add_variable('stabilizer.roll')
lmeas.add_variable('stabilizer.pitch')
lmeas.add_variable('stabilizer.yaw')
try:
self.cf.log.add_config(lmeas)
lmeas.data_received_cb.add_callback(self.meas_data)
lmeas.start()
except KeyError as e:
print('Could not start log configuration,'
'{} not found in TOC'.format(str(e)))
except AttributeError:
print('Could not add Measurement log config, bad configuration.')
class MotorRampExample:
"""Example that connects to a Crazyflie and ramps the motors up/down and
the disconnects"""
def __init__(self, link_uri):
""" 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
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"
# The definition of the logconfig can be made before connecting
self._lg_stab = LogConfig(name="Stabilizer", period_in_ms=100)
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("mag.x", "float")
#self._lg_stab.add_variable("mag.y", "float")
#self._lg_stab.add_variable("mag.z", "float")
#self._lg_stab.add_variable("acc.x", "float")
#self._lg_stab.add_variable("acc.y", "float")
#self._lg_stab.add_variable("acc.z", "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")
# 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")
# Start a separate thread to do the motor test.
# Do not hijack the calling thread!
Thread(target=self._ramp_motors).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)
def _connection_lost(self, link_uri, msg):
"""Callback when disconnected after a connection has been made (i.e
Crazyflie moves out of range)"""
print "Connection to %s lost: %s" % (link_uri, msg)
def _disconnected(self, link_uri):
"""Callback when the Crazyflie is disconnected (called in all cases)"""
print "Disconnected from %s" % link_uri
def _ramp_motors(self):
print "ramp motors start"
thrust_mult = 1
thrust_step = 100
thrust = 29000
pitch = -5.2
roll = 14
yawrate = 0
while thrust >= 20000:
self._cf.commander.send_setpoint(roll, pitch, yawrate, thrust)
time.sleep(0.5)
if thrust >= 25000:
thrust_mult = -1
thrust += thrust_step * thrust_mult
print thrust
print "End"
print thrust
self._cf.commander.send_setpoint(0, 0, 0, 0)
# Make sure that the last packet leaves before the link is closed
# since the message queue is not flushed before closing
time.sleep(0.1)
self._cf.close_link()
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 UpDown:
"""docstring for UpDown"""
def __init__(self, link_uri):
self._cf = Crazyflie()
self._cf.connected.add_callback(self._connected)
self._cf.disconnected.add_callback(self._disconnected)
self._cf.connection_failed.add_callback(self._connection_failed)
self._cf.connection_lost.add_callback(self._connection_lost)
self._logger = LogConfig(name='Logger', period_in_ms=10)
self._logger.add_variable('acc.x', 'float')
self._logger.add_variable('acc.y', 'float')
self._logger.add_variable('acc.z', 'float')
self._logger.add_variable('acc.zw', 'float')
self._acc_x = 0
self._acc_y = 0
self._acc_z = 0
self._acc_zw = 0.0001
self.log = []
self.pidLog = []
self.acc_pid_x = None
self.acc_pid_y = None
self.acc_pid_z = None
self.vel_pid_x = None
self.vel_pid_y = None
self.vel_pid_z = None
print("Connecting to %s" % link_uri)
self._cf.open_link(link_uri)
self.is_connected = True
self.exit = False
self.init = False
Thread(target=self._exit_task).start()
Thread(target=self._run_task).start()
def _connected(self, link_uri):
""" This callback is called form the Crazyflie API when a Crazyflie
has been connected and the TOCs have been downloaded."""
self.initTime = time.clock()
# self.log_file = open('log.txt', 'w')
# self.pid_log = open('pid.txt', 'w')
# self.log_file.write('[\n')
# self.pid_log.write('[\n')
try:
self._cf.log.add_config(self._logger)
# This callback will receive the data
self._logger.data_received_cb.add_callback(self._acc_log_data)
# This callback will be called on errors
self._logger.error_cb.add_callback(self._acc_log_error)
# Start the logging
self._logger.start()
except KeyError as e:
print('Could not start log configuration,'
'{} not found in TOC'.format(str(e)))
except AttributeError:
print('Could not add Logger log config, bad configuration.')
def _acc_log_error(self, logconf, msg):
"""Callback from the log API when an error occurs"""
print('Error when logging %s: %s' % (logconf.name, msg))
def _acc_log_data(self, timestamp, data, logconf):
"""Callback from the log API when data arrives"""
self._acc_x = float(data['acc.x'])
self._acc_y = float(data['acc.y'])
self._acc_z = float(data['acc.z'])
self._acc_zw = data['acc.zw']
# self.log_file.write(json.dumps(data, sort_keys=True) + ",\n")
self.log.append(data)
self.init = True
def _connection_failed(self, link_uri, msg):
"""Callback when connection initial connection fails (i.e no Crazyflie
at the specified address)"""
print('Connection to %s failed: %s' % (link_uri, msg))
self.is_connected = False
# self.log_file.write(']')
# self.pid_log.write(']')
# self.log_file.close()
# self.pid_log.close()
with open('log.txt', 'w') as logFile:
json.dump(self.log, logFile)
with open('pid.txt', 'w') as pidFile:
json.dump(self.pidLog, pidFile)
def _connection_lost(self, link_uri, msg):
"""Callback when disconnected after a connection has been made (i.e
Crazyflie moves out of range)"""
print('Connection to %s lost: %s' % (link_uri, msg))
# self.log_file.write(']')
# self.pid_log.write(']')
# self.log_file.close()
# self.pid_log.close()
with open('log.txt', 'w') as logFile:
json.dump(self.log, logFile)
with open('pid.txt', 'w') as pidFile:
json.dump(self.pidLog, pidFile)
def _disconnected(self, link_uri):
"""Callback when the Crazyflie is disconnected (called in all cases)"""
print('Disconnected from %s' % link_uri)
self.is_connected = False
with open('log.txt', 'w') as logFile:
json.dump(self.log, logFile)
with open('pid.txt', 'w') as pidFile:
json.dump(self.pidLog, pidFile)
def _exit_task(self):
while not self.exit:
inp = int(input('Want to exit? [NO:0/YES:1]\n'))
if inp == 1:
self.exit = 1
def _run_task(self):
print("Running thread")
self._cf.commander.send_setpoint(0, 0, 0, 0)
while not self.exit:
if not self.init:
self.acc_pid_x = pid.PID(0, 0, 4.4, 0.05, 0.1, -1, 1)
self.acc_pid_y = pid.PID(0, 0, 4.4, 0.05, 0.1, -1, 1)
self.acc_pid_z = pid.PID(0, 0, 100, 0.0005, 0.1, 0.4, 2)
# Thread(target=self._log_pid).start()
continue
inp_acc_x = self._acc_x
self.out_acc_x, self.err_acc_x = self.acc_pid_x.compute(inp_acc_x)
self.setPitch = -self.out_acc_x*10
inp_acc_y = self._acc_y
self.out_acc_y, self.err_acc_y = self.acc_pid_y.compute(inp_acc_y)
self.setRoll = self.out_acc_y*10
inp_acc_z = self._acc_zw
self.out_acc_z, self.err_acc_z = self.acc_pid_z.compute(self._acc_zw)
self.setThrust = int(60000*self.out_acc_z/2)
self._thrust = self.setThrust
# data = {"IN_X": inp_acc_x, "OUT_X": out_acc_x,
# "IN_Y": inp_acc_y, "OUT_Y": out_acc_y,
# "IN_Z": self._acc_zw, "OUT_Z": out_acc_z,
# "OUT_Roll": setRoll, "OUT_Pitch": setPitch,
# "OUT_Thrust": setThrust}
# self.pidLog.append(data)
# self.pid_log.write(json.dumps(data, sort_keys=True) + ",\n")
self._cf.commander.send_setpoint(
self.setRoll, self.setPitch, 0, self._thrust)
time.sleep(0.01)
# self.log_file.write(']')
# self.pid_log.write(']')
# self.log_file.close()
# self.pid_log.close()
self._cf.commander.send_setpoint(0, 0, 0, 0)
time.sleep(1)
self._cf.close_link()
def _log_pid(self):
data = {"IN_X": self._acc_x, "OUT_X": self.out_acc_x,
"IN_Y": self._acc_y, "OUT_Y": self.out_acc_y,
"IN_Z": self._acc_zw, "OUT_Z": self.out_acc_z,
"OUT_Roll": self.setRoll, "OUT_Pitch": self.setPitch,
"OUT_Thrust": self.setThrust}
self.pidLog.append(data)
class RaceController:
"""Example that connects to a Crazyflie and ramps the motors up/down and
the disconnects"""
def __init__(self, link_uri):
# Set Initial values
# 43000 @ 3700mV
# 47000 @ 3500mV - not enough
self._thrust = 48000
self._roll = 0
self._rollTrim = 0
self._pitchSetPoint = 30
self._yawSetPoint = 0
self._initialYawSet = False
self._rollThrustFactor = 150
self._pitchTrustFactor = 250
self._yawThrustFactor = 100
self._rollPid = PID()
self._rollPid.SetKp(-2.0) # Proportional Gain
self._rollPid.SetKi(-0.75) # Integral Gain
self._rollPid.SetKd(0) # Derivative Gain
self._accPid = PID()
self._accPid.SetKp(1.0) # Proportional Gain
self._accPid.SetKi(0) # Integral Gain
self._accPid.SetKd(0) # Derivative Gain
self._lastAccZ = 0
self._yawPid = PID();
self._yawPid.SetKp(0.001) # Proportional Gain
self._yawPid.SetKi(0) # Integral Gain
self._yawPid.SetKd(0) # Derivative Gain
self._inRaceMode = True
self._raceInterval = 60
self._start = time.time()
self._lastAddPoint = 0;
""" 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, self._rollTrim, 0, 52000)
self._cf.commander.set_client_xmode(False)
# The definition of the loggconfig 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")
#self._lg_stab.add_variable("acc.x", "float")
#self._lg_stab.add_variable("acc.y", "float")
self._lg_stab.add_variable("acc.z", "float")
#self._lg_stab.add_variable("acc.zw", "float")
#self._lg_stab.add_variable("acc.mag2", "float")
self._lg_stab.add_variable("pm.vbat", "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']
#acc_x = new_dict['Logger']['acc.x']
#acc_y = new_dict['Logger']['acc.y']
acc_z = new_dict['Logger']['acc.z']
#acc_zw = new_dict['Logger']['acc.zw']
#acc_mag2 = new_dict['Logger']['acc.mag2']
battery = new_dict['Logger']['pm.vbat']
if stab_roll > 15 or stab_pitch > 45:
print "I'm out of control!!!!!"
self._cf.commander.send_setpoint(0, 0, 0, 0)
self._cf.close_link()
os._exit(1)
interval = time.time() - self._start
if interval > self._raceInterval and self._inRaceMode:
self._inRaceMode = False
self.land(40000, 2000)
thrust_boost = 0
if interval - self._lastAddPoint > 1:
self._lastAddPoint = interval
thrust_boost = 1000
prevRoll = self._roll
self._roll = self._rollPid.GenOut(stab_roll) + self._rollTrim
accSetPoint = self._accPid.GenOut(acc_z-self._lastAccZ);
self._lastAccZ = acc_z
yaw = 0
if not self._initialYawSet:
self._initialYawSet = True
self._yawSetPoint = stab_yaw
else:
yaw = self._yawPid.GenOut(self._yawSetPoint - stab_yaw)
yaw = 0
if self._inRaceMode:
self._cf.commander.send_setpoint(self._roll, self._pitchSetPoint, yaw, self._thrust + 5000 * accSetPoint + self._rollThrustFactor * abs(self._roll) + self._pitchTrustFactor * abs(stab_pitch) + self._yawThrustFactor * abs(stab_yaw - self._yawSetPoint) + thrust_boost)
print "Roll %s, Old Roll %s, Stab Roll = %s" % (self._roll, prevRoll, stab_roll)
#print "Set Point %s, Stab Yaw %s, Error =%s, Yaw Input %s" % (self._yawSetPoint, stab_yaw, stab_yaw - self._yawSetPoint, yaw)
print (battery * 1000)
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)
time.sleep(0.1)
self._cf.close_link()
os._exit(1)
def land(self, thrust, thrust_increment):
while thrust > thrust_increment:
print "landing"
thrust -= thrust_increment
self._cf.commander.send_setpoint(self._rollTrim, -self._pitchSetPoint , 0, thrust)
time.sleep(0.1)
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)
class Control:
def __init__(self, link_uri):
self.nthrust = 0
self.nroll = 0
self.npitch = 0
self.nyaw = 0
self.init_time = 0
self.time_elapsed = 0
# to plot graphics
self.thrust_v = []
self.roll_v = []
self.pitch_v = []
self.yaw_v = []
self.time_v = []
# 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)
self.is_connected = True
""" This callback is called form the Crazyflie API when a Crazyflie
has been connected and the TOCs have been downloaded."""
"""Callback when disconnected after a connection has been made (i.e
Crazyflie moves out of range)"""
def _connected(self, link_uri):
#initial time to be shown in the screen
self.init_time = time.time()
#Threat 1 that will control the quadridrone
Thread(target=self._ramp_motors).start()
#Thread 2 that will show results in the screen
Thread(target=self._show_values).start()
self._lg_stab = LogConfig(name='Stabilizer', period_in_ms=30)
#self._lg_stab.addVariable(LogVariable("stabilizer.thrust", "uint16_t"))
self._lg_stab.add_variable('stabilizer.roll', 'float')
self._lg_stab.add_variable('stabilizer.pitch', 'float')
self._lg_stab.add_variable('stabilizer.yaw', 'float')
self._lg_stab.add_variable('stabilizer.thrust', "uint16_t")
# Adding the configuration cannot be done until a Crazyflie is
# connected, since we need to check that the variables we
# would like to log are in the TOC.
try:
self._cf.log.add_config(self._lg_stab)
# This callback will receive the data
self._lg_stab.data_received_cb.add_callback(self._stab_log_data)
# This callback will be called on errors
self._lg_stab.error_cb.add_callback(self._stab_log_error)
#Thread 3 that will read from the TOC table, and start the logging
self._lg_stab.start()
except KeyError as e:
print('Could not start log configuration, {} not found in TOC'.format(str(e)))
except AttributeError:
print('Could not add Stabilizer log config, bad configuration.')
def _ramp_motors(self):
thrust_step = 400
self.nthrust = 37500
#thrust = 30500
self.npitch = 0.31
self.nroll = 0.56
yawrate = 0
nthrust_mult = 1
desired_roll = 1
desired_thrust = 35000 #40300
desired_pitch = 0.28
#desired_yaw = 0
gama = 0.3
# Unlock startup thrust protection
self._cf.commander.send_setpoint(0, 0, 0, 0)
while (self.nthrust > 0) and (self.time_elapsed < 13):
# if self.time_elapsed > 8:
# nthrust_mult = -1
#self.nthrust = self.nthrust + (nthrust_mult * thrust_step)
if (self.time_elapsed >= 4) and (self.time_elapsed < 8):
desired_thrust = 20000#27400#32767
if self.time_elapsed > 8:
desired_thrust = 0
st = self.nthrust
delta_thrust = desired_thrust - st
new_thrust = int(st + (delta_thrust*gama))
sr = self.nroll
delta_roll = desired_roll - sr
new_roll = sr + (delta_roll*gama)
#new_roll = sr * (-0.8) + 2
sp = self.npitch
delta_pitch = desired_pitch - sp
new_pitch = sp + (delta_pitch*gama)
#new_pitch = sp * (-0.8) -2
self._cf.commander.send_setpoint(new_roll, new_pitch, yawrate, new_thrust)
time.sleep(0.05)
#self._cf.param.set_value("flightmode.althold", "True")
#time.sleep(0.2)
self._cf.commander.send_setpoint(0, 0, 0, 0)
print("Send point 0 0 0 0")
time.sleep(0.1)
self._cf.close_link()
self.is_connected = False
print("Connection closed")
#pl.plot(self.time_v, self.thrust_v, 'b')
#pl.plot(self.time_v, self.roll_v, 'r')
#pl.plot(self.time_v, self.pitch_v, 'p')
#pl.plot(self.time_v, self.yaw_v, 'y')
#pl.show()#
"""Show values like time, npitch, nyaw, nroll and nthrust from the class"""
def _show_values(self):
while(self.is_connected):
self.time_elapsed = time.time() - self.init_time
print("Time: %.2f\tnroll: %.2f\tnpitch: %.2f\tnyaw: %.2f\tnthrust: %d" % (self.time_elapsed, self.nroll, self.npitch, self.nyaw, self.nthrust))
self.thrust_v = self.thrust_v + [self.nthrust]
self.roll_v = self.roll_v + [self.nroll]
self.pitch_v = self.pitch_v + [self.npitch]
self.yaw_v = self.yaw_v + [self.nyaw]
self.time_v = self.time_v + [self.time_elapsed]
time.sleep(0.1)
"""Callback when the Crazyflie is disconnected (called in all cases)"""
def _disconnected(self, link_uri):
print('Disconnected from %s' % link_uri)
self.is_connected = False
"""Callback when connection initial connection fails (i.e no Crazyflie
at the specified address)"""
def _connection_failed(self, link_uri, msg):
print('Connection to %s failed: %s' % (link_uri, msg))
"""Callback when disconnected after a connection has been made (i.e
Crazyflie moves out of range)"""
def _connection_lost(self, link_uri, msg):
print('Connection to %s lost: %s' % (link_uri, msg))
self.is_connected = False
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.nroll = data.get('stabilizer.roll', None)
self.npitch = data.get('stabilizer.pitch', None)
self.nyaw = data.get('stabilizer.yaw', None)
self.nthrust = data.get('stabilizer.thrust', None)
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 FlightTab(Tab, flight_tab_class):
uiSetupReadySignal = pyqtSignal()
_accel_data_signal = pyqtSignal(int, object, object)
_gyro_data_signal = pyqtSignal(int, object, object)
_motor_data_signal = pyqtSignal(int, object, object)
_imu_data_signal = pyqtSignal(int, object, object)
_althold_data_signal = pyqtSignal(int, object, object)
_baro_data_signal = pyqtSignal(int, object, object)
_input_updated_signal = pyqtSignal(float, float, float, float)
_rp_trim_updated_signal = pyqtSignal(float, float)
_emergency_stop_updated_signal = pyqtSignal(bool)
_log_error_signal = pyqtSignal(object, str)
#UI_DATA_UPDATE_FPS = 10
connectionFinishedSignal = pyqtSignal(str)
disconnectedSignal = pyqtSignal(str)
def __init__(self, tabWidget, helper, *args):
global D
D.thrust = 0
D.pitch = 0
D.yawrate = 0
D.roll = 0
rospy.init_node("cf_flightTab")
self.motor_pub = rospy.Publisher("cf_motorData", MotorData)
self.stab_pub = rospy.Publisher("cf_stabData", StabData)
self.acc_pub = rospy.Publisher("cf_accData", AccelData)
self.gyro_pub = rospy.Publisher("cf_gyroData", GyroData)
rospy.Subscriber("cf_textcmd", String, self._cmdCB)
super(FlightTab, self).__init__(*args)
self.setupUi(self)
self.tabName = "Flight Control"
self.menuName = "Flight Control"
self.tabWidget = tabWidget
self.helper = helper
self.disconnectedSignal.connect(self.disconnected)
self.connectionFinishedSignal.connect(self.connected)
# Incomming signals
self.helper.cf.connected.add_callback(
self.connectionFinishedSignal.emit)
self.helper.cf.disconnected.add_callback(self.disconnectedSignal.emit)
self._input_updated_signal.connect(self.updateInputControl)
self.helper.inputDeviceReader.input_updated.add_callback(
self._input_updated_signal.emit)
self._rp_trim_updated_signal.connect(self.calUpdateFromInput)
self.helper.inputDeviceReader.rp_trim_updated.add_callback(
self._rp_trim_updated_signal.emit)
self._emergency_stop_updated_signal.connect(self.updateEmergencyStop)
self.helper.inputDeviceReader.emergency_stop_updated.add_callback(
self._emergency_stop_updated_signal.emit)
self.helper.inputDeviceReader.althold_updated.add_callback(
lambda enabled: self.helper.cf.param.set_value("flightmode.althold", enabled))
self._gyro_data_signal.connect(self._gyro_data_received)
self._accel_data_signal.connect(self._accel_data_received)
self._imu_data_signal.connect(self._imu_data_received)
self._baro_data_signal.connect(self._baro_data_received)
self._althold_data_signal.connect(self._althold_data_received)
self._motor_data_signal.connect(self._motor_data_received)
self._log_error_signal.connect(self._logging_error)
# Connect UI signals that are in this tab
self.flightModeCombo.currentIndexChanged.connect(self.flightmodeChange)
self.minThrust.valueChanged.connect(self.minMaxThrustChanged)
self.maxThrust.valueChanged.connect(self.minMaxThrustChanged)
self.thrustLoweringSlewRateLimit.valueChanged.connect(
self.thrustLoweringSlewRateLimitChanged)
self.slewEnableLimit.valueChanged.connect(
self.thrustLoweringSlewRateLimitChanged)
self.targetCalRoll.valueChanged.connect(self._trim_roll_changed)
self.targetCalPitch.valueChanged.connect(self._trim_pitch_changed)
self.maxAngle.valueChanged.connect(self.maxAngleChanged)
self.maxYawRate.valueChanged.connect(self.maxYawRateChanged)
self.uiSetupReadySignal.connect(self.uiSetupReady)
self.clientXModeCheckbox.toggled.connect(self.changeXmode)
self.isInCrazyFlightmode = False
self.uiSetupReady()
self.clientXModeCheckbox.setChecked(GuiConfig().get("client_side_xmode"))
self.crazyflieXModeCheckbox.clicked.connect(
lambda enabled:
self.helper.cf.param.set_value("flightmode.x",
str(enabled)))
self.helper.cf.param.add_update_callback(
group="flightmode", name="xmode",
cb=( lambda name, checked:
self.crazyflieXModeCheckbox.setChecked(eval(checked))))
self.ratePidRadioButton.clicked.connect(
lambda enabled:
self.helper.cf.param.set_value("flightmode.ratepid",
str(enabled)))
self.angularPidRadioButton.clicked.connect(
lambda enabled:
self.helper.cf.param.set_value("flightmode.ratepid",
str(not enabled)))
self.helper.cf.param.add_update_callback(
group="flightmode", name="ratepid",
cb=(lambda name, checked:
self.ratePidRadioButton.setChecked(eval(checked))))
self.helper.cf.param.add_update_callback(
group="flightmode", name="althold",
cb=(lambda name, enabled:
self.helper.inputDeviceReader.setAltHold(eval(enabled))))
self.helper.cf.param.add_update_callback(
group="imu_sensors",
cb=self._set_available_sensors)
self.logBaro = None
self.logAltHold = None
self.ai = AttitudeIndicator()
self.verticalLayout_4.addWidget(self.ai)
self.splitter.setSizes([1000,1])
self.targetCalPitch.setValue(GuiConfig().get("trim_pitch"))
self.targetCalRoll.setValue(GuiConfig().get("trim_roll"))
def thrustToPercentage(self, thrust):
return ((thrust / MAX_THRUST) * 100.0)
def uiSetupReady(self):
flightComboIndex = self.flightModeCombo.findText(
GuiConfig().get("flightmode"), Qt.MatchFixedString)
if (flightComboIndex < 0):
self.flightModeCombo.setCurrentIndex(0)
self.flightModeCombo.currentIndexChanged.emit(0)
else:
self.flightModeCombo.setCurrentIndex(flightComboIndex)
self.flightModeCombo.currentIndexChanged.emit(flightComboIndex)
def _logging_error(self, log_conf, msg):
QMessageBox.about(self, "Log error", "Error when starting log config"
" [%s]: %s" % (log_conf.name, msg))
def _cmdCB(self, data):
m = data.data
print ("Recieved command: " + m)
if m is 'q':
self.helper.cf.commander.send_setpoint(0, 0, 0, 0)
elif m.count(" ")>0:
hpr,value=m.split(" ")
if(hpr is "pitch" or hpr is "p"):
D.pitch= float(value)
if(hpr=="yawrate" or hpr is "y"):
D.yawrate= float(value)
if(hpr=="roll" or hpr is "r"):
D.roll=float(value)
if(hpr=="thrust" or hpr is "t"):
D.thrust= int(value)
print (D.thrust)
if D.thrust <= 10000:
D.thrust = 10001
elif D.thrust > 60000:
D.thrust = 60000
self.helper.cf.commander.send_setpoint(D.roll, D.pitch, D.yawrate, D.thrust)
def _motor_data_received(self, timestamp, data, logconf):
if self.isVisible():
self.actualM1.setValue(data["motor.m1"])
self.actualM2.setValue(data["motor.m2"])
self.actualM3.setValue(data["motor.m3"])
self.actualM4.setValue(data["motor.m4"])
d = MotorData()
d.m1 = data["motor.m1"]
d.m2 = data["motor.m2"]
d.m3 = data["motor.m3"]
d.m4 = data["motor.m4"]
self.motor_pub.publish(d)
def _gyro_data_received(self, timestamp, data, logconf):
if self.isVisible():
d = GyroData()
d.x = data["gyro.x"]
d.y = data["gyro.y"]
d.z = data["gyro.z"]
self.gyro_pub.publish(d)
def _accel_data_received(self, timestamp, data, logconf):
if self.isVisible():
d = AccelData()
d.x = data["acc.x"]
d.y = data["acc.y"]
d.z = data["acc.z"]
self.acc_pub.publish(d)
def _baro_data_received(self, timestamp, data, logconf):
if self.isVisible():
self.actualASL.setText(("%.2f" % data["baro.aslLong"]))
self.ai.setBaro(data["baro.aslLong"])
def _althold_data_received(self, timestamp, data, logconf):
if self.isVisible():
target = data["altHold.target"]
if target>0:
if not self.targetASL.isEnabled():
self.targetASL.setEnabled(True)
self.targetASL.setText(("%.2f" % target))
self.ai.setHover(target)
elif self.targetASL.isEnabled():
self.targetASL.setEnabled(False)
self.targetASL.setText("Not set")
self.ai.setHover(0)
def _imu_data_received(self, timestamp, data, logconf):
if self.isVisible():
self.actualRoll.setText(("%.2f" % data["stabilizer.roll"]))
self.actualPitch.setText(("%.2f" % data["stabilizer.pitch"]))
self.actualYaw.setText(("%.2f" % data["stabilizer.yaw"]))
self.actualThrust.setText("%.2f%%" %
self.thrustToPercentage(
data["stabilizer.thrust"]))
self.ai.setRollPitch(-data["stabilizer.roll"],
data["stabilizer.pitch"])
d = StabData()
d.pitch = data["stabilizer.pitch"]
d.roll = data["stabilizer.roll"]
d.yaw = data["stabilizer.yaw"]
self.stab_pub.publish(d)
def connected(self, linkURI):
# IMU & THRUST
lg = LogConfig("Stabalizer", GuiConfig().get("ui_update_period"))
lg.add_variable("stabilizer.roll", "float")
lg.add_variable("stabilizer.pitch", "float")
lg.add_variable("stabilizer.yaw", "float")
lg.add_variable("stabilizer.thrust", "uint16_t")
self.helper.cf.log.add_config(lg)
if (lg.valid):
lg.data_received_cb.add_callback(self._imu_data_signal.emit)
lg.error_cb.add_callback(self._log_error_signal.emit)
lg.start()
else:
logger.warning("Could not setup logconfiguration after "
"connection!")
# MOTOR
lg = LogConfig("Motors", GuiConfig().get("ui_update_period"))
lg.add_variable("motor.m1")
lg.add_variable("motor.m2")
lg.add_variable("motor.m3")
lg.add_variable("motor.m4")
self.helper.cf.log.add_config(lg)
if lg.valid:
lg.data_received_cb.add_callback(self._motor_data_signal.emit)
lg.error_cb.add_callback(self._log_error_signal.emit)
lg.start()
else:
logger.warning("Could not setup logconfiguration after "
"connection!")
#Accel
lg = LogConfig("Accelerometer", GuiConfig().get("ui_update_period"))
lg.add_variable("acc.x")
lg.add_variable("acc.y")
lg.add_variable("acc.z")
self.helper.cf.log.add_config(lg)
if lg.valid:
lg.data_received_cb.add_callback(self._accel_data_signal.emit)
lg.error_cb.add_callback(self._log_error_signal.emit)
lg.start()
else:
logger.warning("Could not setup logconfiguration after "
"connection!")
#Gyroscope
lg = LogConfig("Gyroscope", GuiConfig().get("ui_update_period"))
lg.add_variable("gyro.x")
lg.add_variable("gyro.y")
lg.add_variable("gyro.z")
self.helper.cf.log.add_config(lg)
if lg.valid:
print ("Gyro logging started correctly")
lg.data_received_cb.add_callback(self._gyro_data_signal.emit)
lg.error_cb.add_callback(self._log_error_signal.emit)
lg.start()
else:
logger.warning("Could not setup logconfiguration after "
"connection!")
def _set_available_sensors(self, name, available):
logger.info("[%s]: %s", name, available)
available = eval(available)
if ("HMC5883L" in name):
if (not available):
self.actualASL.setText("N/A")
self.actualASL.setEnabled(False)
else:
self.actualASL.setEnabled(True)
self.helper.inputDeviceReader.setAltHoldAvailable(available)
if (not self.logBaro and not self.logAltHold):
# The sensor is available, set up the logging
self.logBaro = LogConfig("Baro", 200)
self.logBaro.add_variable("baro.aslLong", "float")
self.helper.cf.log.add_config(self.logBaro)
if self.logBaro.valid:
self.logBaro.data_received_cb.add_callback(
self._baro_data_signal.emit)
self.logBaro.error_cb.add_callback(
self._log_error_signal.emit)
self.logBaro.start()
else:
logger.warning("Could not setup logconfiguration after "
"connection!")
self.logAltHold = LogConfig("AltHold", 200)
self.logAltHold.add_variable("altHold.target", "float")
self.helper.cf.log.add_config(self.logAltHold)
if self.logAltHold.valid:
self.logAltHold.data_received_cb.add_callback(
self._althold_data_signal.emit)
self.logAltHold.error_cb.add_callback(
self._log_error_signal.emit)
self.logAltHold.start()
else:
logger.warning("Could not setup logconfiguration after "
"connection!")
def disconnected(self, linkURI):
self.ai.setRollPitch(0, 0)
self.actualM1.setValue(0)
self.actualM2.setValue(0)
self.actualM3.setValue(0)
self.actualM4.setValue(0)
self.actualRoll.setText("")
self.actualPitch.setText("")
self.actualYaw.setText("")
self.actualThrust.setText("")
self.actualASL.setText("")
self.targetASL.setText("Not Set")
self.targetASL.setEnabled(False)
self.actualASL.setEnabled(False)
self.logBaro = None
self.logAltHold = None
def minMaxThrustChanged(self):
self.helper.inputDeviceReader.set_thrust_limits(
self.minThrust.value(), self.maxThrust.value())
if (self.isInCrazyFlightmode == True):
GuiConfig().set("min_thrust", self.minThrust.value())
GuiConfig().set("max_thrust", self.maxThrust.value())
def thrustLoweringSlewRateLimitChanged(self):
self.helper.inputDeviceReader.set_thrust_slew_limiting(
self.thrustLoweringSlewRateLimit.value(),
self.slewEnableLimit.value())
if (self.isInCrazyFlightmode == True):
GuiConfig().set("slew_limit", self.slewEnableLimit.value())
GuiConfig().set("slew_rate", self.thrustLoweringSlewRateLimit.value())
def maxYawRateChanged(self):
logger.debug("MaxYawrate changed to %d", self.maxYawRate.value())
self.helper.inputDeviceReader.set_yaw_limit(self.maxYawRate.value())
if (self.isInCrazyFlightmode == True):
GuiConfig().set("max_yaw", self.maxYawRate.value())
def maxAngleChanged(self):
logger.debug("MaxAngle changed to %d", self.maxAngle.value())
self.helper.inputDeviceReader.set_rp_limit(self.maxAngle.value())
if (self.isInCrazyFlightmode == True):
GuiConfig().set("max_rp", self.maxAngle.value())
def _trim_pitch_changed(self, value):
logger.debug("Pitch trim updated to [%f]" % value)
self.helper.inputDeviceReader.set_trim_pitch(value)
GuiConfig().set("trim_pitch", value)
def _trim_roll_changed(self, value):
logger.debug("Roll trim updated to [%f]" % value)
self.helper.inputDeviceReader.set_trim_roll(value)
GuiConfig().set("trim_roll", value)
def calUpdateFromInput(self, rollCal, pitchCal):
logger.debug("Trim changed on joystick: roll=%.2f, pitch=%.2f",
rollCal, pitchCal)
self.targetCalRoll.setValue(rollCal)
self.targetCalPitch.setValue(pitchCal)
def updateInputControl(self, roll, pitch, yaw, thrust):
self.targetRoll.setText(("%0.2f" % roll))
self.targetPitch.setText(("%0.2f" % pitch))
self.targetYaw.setText(("%0.2f" % yaw))
self.targetThrust.setText(("%0.2f %%" %
self.thrustToPercentage(thrust)))
self.thrustProgress.setValue(thrust)
def setMotorLabelsEnabled(self, enabled):
self.M1label.setEnabled(enabled)
self.M2label.setEnabled(enabled)
self.M3label.setEnabled(enabled)
self.M4label.setEnabled(enabled)
def emergencyStopStringWithText(self, text):
return ("<html><head/><body><p>"
"<span style='font-weight:600; color:#7b0005;'>{}</span>"
"</p></body></html>".format(text))
def updateEmergencyStop(self, emergencyStop):
if emergencyStop:
self.setMotorLabelsEnabled(False)
self.emergency_stop_label.setText(
self.emergencyStopStringWithText("Kill switch active"))
else:
self.setMotorLabelsEnabled(True)
self.emergency_stop_label.setText("")
def flightmodeChange(self, item):
GuiConfig().set("flightmode", self.flightModeCombo.itemText(item))
logger.info("Changed flightmode to %s",
self.flightModeCombo.itemText(item))
self.isInCrazyFlightmode = False
if (item == 0): # Normal
self.maxAngle.setValue(GuiConfig().get("normal_max_rp"))
self.maxThrust.setValue(GuiConfig().get("normal_max_thrust"))
self.minThrust.setValue(GuiConfig().get("normal_min_thrust"))
self.slewEnableLimit.setValue(GuiConfig().get("normal_slew_limit"))
self.thrustLoweringSlewRateLimit.setValue(
GuiConfig().get("normal_slew_rate"))
self.maxYawRate.setValue(GuiConfig().get("normal_max_yaw"))
if (item == 1): # Advanced
self.maxAngle.setValue(GuiConfig().get("max_rp"))
self.maxThrust.setValue(GuiConfig().get("max_thrust"))
self.minThrust.setValue(GuiConfig().get("min_thrust"))
self.slewEnableLimit.setValue(GuiConfig().get("slew_limit"))
self.thrustLoweringSlewRateLimit.setValue(
GuiConfig().get("slew_rate"))
self.maxYawRate.setValue(GuiConfig().get("max_yaw"))
self.isInCrazyFlightmode = True
if (item == 0):
newState = False
else:
newState = True
self.maxThrust.setEnabled(newState)
self.maxAngle.setEnabled(newState)
self.minThrust.setEnabled(newState)
self.thrustLoweringSlewRateLimit.setEnabled(newState)
self.slewEnableLimit.setEnabled(newState)
self.maxYawRate.setEnabled(newState)
@pyqtSlot(bool)
def changeXmode(self, checked):
self.helper.cf.commander.set_client_xmode(checked)
GuiConfig().set("client_side_xmode", checked)
logger.info("Clientside X-mode enabled: %s", checked)
class 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 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 LogGroup(QTreeWidgetItem):
""" Group
If turned on and no children are active, activate all children
if hz changed, create new log (and start if on)
Everytime the children change, we stop, delete the old log and create a new one, optionally starting it
Everytime the HZ changes, we stop, delete the old log and create a new one, optionally starting it
Everytime we change, we start/stop the log
We request a start, use partially checked
Once started, use totally checked
"""
def __init__(self, parent, name, children, hz=50):
super(LogGroup, self).__init__(parent)
self.name = name
self.setFlags(self.flags() | Qt.ItemIsEditable | Qt.ItemIsUserCheckable)
# Keep track of if all/none of the children are active
self.cAll = True
self.cNone = True
self.validHZ = list(OrderedDict.fromkeys([round(100/floor(100/x),2) for x in range(1,100)]))
self.hzTarget = hz
# Monitor the incoming frequency
self.fm = None
self.fmOn = True
# log config
self.lg = None
# Show text
QtGui.QTreeWidgetItem.setData(self, 0, Qt.DisplayRole, QVariant(name))
QtGui.QTreeWidgetItem.setData(self, 0, Qt.CheckStateRole, Qt.Unchecked)
QtGui.QTreeWidgetItem.setData(self, 1, Qt.DisplayRole, "Off")
QtGui.QTreeWidgetItem.setData(self, 3, Qt.DisplayRole, QVariant(0))
#QtGui.QTreeWidgetItem.setData(self, 4, Qt.CheckStateRole, Qt.Unchecked)
self.readSettings()
# Initialise Children
for c in sorted(children.keys()):
self.addChild(LogItem(self, children[c]))
self.c = 0
# Now everything is initialised except the logging
# Start logging if active
if self.checkState(0) == Qt.Checked:
self.requestLog()
def setEstimateHzOn(self, on):
""" If on then we esstiamte the HZ """
self.fmOn = on
def getValidHZ(self, hz):
# zip(list(OrderedDict.fromkeys([round(100/floor(100/x),2) for x in range(1,100)])),list(OrderedDict.fromkeys([1000/round(100/floor(100/x),2) for x in range(1,100)])))
i = bisect.bisect_left(self.validHZ, hz)
vHZ = min(self.validHZ[max(0, i-1): i+2], key=lambda t: abs(hz - t))
if hz!=vHZ:
rospy.loginfo("Could not set HZ to specific value [%d], rounded to valid HZ [%.4f]", hz, vHZ)
return vHZ
def readSettings(self):
""" Read the HZ and selected things to log from previous session """
settings = QSettings("omwdunkley", "flieROS")
# Read target HZ
hzQv = settings.value("log_"+self.name+"_hz",QVariant(20))
QtGui.QTreeWidgetItem.setData(self, 2, Qt.DisplayRole, hzQv)
self.hzTarget = self.getValidHZ(hzQv.toFloat()[0])
# Read if checked. If partially checked, uncheck
onQv = settings.value("log_"+self.name+"_on", QVariant(Qt.Unchecked))
QtGui.QTreeWidgetItem.setData(self, 0, Qt.CheckStateRole, onQv if onQv.toInt()[0]!=Qt.PartiallyChecked else Qt.Unchecked)
#onRos = settings.value("ros_"+self.name+"_on", QVariant(Qt.Unchecked))
#QtGui.QTreeWidgetItem.setData(self, 0, Qt.CheckStateRole, onRos)
def writeSettings(self):
""" Save the HZ and selected things to log between sessions"""
settings = QSettings("omwdunkley", "flieROS")
# Save target HZ
settings.setValue("log_"+self.name+"_hz", self.data(2, Qt.DisplayRole))
# Save checked state
settings.setValue("log_"+self.name+"_on", self.data(0, Qt.CheckStateRole))
#settings.setValue("ros_"+self.name+"_on", self.data(4, Qt.CheckStateRole))
# Save children state too
for x in range(self.childCount()):
self.child(x).writeSettings()
def updateFM(self):
# if not self.isActive()
if self.fm:
hz = self.fm.get_hz()
QtGui.QTreeWidgetItem.setData(self, 3, Qt.DisplayRole, round(hz,2))
return self.name, hz#, -1.0 if self.lg is None else self.hzTarget
else:
return self.name, -1.0#, -1.0 if self.lg is None else self.hzTarget
def logDataCB(self, ts, data, lg):
""" Our data from the log """
#print "LogCB data:", data.keys()
# Update out HZ monitor
if self.fmOn:
self.fm.count()
#TODO: add a checkbox in the row if we wanan send or not
self.treeWidget().incomingData(data, ts)
#TODO: there is bug here where the callbacks are called twice!! Holds for this + deleted CB
def logStartedCB(self, on):
""" Called when we actually start logging """
self.c+=1
#print "counter[%s]"%self.name, self.c
#if self.c % 2 == 1:
# return
if on:
rospy.loginfo( "[%d: %s @ %.2fhz] LogCB STARTED" % (self.lg.id, self.name, self.hzTarget))
QtGui.QTreeWidgetItem.setData(self, 0, Qt.CheckStateRole, Qt.Checked)
QtGui.QTreeWidgetItem.setData(self, 1, Qt.DisplayRole, "On")
self.setAllState(Qt.Checked, activeOnly=True)
else:
rospy.loginfo( "[%s] LogCB STOPPED" % ( self.name))
#print "[%s]LogCB ENDED(%d) " % (self.name, self.c)
#if self.allSelected():
# self.setAllState(Qt.Unchecked)
##else:
## self.setAllState(Qt.Checked, activeOnly=True)
#QtGui.QTreeWidgetItem.setData(self, 0, Qt.CheckStateRole, Qt.Unchecked)
#QtGui.QTreeWidgetItem.setData(self, 1, Qt.DisplayRole, "Off")
# def logAddedCB(self, l):
# """ Called when log added """
# print "LogCB added"
# QtGui.QTreeWidgetItem.setData(self, 0, Qt.CheckStateRole, Qt.Checked)
# QtGui.QTreeWidgetItem.setData(self, 1, Qt.DisplayRole, "Added")
def requestLog(self):
""" user requested start """
#print "Requested log start"
#print "None selected:",self.noneSelected()
self.setAllState(Qt.PartiallyChecked, activeOnly=not self.noneSelected())
QtGui.QTreeWidgetItem.setData(self, 0, Qt.CheckStateRole, Qt.PartiallyChecked)
QtGui.QTreeWidgetItem.setData(self, 1, Qt.DisplayRole, "Requested")
# Make and start log config
self.makeLog()
def stopLog(self):
""" User request halt """
#print "Requested log stop"
if self.lg:
self.lg.delete() #Invokes logStarted(False)
self.lg = None
if self.allSelected():
self.setAllState(Qt.Unchecked)
#else:
# self.setAllState(Qt.Checked, activeOnly=True)
QtGui.QTreeWidgetItem.setData(self, 0, Qt.CheckStateRole, Qt.Unchecked)
QtGui.QTreeWidgetItem.setData(self, 1, Qt.DisplayRole, "Off")
self.treeWidget().sig_logStatus.emit(self.name, -1)
self.fm = None
def errorLog(self, block, msg):
""" When a log error occurs """
#print "Log error: %s:", msg
QtGui.QTreeWidgetItem.setData(self, 1, Qt.DisplayRole, "Err: %s"%msg)
QtGui.QTreeWidgetItem.setData(self, 0, Qt.CheckStateRole, Qt.Unchecked)
if self.allSelected():
self.setAllState(Qt.Unchecked)
else:
self.setAllState(Qt.Checked, activeOnly=True)
self.fm = None
def setData(self, column, role, value):
""" Only allow changing of the check state and the hz """
# GROUP ON/OFF
preValue = self.checkState(0)
if role == Qt.CheckStateRole and column == 0 and preValue != value:
# Check box changed, allow and trigger
#print "\n\nCONFIG CHANGED\n"
if preValue==Qt.PartiallyChecked:
#print "User clicked: Waiting for log callback, cannot change config now"
pass
elif value==Qt.Checked:
#print "User clicked: request logging"
self.requestLog()
elif value==Qt.Unchecked:
#print "User clicked: request delete logging"
self.stopLog()
# HZ CHANGED
if column == 2:
#hz = min(max(value.toInt()[0],1),100)
hz = self.getValidHZ(value.toFloat()[0])
preHz = self.data(column, Qt.DisplayRole).toFloat()[0]
#print "\n\nHZ CHANGED\n"
if hz!=preHz:
if preValue==Qt.PartiallyChecked:
pass
#print "Waiting for log callback, cannot change hz now"
elif preValue==Qt.Checked:
#print "hz changed from %.2f to %.2f while logging, replace log" % (preHz, hz)
QtGui.QTreeWidgetItem.setData(self, column, role, hz)
self.hzTarget = hz
self.requestLog()
elif preValue==Qt.Unchecked:
#print "hz changed from %.2f to %.2f, not logging yet" % (preHz, hz)
QtGui.QTreeWidgetItem.setData(self, column, role, hz)
self.hzTarget = hz
def childStateChanged(self, child, newState):
if self.checkState(0) == Qt.Unchecked:
#print "Child Changed while not logging"
QtGui.QTreeWidgetItem.setData(child, 0, Qt.CheckStateRole, newState)
elif self.checkState(0) == Qt.PartiallyChecked:
pass
#print "Waiting for log callback"
elif self.checkState(0) == Qt.Checked:
QtGui.QTreeWidgetItem.setData(child, 0, Qt.CheckStateRole, newState)
if self.noneSelected():
#print "Last child unselected, remove logger"
self.stopLog()
elif self.allSelected():
#print "All children selected while logging, update logger"
self.requestLog()
else:
#print "Child Changed while logging, update logger"
self.requestLog()
def makeLog(self):
""" Makes a log. All ative children are added """
#print " -> Making new Log with %d ms interval"
if self.lg:
#print " ---> Previous Log detected, removing first"
self.lg.delete()
self.fm = FreqMonitor(window=max(10, int(1.2*self.hzTarget)))
self.lg = LogConfig(self.name, int(round(1000./self.hzTarget)))
#print " ---> Adding children to new log"
for x in range(self.childCount()):
c = self.child(x)
if c.isChecked():
name = c.log.group+"."+c.log.name
self.lg.add_variable(name, c.log.ctype)
#print " --- --> Adding child[%d]: [%s] to new log"%(x, name)
#print " ---> Checking log with TOC"
self.treeWidget().cf.log.add_config(self.lg)
if self.lg.valid:
#print " --- --> PASS"
self.lg.data_received_cb.add_callback(self.logDataCB)
self.lg.started_cb.add_callback(self.logStartedCB)
self.lg.error_cb.add_callback(self.treeWidget().sig_logError.emit)
self.lg.error_cb.add_callback(self.errorLog)
self.treeWidget().sig_logStatus.emit(self.name, self.hzTarget)
#print " --- --> callbacks added, starting new log NOW"
self.lg.start()
else:
#print " --- --> FAIL"
self.errorLog(None, "Invalid Config")
self.lg = None
self.fm = None
def setAllState(self, chkState, activeOnly=False):
""" Set all children on or off without their setData function. If activeOnly only set ones who are not off """
if activeOnly:
#print " -> Setting all ACTIVE children to checkstate [%d]", chkState
for x in range(self.childCount()):
if self.child(x).isChecked():
self.child(x).setState(chkState)
#print " --->", self.child(x).log.name
else:
#print " -> Setting all children to checkstate [%d]", chkState
for x in range(self.childCount()):
self.child(x).setState(chkState)
#print " --->", self.child(x).log.name
def allSelected(self):
""" Returns true if all children are selected """
for x in range(self.childCount()):
if not self.child(x).isChecked():
return False
return True
def noneSelected(self):
""" Returns true if no children are selected """
for x in range(self.childCount()):
if self.child(x).isChecked():
return False
return True
class DanceController:
def __init__(self, link_uri):
# Set Initial values
# 43000 @ 3700mV
# 47000 @ 3500mV - not enough
self._thrust = 10000
self._rollTrim = 0
self._pitchTrim = 0
self._rollThrustFactor = 150
self._pitchTrustFactor = 250
self._maxBatteryCounter = 50
self._batteryCounter = 0
self._batteryVoltage = 4000 #about full
""" 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.is_connected = False
self._cf.open_link(link_uri)
print "Connecting to %s" % link_uri
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, self._rollTrim, 0, 29000)
self._cf.commander.set_client_xmode(False)
# The definition of the loggconfig 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")
self._lg_stab.add_variable("pm.vbat", "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")
# Variable used to keep main loop occupied until disconnect
self.is_connected = True
print "Starting dance thread..."
Thread(target=self.dance).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)
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']
battery = new_dict['Logger']['pm.vbat']
if stab_roll > 99 or stab_pitch > 99:
print "I'm out of control!!!!!"
self._cf.commander.send_setpoint(0, 0, 0, 0)
self._cf.close_link()
os._exit(1)
# prevRoll = self._roll
# self._roll = self._rollPid.GenOut(stab_roll) + self._rollTrim
# print "Roll %s, Old Roll %s, Stab Roll = %s" % (self._roll, prevRoll, stab_roll)
# print battery
self._batteryCounter += 1
if(self._batteryCounter > self._maxBatteryCounter):
self._batteryVoltage = battery * 1000
print self._batteryVoltage
self._batteryCounter = 0
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)
time.sleep(0.1)
self._cf.close_link()
os._exit(1)
def dance(self):
print "Dance start"
# start music
Thread(target=self.play_music).start()
primary_beat_interval = 1.3
thrust_increment = 2000
max_thrust = 54000
self.adjust_thrust(30000)
roll = 0
pitch = 0
yaw = 0
self.rev_motors(primary_beat_interval)
self.lift_off(self._thrust, max_thrust, thrust_increment)
# hover
self.adjust_thrust(48000)
self.hover(roll, yaw, self._thrust, thrust_increment)
# spin
self.adjust_thrust(45000)
magnitude = -90
spinCounter = 4
while spinCounter > 0:
self.spin(magnitude, 1, self._thrust)
spinCounter -= 1
# level out
self.adjust_thrust(48000)
# turn to give enough space for dart
# self.spin(magnitude, -1, self._thrust)
# level out
self.level_out(self._thrust)
# dart left
self.adjust_thrust(38000)
magnitude = 80
self.dart(magnitude, 1, self._thrust)
# level out
self.adjust_thrust(45000)
self.level_out(self._thrust)
# gain altitude and turn
altitude_counter = 2
while altitude_counter > 0:
self.adjust_thrust(max_thrust)
self.hover(0, 90, self._thrust, thrust_increment)
altitude_counter -= 1
# level out
self.adjust_thrust(44000)
self.level_out(self._thrust)
# dart right
self.adjust_thrust(37000)
magnitude = 80
self.dart(magnitude, 1, self._thrust)
# level out
self.adjust_thrust(46000)
self.level_out(self._thrust)
# spin
self.adjust_thrust(44000)
magnitude = 40
spinCounter = 4
while spinCounter > 0:
self.spin(magnitude, 1, self._thrust)
spinCounter -= 1
# level out
self.level_out(self._thrust)
# spin
self.adjust_thrust(45000)
self.spin(magnitude, -1, self._thrust)
self.spin(magnitude, -1, self._thrust)
self.spin(magnitude, 1, self._thrust)
self.spin(magnitude, 1, self._thrust)
self.spin(magnitude, -1, self._thrust)
self.spin(magnitude, -1, self._thrust)
# level out
self.level_out(self._thrust)
# spin up
magnitude = 40
self.adjust_thrust(max_thrust)
self.spin(magnitude, -1, self._thrust)
self.spin(magnitude, -1, self._thrust)
# spin down
self.adjust_thrust(42000)
self.spin(magnitude, -1, self._thrust)
self.spin(magnitude, -1, self._thrust)
# spin up
magnitude = 70
self.adjust_thrust(max_thrust - 4000)
self.spin(magnitude, -1, self._thrust)
self.spin(magnitude, -1, self._thrust)
# spin down
self.adjust_thrust(42000)
self.spin(magnitude, -1, self._thrust)
self.spin(magnitude, -1, self._thrust)
# spin up
magnitude = 100
self.adjust_thrust(46000)
self.spin(magnitude, -1, self._thrust)
self.spin(magnitude, -1, self._thrust)
# spin down
magnitude = 70
self.adjust_thrust(42000)
self.spin(magnitude, -1, self._thrust)
self.spin(magnitude, -1, self._thrust)
# land
self.land(self._thrust, thrust_increment)
print "End"
self._cf.commander.send_setpoint(0, 0, 0, 0)
# make sure the music has time to end
time.sleep(8.0)
# Make sure that the last packet leaves before the link is closed
# since the message queue is not flushed before closing
time.sleep(0.1)
self._cf.close_link()
os.exit(0)
# dance helper functions
def adjust_thrust(self, newThrust):
self._thrust = newThrust
print self._batteryVoltage
if(self._batteryVoltage > 4100):
self._thrust -= 2000
if(self._batteryVoltage > 4000):
self._thrust -= 2000
if(self._batteryVoltage < 3700):
self._thrust += 1000
if(self._batteryVoltage < 3600):
self._thrust += 1000
if(self._batteryVoltage < 3300):
self._thrust += 2000
if(self._batteryVoltage < 3000):
self._thrust += 2000
print "ADJUSTED THRUST"
print self._thrust
def rev_motors(self, primary_beat_interval):
revCount = 2
while revCount > 0:
self._cf.commander.send_setpoint(self._rollTrim, self._pitchTrim, 0, 30000)
# time.sleep(primary_beat_interval)
time.sleep(0.01)
self._cf.commander.send_setpoint(self._rollTrim, self._pitchTrim, 0, 0)
time.sleep(primary_beat_interval)
revCount -= 1
def lift_off(self, thrust, max_thrust, thrust_increment):
print "lifting off"
while self._thrust < max_thrust:
self.adjust_thrust(self._thrust + thrust_increment)
self._cf.commander.send_setpoint(self._rollTrim, self._pitchTrim, 0, self._thrust)
time.sleep(0.1)
def hover(self, roll, yaw, thrust, thrust_increment):
print "hovering"
hoverCount = 4
while hoverCount > 0:
thrust -= thrust_increment
self._cf.commander.send_setpoint(roll + self._rollTrim, -4 + self._pitchTrim, yaw, thrust)
time.sleep(0.1)
thrust += thrust_increment
self._cf.commander.send_setpoint(roll + self._rollTrim, 4 + self._pitchTrim, yaw, thrust)
time.sleep(0.1)
hoverCount -= 1
def dart(self, magnitude, direction, thrust):
print "darting"
self._cf.commander.send_setpoint(magnitude*direction, magnitude*direction, 0, thrust)
time.sleep(0.3)
def spin(self, magnitude, direction, thrust):
print "spinning"
self._cf.commander.send_setpoint(self._rollTrim, self._pitchTrim, magnitude*direction*4, thrust)
time.sleep(0.3)
self._cf.commander.send_setpoint(self._rollTrim, self._pitchTrim, magnitude*direction*4, thrust)
time.sleep(0.3)
self._cf.commander.send_setpoint(self._rollTrim, self._pitchTrim, 0, thrust)
def level_out(self, thrust):
counter = 4
while counter > 0:
print "leveling out"
self._cf.commander.send_setpoint(self._rollTrim, self._pitchTrim, 0, thrust)
time.sleep(0.1)
counter -= 1
def land(self, thrust, thrust_increment):
while thrust > thrust_increment:
print "landing"
thrust -= thrust_increment
self._cf.commander.send_setpoint(self._rollTrim, self._pitchTrim, 0, thrust)
time.sleep(0.1)
def play_music(self):
winsound.PlaySound('FlightOfTheBumblebeeEDIT.wav', winsound.SND_FILENAME)
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, linkURI):
# PROXIMITY
lg = LogConfig("Proximity", GuiConfig().get("ui_update_period"))
lg.add_variable("adc.vProx", "float")
self.helper.cf.log.add_config(lg)
if (lg.valid):
lg.data_received_cb.add_callback(self._prox_data_signal.emit)
lg.error_cb.add_callback(self._log_error_signal.emit)
lg.start()
else:
logger.warning("Could not setup logconfiguration after connection!")
# ACCELEROMETER
lg = LogConfig("Accelerometer", GuiConfig().get("ui_update_period"))
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._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!")
# MAGNETOMETER
lg = LogConfig("Magnetometer", GuiConfig().get("ui_update_period"))
lg.add_variable("mag.x", "float")
lg.add_variable("mag.y", "float")
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!")
# 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!")
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)
