def __init__(self, link_uri, cf):
""" Initialize and run the example with the specified link_uri and cf object """
# Variable used to keep main loop occupied until disconnect
self._cf = cf
self.is_connected = True
# The definition of the logconfig can be made before connecting
self._lg_stab = LogConfig(
name='Stabilizer', period_in_ms=1000
) # self.lg_stab is a LogConfig object FOR THE STABILIZER. See
# log.py for details.
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', 'float')
try:
self._cf.log.add_config(
self._lg_stab) #cf is cf object created in main loop
# This callback will receive the data
self._lg_stab.data_received_cb.add_callback(
self._log_data
) # this says what to do when a new set of data is received
# This callback will be called on errors
self._lg_stab.error_cb.add_callback(self._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.')
print('bla')
t = Timer(10, self._disconnected)
t.start()
# Start a timer to disconnect in 5s
#t = Timer(5,print('Done logging')) #TIMER NEEDS A FUNCTION TO DO WHEN IT RUNS OUT OF TIME.
#t.start()
while self.is_connected:
time.sleep(1)
def wait_for_position_estimator(scf):
print('Waiting for estimator to find position...')
log_config = LogConfig(name='Kalman Variance', period_in_ms=500)
log_config.add_variable('kalman.varPX', 'float')
log_config.add_variable('kalman.varPY', 'float')
log_config.add_variable('kalman.varPZ', 'float')
var_y_history = [1000] * 10
var_x_history = [1000] * 10
var_z_history = [1000] * 10
threshold = 0.001
with SyncLogger(scf, log_config) as logger:
for log_entry in logger:
data = log_entry[1]
var_x_history.append(data['kalman.varPX'])
var_x_history.pop(0)
var_y_history.append(data['kalman.varPY'])
var_y_history.pop(0)
var_z_history.append(data['kalman.varPZ'])
var_z_history.pop(0)
min_x = min(var_x_history)
max_x = max(var_x_history)
min_y = min(var_y_history)
max_y = max(var_y_history)
min_z = min(var_z_history)
max_z = max(var_z_history)
# print("{} {} {}".
# format(max_x - min_x, max_y - min_y, max_z - min_z))
if (max_x - min_x) < threshold and (
max_y - min_y) < threshold and (max_z - min_z) < threshold:
break
def log_position(scf):
lg_pos = LogConfig(name='LoPoTab2', period_in_ms=10)
lg_pos.add_variable('kalman.stateX', 'float')
lg_pos.add_variable('kalman.stateY', 'float')
try:
with SyncLogger(scf, lg_pos) as logger:
first = next(logger)
timestamp = first[0]
data = first[1]
print('[%d]: %s' % (timestamp, data))
current_x = data['kalman.stateX']
current_y = data['kalman.stateY']
# check if drone out of bounds
# make rest call to node for out of bound drone
if (current_x < min_x or current_x > max_x or current_y < min_y
or current_y > max_y):
droneId = drone_URIs.index(scf.cf.link_uri)
scf.cf.commander.send_stop_setpoint()
getDeleteDrone(droneId)
time.sleep(0.1)
except Exception as e:
print("log position exception")
print(e)
def _getStabilizer(self):
# 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.')
def create_empty_log_conf(self, category):
""" Creates an empty log-configuration with a default name """
log_path = self._get_log_path(category)
conf_name = self._get_default_conf_name(log_path)
file_path = os.path.join(log_path, conf_name) + '.json'
if not os.path.exists(file_path):
with open(file_path, 'w') as f:
f.write(
json.dumps(
{
'logconfig': {
'logblock': {
'variables': [],
'name': conf_name,
'period': 100
}
}
},
indent=2))
self._log_configs[category].append(LogConfig(conf_name, 100))
return conf_name
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
cflib.crtp.init_drivers(enable_debug_driver=True)
cf = Crazyflie(rw_cache='./cache')
with SyncCrazyflie(URI, cf=cf) as scf:
with MotionCommander(scf) as motion_commander:
with Multiranger(scf) as multi_ranger:
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 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 _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._cf.param.add_update_callback(group='controller',
name='tiltComp',
cb=self._param_callback)
self._cf.param.add_update_callback(group='motorPowerSet',
name=None,
cb=self._param_callback)
self._lg_pwm = LogConfig(name='PWM', period_in_ms=50)
self._lg_pwm.add_variable('pwm.m1_pwm', 'float')
self._lg_pwm.add_variable('pwm.m2_pwm', 'float')
self._lg_pwm.add_variable('pwm.m3_pwm', 'float')
self._lg_pwm.add_variable('pwm.m4_pwm', 'float')
self._lg_pwm.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.
try:
self._cf.log.add_config(self._lg_pwm)
# This callback will receive the data
self._lg_pwm.data_received_cb.add_callback(self._pwm_log_data)
# This callback will be called on errors
self._lg_pwm.error_cb.add_callback(self._pwm_log_error)
# Start the logging and give timestamp
print(time.ctime())
self._lg_pwm.start()
except KeyError as e:
print('Could not start log configuration,'
'{} not found in TOC'.format(str(e)))
except AttributeError:
print('Could not add PWM log config, bad configuration.')
# Start a separate thread to do the motor test.
# Do not hijack the calling thread!
Thread(target=self._ramp_motors).start()
def log_ranging(link_uri,
log_cfg_name='TSranging',
log_var={},
log_save_path="./default.csv",
period_in_ms=100,
keep_time_in_s=5):
cflib.crtp.init_drivers(enable_debug_driver=False)
log_data = pd.DataFrame(columns=log_var.keys())
with SyncCrazyflie(link_uri=link_uri,
cf=Crazyflie(rw_cache="./cache")) as scf:
log_cfg = LogConfig(name=log_cfg_name, period_in_ms=period_in_ms)
for log_var_name, log_var_type in log_var.items():
log_cfg.add_variable(log_cfg.name + '.' + log_var_name,
log_var_type)
with SyncLogger(scf, log_cfg) as logger:
end_time = time.time() + keep_time_in_s
while time.time() < end_time:
continue
for i in range(logger._queue.qsize()):
log_entry = logger.next()
timestamp = log_entry[0]
data = log_entry[1]
logconf_name = log_entry[2]
temp = {}
temp['timestamp'] = timestamp
for log_var_name, log_var_type in log_var.items():
temp[log_var_name] = data[log_cfg.name + '.' +
log_var_name]
log_data = log_data.append(temp, ignore_index=True)
print(temp)
log_data.to_csv(log_save_path, index=False)
return log_data
def _wait_for_position_estimator(self):
"""Waits until the position estimator reports a consistent location after resetting."""
print('Waiting for estimator to find position...')
log_config = LogConfig(name='Kalman Variance', period_in_ms=500)
log_config.add_variable('kalman.varPX', 'float')
log_config.add_variable('kalman.varPY', 'float')
log_config.add_variable('kalman.varPZ', 'float')
var_y_history = [1000] * 10
var_x_history = [1000] * 10
var_z_history = [1000] * 10
threshold = 0.001
with SyncLogger(self.scf, log_config) as logger:
for log_entry in logger:
data = log_entry[1]
var_x_history.append(data['kalman.varPX'])
var_x_history.pop(0)
var_y_history.append(data['kalman.varPY'])
var_y_history.pop(0)
var_z_history.append(data['kalman.varPZ'])
var_z_history.pop(0)
min_x = min(var_x_history)
max_x = max(var_x_history)
min_y = min(var_y_history)
max_y = max(var_y_history)
min_z = min(var_z_history)
max_z = max(var_z_history)
if (max_x - min_x) < threshold and (
max_y - min_y) < threshold and (max_z -
min_z) < threshold:
break
def start_logging(self, scf):
#with open("ugly_log.txt","a") as file:
# file.write("timestamp,roll,pitch,yaw,height\n")
log_conf = LogConfig(name='logdata', period_in_ms=10)
#-- States
log_conf.add_variable('stateEstimate.x', 'float')
log_conf.add_variable('stateEstimate.y', 'float')
log_conf.add_variable('stateEstimate.z', 'float')
log_conf.add_variable('stabilizer.roll', 'float')
log_conf.add_variable('stabilizer.pitch', 'float')
log_conf.add_variable('stabilizer.yaw', 'float')
log_conf.add_variable('range.zrange', 'uint16_t')
#-- Battery voltage
#log_conf.add_variable('pm.vbat', 'float')
#-- Command
#log_conf.add_variable('posCtl.targetVX','float')
#log_conf.add_variable('posCtl.targetVY','float')
#log_conf.add_variable('posCtl.targetVZ','float')
#param_conf.add_variable('posCtl.VZp')
#param_conf.add_variable('posCtl.VZi')
#param_conf.add_variable('posCtl.VZd')
#logz_conf = LogConfig(name='logzdata', period_in_ms=25)
#logz_conf.add_variable('range.zrange','float')
scf.cf.log.add_config(log_conf)
log_conf.data_received_cb.add_callback(self.log_callback)
#logz_conf.data_received_cb.add_callback(self.logz_callback)
log_conf.start()
def __init__(self, cf):
""" Initialize and run the example with the specified link_uri """
self._cf = cf
self.data = {'t': [], 'motor': [], 'z': []}
self._lg_alt = LogConfig(name='Altitude', period_in_ms=10)
self._lg_alt.add_variable('motor.m1', 'float')
self._lg_alt.add_variable('motor.m2', 'float')
self._lg_alt.add_variable('motor.m3', 'float')
self._lg_alt.add_variable('motor.m4', 'float')
self._lg_alt.add_variable('stateEstimate.z', 'float')
try:
self._cf.log.add_config(self._lg_alt)
# This callback will receive the data
self._lg_alt.data_received_cb.add_callback(self._alt_log_data)
# This callback will be called on errors
self._lg_alt.error_cb.add_callback(self._alt_log_error)
# Start the logging
self._lg_alt.start()
except KeyError as e:
print('Could not start log configuration,'
'{} not found in TOC'.format(str(e)))
except AttributeError:
print('Could not add log config, bad configuration.')
def connected(self, uri):
print("Connected to Crazyflie at URI: %s" % uri)
self.cf_active = True
try:
self.log_data = LogConfig(name="Data", period_in_ms=10)
self.log_data.add_variable('acc.x', 'float')
self.log_data.add_variable('acc.y', 'float')
self.log_data.add_variable('acc.z', 'float')
self.log_data.add_variable('pm.vbat', 'float')
self.log_data.add_variable('stateEstimate.z', 'float')
self.cf.log.add_config(self.log_data)
self.log_data.data_received_cb.add_callback(self.received_data)
#begins logging and publishing
self.log_data.start()
print("Logging Setup Complete. Starting...")
except KeyError as e:
print('Could not start log configuration,'
'{} not found in TOC'.format(str(e)))
except AttributeError:
print('Could not add log config, bad configuration.')
def _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=100)
self._lg_stab.add_variable('stateEstimate.x', 'float')
self._lg_stab.add_variable('stateEstimate.y', 'float')
self._lg_stab.add_variable('stateEstimate.z', 'float')
self._lg_stab.add_variable('stabilizer.roll', 'float')
self._lg_stab.add_variable('stabilizer.pitch', 'float')
self._lg_stab.add_variable('stabilizer.yaw', 'float')
# The fetch-as argument can be set to FP16 to save space in the log packet
self._lg_stab.add_variable('pm.vbat', 'FP16')
# Adding the configuration cannot be done until a Crazyflie is
# connected, since we need to check that the variables we
# would like to log are in the TOC.
try:
self._cf.log.add_config(self._lg_stab)
# This callback will receive the data
self._lg_stab.data_received_cb.add_callback(self._stab_log_data)
# This callback will be called on errors
self._lg_stab.error_cb.add_callback(self._stab_log_error)
# Start the logging
self._lg_stab.start()
except KeyError as e:
print('Could not start log configuration,'
'{} not found in TOC'.format(str(e)))
except AttributeError:
print('Could not add Stabilizer log config, bad configuration.')
# Start a timer to disconnect in 10s
t = Timer(5, self._cf.close_link)
t.start()
def _connected(self, link_uri):
""" 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.connection.emit("progress")
# The definition of the logconfig can be made before connecting
self.logger = LogConfig("Battery", 1000) # delay
self.logger.add_variable("pm.vbat", "float")
try:
self._cf.log.add_config(self.logger)
self.logger.data_received_cb.add_callback(
lambda e, f, g: self.batteryValue.emit(float(f['pm.vbat'])))
# self.logger.error_cb.add_callback(lambda: print('error'))
self.logger.start()
except KeyError as e:
print(e)
self.connection.emit("on")
self.motion_commander = MotionCommander(self._cf, 0.5)
self.multiranger = Multiranger(self._cf, rate_ms=50)
self.multiranger.start()
def _set_available_sensors(self, name, available):
logger.info("[%s]: %s", name, available)
available = eval(available)
self._enable_estimators(True)
self.helper.inputDeviceReader.set_alt_hold_available(available)
if not self.logBaro:
# The sensor is available, set up the logging
self.logBaro = LogConfig("Baro", 200)
self.logBaro.add_variable(LOG_NAME_ESTIMATE_X, "float")
self.logBaro.add_variable(LOG_NAME_ESTIMATE_Y, "float")
self.logBaro.add_variable(LOG_NAME_ESTIMATE_Z, "float")
try:
self.helper.cf.log.add_config(self.logBaro)
self.logBaro.data_received_cb.add_callback(
self._baro_data_signal.emit)
self.logBaro.error_cb.add_callback(self._log_error_signal.emit)
self.logBaro.start()
except KeyError as e:
logger.warning(str(e))
except AttributeError as e:
logger.warning(str(e))
def check_battery(scf, name):
"""
Checks the battery level. If the battery is too low, it prints an error
and returns false, otherwise it returns true.
Too low is considered to be 3.4 volts
:param scf: SyncCrazyflie object
:param name: The "Name" for logging
"""
print("Checking battery")
log_config = LogConfig(name='Battery', period_in_ms=500)
log_config.add_variable('pm.vbat', 'float')
with SyncLogger(scf, log_config) as logger:
for log_entry in logger:
print(name + ": battery at " + str(log_entry[1]['pm.vbat']))
if log_entry[1]['pm.vbat'] > 3.4:
print(name + ": Battery at good level")
return True
else:
print(name + ": Battery too low")
return False
def _connected(self, link_uri):
""" 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=1000
) #self.lg_stab is a LogConfig object. See log.py for details.
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', 'float')
print(time.strftime("%d/%m/%Y"))
# 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 says what to do when a new set of data is received
# 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 5s
t = Timer(5, self._cf.close_link)
t.start()
def _connected(self, link_uri):
"""Callback when the Crazyflie has been connected"""
logger.debug("Crazyflie connected to {}".format(link_uri))
# self.pub = rospy.Publisher('chatter', String, queue_size=10)
# rospy.init_node('talker', anonymous=True, disable_signals=False)
# # rate = rospy.Rate(10)
lg = LogConfig("Stabilizer", Config().get("ui_update_period"))
lg.add_variable("stabilizer.roll", "float")
# lg.add_variable("stabilizer.pitch", "float")
# lg.add_variable("stabilizer.yaw", "float")
# lg.add_variable("stabilizer.thrust", "uint16_t")
try:
self.helper.cf.log.add_config(lg)
lg.data_received_cb.add_callback(self._log_data_signal.emit)
lg.error_cb.add_callback(self._log_error_signal.emit)
lg.start()
except KeyError as e:
logger.warning(str(e))
except AttributeError as e:
logger.warning(str(e))
def _connected(self, uri):
print(f"Connected to {uri}")
self.is_connected = True
self._lg_stateEstimate = LogConfig(name="StateEstimation", period_in_ms=10)
self._lg_stateEstimate.add_variable('stateEstimate.x', 'float')
self._lg_stateEstimate.add_variable('stateEstimate.y', 'float')
self._lg_stateEstimate.add_variable('stateEstimate.z', 'float')
self._lg_stateEstimate.add_variable('stateEstimate.vx', 'float')
self._lg_stateEstimate.add_variable('stateEstimate.vy', 'float')
self._lg_stateEstimate.add_variable('stateEstimate.vz', 'float')
try:
self._cf.log.add_config(self._lg_stateEstimate)
self._lg_stateEstimate.data_received_cb.add_callback(self.dataReceived_callback)
self._lg_stateEstimate.error_cb.add_callback(self.error_callback)
self._lg_stateEstimate.start()
except KeyError as e:
print(f"Could not start log config, {e} not found in TOC")
except AttributeError:
print('Could not add Stabilizer log config, bad configuration.')
logging.basicConfig(level=logging.ERROR)
# Initialize the low-level drivers (don't list the debug drivers)
cflib.crtp.init_drivers(enable_debug_driver=False)
# Scan for Crazyflies and use the first one found
print('Scanning interfaces for Crazyflies...')
available = cflib.crtp.scan_interfaces()
print('Crazyflies found:')
for i in available:
print(i[0])
if len(available) == 0:
print(err_dict["2"])
else:
cf = Crazyflie(rw_cache='./cache')
# Add logging config to logger
lg_stab = LogConfig(name='Stabilizer', period_in_ms=12)
lg_stab.add_variable('pm.vbat', 'float')
lg_stab.add_variable('pm.batteryLevel', 'float')
lg_stab.add_variable('pwm.m1_pwm', 'float')
lg_stab.add_variable('baro.temp', 'float')
with SyncCrazyflie(URI) as scf:
# Define motion commander to crazyflie as mc:
cf2 = CF(scf, available)
cf2_psi = 0 # Get current yaw-angle of cf
cf2_bat = cf2.vbat # Get current battery level of cf
cf2_blevel = cf2.blevel
cf2_pwm = cf2.m1_pwm
cf2_temp = cf2.temp
cf2_phi = 0
cf2_theta = 0
marker_psi = 0 # Set marker angle to zero initially
streamingClient.rigidBodyListener = receiveRigidBodyFrame
streamingClient.run()
URI = 'radio://0/80/250K'
# Only output errors from the logging framework
logging.basicConfig(level=logging.ERROR)
if __name__ == '__main__':
# Initialize the low-level drivers (don't list the debug drivers)
cflib.crtp.init_drivers(enable_debug_driver=False)
cf = Crazyflie(rw_cache='./cache')
lg_states = LogConfig(name='kalman_states', period_in_ms=10)
lg_states.add_variable('kalman_states.ox')
lg_states.add_variable('kalman_states.oy')
with SyncCrazyflie(URI, cf=cf) as scf:
with SyncLogger(scf, lg_states) as logger_states:
with MotionCommander(scf) as motion_commander:
keep_flying = True
char = readchar.readchar()
motion_commander.up(0.5)
time.sleep(1)
while (keep_flying):
if len(pos) > 0:
cf.extpos.send_extpos(pos[0], pos[1], pos[2])
#print("external",pos[1])
def _connected(self, link_uri):
""" This callback is called from the Crazyflie API when a Crazyflie
has been connected adn TOCs have been downloaded """
print('connected to: %s' % link_uri)
# Open text file for recording data
self.datalog_Pos = open("log_data/Cf2log_Pos", "w")
#self.datalog_Vel = open("log_data/Cf2log_Vel","w")
#self.datalog_Att = open("log_data/Cf2log_Att","w")
self.datalog_Att = open("log_data/Cf2log_Mtr", "w")
# Reset Kalman filter
self._cf.param.set_value('kalman.resetEstimation', '1')
time.sleep(0.1)
self._cf.param.set_value('kalman.resetEstimation', '0')
time.sleep(2)
print('Wait for Kalamn filter')
# Feedback position estimated by KF
logPos = LogConfig(name='Kalman Position',
period_in_ms=self.statefb_rate)
logPos.add_variable('kalman.stateX', 'float')
logPos.add_variable('kalman.stateY', 'float')
logPos.add_variable('kalman.stateZ', 'float')
# Feedback velocity estimated by KF
logVel = LogConfig(name='Kalman Velocity',
period_in_ms=self.statefb_rate)
logVel.add_variable('kalman.statePX', 'float')
logVel.add_variable('kalman.statePY', 'float')
logVel.add_variable('kalman.statePZ', 'float')
# Feedback Quaternion attitude estimated by KF
logAtt = LogConfig(name='Kalman Attitude',
period_in_ms=self.statefb_rate)
logAtt.add_variable('kalman.q0', 'float')
logAtt.add_variable('kalman.q1', 'float')
logAtt.add_variable('kalman.q2', 'float')
logAtt.add_variable('kalman.q3', 'float')
# Feedback Multi ranger
logMultiRa = LogConfig(name='Range', period_in_ms=self.statefb_rate)
logMultiRa.add_variable('range.front', 'uint16_t')
logMultiRa.add_variable('range.back', 'uint16_t')
logMultiRa.add_variable('range.left', 'uint16_t')
logMultiRa.add_variable('range.right', 'uint16_t')
logMultiRa.add_variable('range.up', 'uint16_t')
# Invoke logged states
self._cf.log.add_config(logPos)
self._cf.log.add_config(logVel)
self._cf.log.add_config(logAtt)
self._cf.log.add_config(logMultiRa)
# Start invoking logged states
if logPos.valid and logVel.valid:
# Position
logPos.data_received_cb.add_callback(self.log_pos_callback)
logPos.error_cb.add_callback(logging_error)
logPos.start()
# Velocity
logVel.data_received_cb.add_callback(self.log_vel_callback)
logVel.error_cb.add_callback(logging_error)
logVel.start()
# Quadternion attitude
logAtt.data_received_cb.add_callback(self.log_att_callback)
logAtt.error_cb.add_callback(logging_error)
logAtt.start()
# Multi-Ranger
logMultiRa.data_received_cb.add_callback(self.log_mtr_callback)
logMultiRa.error_cb.add_callback(logging_error)
logMultiRa.start()
else:
print(
"One or more of the variables in the configuration was not found"
+ "in log TOC. No logging will be possible")
# Start in a thread
threading.Thread(target=self._run_controller).start()
time.sleep(0.1)
if __name__ == '__main__':
# # initializing the queue and event object
q = queue.Queue(maxsize=0)
e1 = threading.Event()
e2 = threading.Event()
# # initializing Crazyflie
cflib.crtp.init_drivers(enable_debug_driver=False)
with SyncCrazyflie(uri_3, cf=Crazyflie(rw_cache='./cache')) as scf_3:
logconf_3 = LogConfig(name='Position', period_in_ms=500)
logconf_3.add_variable('kalman.stateX', 'float')
logconf_3.add_variable('kalman.stateY', 'float')
logconf_3.add_variable('kalman.stateZ', 'float')
scf_3.cf.log.add_config(logconf_3)
logconf_3.data_received_cb.add_callback(
lambda timestamp, data, logconf_3: log_pos_callback_2(
uri_3, timestamp, data, logconf_3))
with SyncCrazyflie(uri_2, cf=Crazyflie(rw_cache='./cache')) as scf_2:
logconf_2 = LogConfig(name='Position', period_in_ms=500)
logconf_2.add_variable('kalman.stateX', 'float')
logconf_2.add_variable('kalman.stateY', 'float')
logconf_2.add_variable('kalman.stateZ', 'float')
scf_2.cf.log.add_config(logconf_2)
logconf_2.data_received_cb.add_callback(
def run_sequence(scf, params):
cf = scf.cf
base = params['base']
z = params['h']
#sets variables for liftoff, beginning, and end heights
end = 0.3
base = params['base']
h = params['h']
num = params['num']
# lifts drones off and sends to beginning heights
poshold(cf, 2, base)
poshold(cf, 5, h)
# The definition of the logconfig can be made before connecting
log_config = LogConfig(name='Height', period_in_ms=50)
log_config.add_variable('stateEstimate.z', 'float')
with SyncLogger(scf, log_config) as logger:
#sets the runtime
endTime = time.time() + 45
j = 0
time_elapsed = float(0)
for log_entry in logger:
timestamp = log_entry[0]
data = log_entry[1]
currentPos[num] = data['stateEstimate.z']
#first runthrough sets begin time of drone, and holds it in
#current position for another second
if j == 0:
begin_T = timestamp
j = 1
poshold(cf, 1, h)
print(0)
#concurrent runthroughs run the consensus algorithm every
#three seconds and sends the desired position every second
else:
time_elapsed = float((timestamp - begin_T)) / 1000
#every three seconds runs consensus alg
if time_elapsed.is_integer() and (int(time_elapsed) % 3) == 0:
#ensures all three drones have sent their positions before continuuing
while currentPos[0] == 0 or currentPos[
1] == 0 or currentPos[2] == 0 or currentPos[
3] == 0 or currentPos[4] == 0:
time.sleep(0.001)
#runs consensus algorithm only on one of the 3 parallel programs running
if num == 0:
print('running consensus')
consensus(currentPos)
#(nextPos)
#waits till the algorithm runs to continue
while (nextPos[0] == 0 or nextPos[1] == 0
or nextPos[2] == 0 or nextPos[3] == 0
or nextPos[4] == 0):
time.sleep(0.001)
#sends the update positions to the drones
poshold(cf, 1, nextPos[num])
#print(nextPos)
#sends position for drones every second
elif time_elapsed.is_integer():
if time_elapsed < 3:
poshold(cf, 1, h)
else:
poshold(cf, 1, nextPos[num])
#if time_elapsed.is_integer():
#print(time_elapsed)
#print(currentPos)
#appends the time and height data to a log
savelog[num].append([time_elapsed, currentPos[num]])
#ends program after endTime seconds
if time.time() > endTime:
break
#poshold(cf,2, end)
# # Base altitude in meters
#print(currentPos)
#sends drones to base height
poshold(cf, 2, base)
#saves the data for future use
str1 = 'testing'
str2 = '.csv'
with open(str1 + str(num) + str2, 'w') as f:
writer = csv.writer(f, delimiter=',')
writer.writerows(savelog[num])
cf.commander.send_stop_setpoint()
def crazyFlieloop(self):
#wall_follower = WallFollowerController()
#wall_follower.init(0.5)
twist = Twist()
#log_config.data_received_cb.add_callback(self.data_received)
cflib.crtp.init_drivers(enable_debug_driver=False)
cf = Crazyflie(rw_cache='./cache')
lg_states = LogConfig(name='kalman_states', period_in_ms=100)
lg_states.add_variable('stabilizer.yaw')
lg_states.add_variable('kalman_states.ox')
lg_states.add_variable('kalman_states.oy')
lg_states.add_variable('updown_laser.back_range')
lg_states.add_variable('updown_laser.front_range')
lg_states.add_variable('updown_laser.left_range')
lg_states.add_variable('updown_laser.right_range')
lg_states.data_received_cb.add_callback(self.data_received)
''' lg_states.add_variable('rssiCR.rssi')
lg_states.add_variable('rssiCR.distance')
lg_states.add_variable('rssiCR.pos_x')
lg_states.add_variable('rssiCR.pos_y')'''
fh = open("log_test.txt", "w")
with SyncCrazyflie(self.URI, cf=cf) as scf:
with MotionCommander(scf,0.3) as motion_commander:
with MultiRanger(scf) as multi_ranger:
with Stabilization(scf) as stabilization:
with SyncLogger(scf, lg_states) as logger_states:
bug_controller = WallFollowerController()
bug_controller.init(0.7,0.5)
keep_flying = True
time.sleep(1)
# param_name = "rssiCR.start"
# param_value = "1"
# cf.param.set_value(param_name, param_value)
twist.linear.x = 0.2
twist.linear.y = 0.0
twist.angular.z = 0
heading_prev = 0.0
heading = 0.0
angle_to_goal = 0.0;
kalman_x = 0.0
kalman_y = 0.0
already_reached_far_enough = False
state = "TURN_TO_GOAL"
prev_heading = stabilization.heading
angle_outbound = stabilization.heading;
state_start_time =0
rssi = 0
distance = 0
first_run = True
x_0 = 0
y_0 = 0
state_WF="FORWARD"
while keep_flying:
# crazyflie related stuff
for log_entry_1 in logger_states:
data = log_entry_1[1]
heading = math.radians(float(data["stabilizer.yaw"]));
pos_x =float(data["kalman_states.ox"])-0.5
pos_y =float(data["kalman_states.oy"])-1.5
kalman_x = float(data["kalman_states.ox"])
kalman_y = float(data["kalman_states.oy"])
#rssi = float(data["rssiCR.rssi"])
#distance = float(data["rssiCR.distance"])
if first_run is True:
x_0 = kalman_x
y_0 = kalman_y
first_run = False
if pos_x == 0:
pos_x = 0.02
if pos_y == 0:
pos_y = 0.02
rel_coord_x = self.goal_coord_x[self.coord_index]-(kalman_x-x_0);
rel_coord_y = self.goal_coord_y[self.coord_index]-(kalman_y-y_0);
self.distance_to_goal = math.sqrt(math.pow(rel_coord_x,2)+math.pow(rel_coord_y,2))
self.angle_to_goal = wraptopi(np.arctan2(rel_coord_y,rel_coord_x))
break
time.sleep(0.1)
#Handle state transitions
if state =="STATE_MACHINE":
if self.distance_to_goal<1.0 and self.coord_index is not len(self.goal_coord_x)-1:
state = self.transition("TURN_TO_GOAL")
self.coord_index = self.coord_index + 1
self.distance_to_goal = 10
self.angle_to_goal = 10
if self.distance_to_goal<1.0 and self.coord_index is len(self.goal_coord_x)-1:
state = self.transition("STOP")
if state =="TURN_TO_GOAL":
if time.time()-self.state_start_time > 1 and logicIsCloseTo(stabilization.heading,wraptopi(self.angle_to_goal),0.1):
state = self.transition("STATE_MACHINE")
#Handle state actions
if state=="STATE_MACHINE":
#twist = bug_controller.stateMachine(multi_ranger.front,multi_ranger.right,multi_ranger.left,stabilization.heading,wraptopi(self.angle_to_goal),self.distance_to_goal)
print("front_range",self.front_range)
twist,state_WF = bug_controller.stateMachine(self.front_range,self.left_range,self.right_range,stabilization.heading,wraptopi(self.angle_to_goal),self.distance_to_goal)
twist.linear.x = 0.5;
twist.linear.y = 0.0;
circumference = 2 * 0.5 * math.pi
rate = 2*math.pi * 0.5 / circumference
twist.angular.z = rate;
if state =="TURN_TO_GOAL":
twist.linear.x = 0.0;
twist.linear.y = 0.0;
twist.angular.z = 0.6;
if state =="STOP":
twist.linear.x = 0.0;
twist.linear.y = 0.0;
twist.angular.z = 0.0;
keep_flying = False
if self.front_range == None:
self.front_range = 8
if self.back_range == None:
self.back_range = 8
if self.left_range == None:
self.left_range = 8
if self.right_range == None:
self.right_range = 8
fh.write("%f, %f, %f, %f, %s\n"% (self.front_range,self.back_range,self.left_range,self.right_range,state_WF))
''' if len(pos)>0:
fh.write("%f, %f, %f, %f, %f, %f, %f, %f, %f, %f, %f, %f, %f\n"% (twist.linear.x, -1*math.degrees(twist.angular.z), self.distance_to_goal,self.angle_to_goal, stabilization.heading, kalman_x, kalman_y, pos_x, pos_y,pos[0],pos[2],rssi,distance))
else:
fh.write("%f, %f, %f, %f, %f, %f, %f, %f, %f, 0, 0, %f, %f\n"% (twist.linear.x, -1*math.degrees(twist.angular.z), self.distance_to_goal,self.angle_to_goal, stabilization.heading, kalman_x, kalman_y, pos_x, pos_y,rssi,distance))'''
print(state)
motion_commander._set_vel_setpoint(twist.linear.x,twist.linear.y,0,-1*math.degrees(twist.angular.z))
'''
if self.back_range is not None :
if self.back_range < 0.2:
print("up range is activated")
keep_flying = False'''
motion_commander.stop()
print("demo terminated")
fh.close()
def main():
# Initialize the low-level drivers (don't list the debug drivers)
cflib.crtp.init_drivers(enable_debug_driver=False)
# Scan for Crazyflies and use the first one found
print('Scanning interfaces for Crazyflies...')
available = cflib.crtp.scan_interfaces()
print('Crazyflies found:')
for i in available:
print(i[0])
if len(available) == 0:
print('No Crazyflies found, cannot run example')
else:
#create log config
if (small_log):
logConfig_stab = LogConfig(name='Stabilizer',
period_in_ms=log_period)
logConfig_stab.add_variable('stabilizer.roll', 'float')
logConfig_stab.add_variable('stabilizer.pitch', 'float')
logConfig_stab.add_variable('stabilizer.yaw', 'float')
logConfig_stab.add_variable('stabilizer.thrust', 'float')
logConfig_state = LogConfig(name='State', period_in_ms=log_period)
logConfig_state.add_variable('stateEstimate.x', 'float')
logConfig_state.add_variable('stateEstimate.y', 'float')
logConfig_state.add_variable('stateEstimate.z', 'float')
"""
logConfig_target = LogConfig(name='ControlTarget', period_in_ms=log_period)
logConfig_target.add_variable('ctrltarget.roll', 'float')
logConfig_target.add_variable('ctrltarget.pitch', 'float')
logConfig_target.add_variable('ctrltarget.yaw', 'float')
"""
logConfig_mpow = LogConfig(name='MotorPower',
period_in_ms=log_period)
logConfig_mpow.add_variable('motor.m1', 'int32_t')
logConfig_mpow.add_variable('motor.m2', 'int32_t')
logConfig_mpow.add_variable('motor.m3', 'int32_t')
logConfig_mpow.add_variable('motor.m4', 'int32_t')
if (big_log):
logConfig_all = LogConfig(name='all', period_in_ms=log_period)
logConfig_all.add_variable('stabilizer.thrust', 'float')
logConfig_all.add_variable('motor.m1', 'int32_t')
logConfig_all.add_variable('motor.m2', 'int32_t')
logConfig_all.add_variable('motor.m3', 'int32_t')
logConfig_all.add_variable('motor.m4', 'int32_t')
with SyncCrazyflie(URI) as scf:
cf = scf.cf
#create logger
if (small_log):
logger_stab = SyncLogger(scf, logConfig_stab)
logger_state = SyncLogger(scf, logConfig_state)
logger_mpow = SyncLogger(scf, logConfig_mpow)
logger_stab.connect()
logger_state.connect()
logger_mpow.connect()
#logger_target = SyncLogger(scf, logConfig_target)
#logger_target.connect()
if (big_log):
logger_all = SyncLogger(scf, logConfig_all)
logger_all.connect()
cf.param.set_value('kalman.resetEstimation', '1')
time.sleep(0.1)
cf.param.set_value('kalman.resetEstimation', '0')
time.sleep(2)
for y in range(10):
cf.commander.send_hover_setpoint(0, 0, 0, y / 20)
time.sleep(0.1)
#start point logging
if (small_log):
log_start = logger_stab.qsize()
else:
log_start = logger_all.qsize()
for i in range(50):
print_message("Going " + str(i))
cf.commander.send_hover_setpoint(0, 0, 0, 0.3)
time.sleep(0.1)
#end point logging
if (small_log):
log_end = logger_stab.qsize()
else:
log_end = logger_all.qsize()
log_count = log_end - log_start
print_message("Start:{}, end:{}".format(log_start, log_end))
print_message("To log {} lines.".format(log_end - log_start + 1))
for _ in range(10):
cf.commander.send_hover_setpoint(0, 0, 0, 0.2)
time.sleep(0.1)
cf.commander.send_hover_setpoint(0, 0, 0, 0.1)
time.sleep(0.2)
cf.commander.send_stop_setpoint() #land
time.sleep(1.5)
print_message("Landed, start logging...")
#write log files
if (small_log):
with open(log_file_name(logConfig_stab), "w") as f:
log_count = write_out_log(logger_stab, f, log_start,
log_end)
print_message("logged stabilizer")
with open(log_file_name(logConfig_state), "w") as f:
log_count = write_out_log(logger_state, f, log_start,
log_end)
print_message("logged state")
"""
with open(log_file_name(logConfig_target), "w") as f:
log_count = write_out_log(logger_target, f, log_start, log_end)
print_message("logged control target")
"""
with open(log_file_name(logConfig_mpow), "w") as f:
log_count = write_out_log(logger_mpow, f, log_start,
log_end)
print_message("logged motor power")
logger_stab.disconnect()
logger_state.disconnect()
logger_mpow.disconnect()
#logger_target.disconnect()
if (big_log):
with open(log_file_name(logConfig_all), "w") as f:
log_count = write_out_log(logger_all, f, log_start,
log_end)
logger_all.disconnect()
print_message("Done logging")
if __name__ == '__main__':
cflib.crtp.init_drivers(enable_debug_driver=False)
# Set these to the position and yaw based on how your Crazyflie is placed
# on the floor
initial_x = 0.0
initial_y = 0.0
initial_z = 0.0
initial_yaw = 0 # In degrees
# 0: positive X direction
# 90: positive Y direction
# 180: negative X direction
# 270: negative Y direction
# define logging parameters
lg_posEst = LogConfig(name='stateEstimate', period_in_ms=20)
lg_posEst.add_variable('stateEstimate.x', 'float')
lg_posEst.add_variable('stateEstimate.y', 'float')
lg_posEst.add_variable('stateEstimate.z', 'float')
lg_posEst.add_variable('stateEstimate.roll', 'float')
lg_posEst.add_variable('stateEstimate.pitch', 'float')
lg_posEst.add_variable('stateEstimate.yaw', 'float')
# lg_posSet = LogConfig(name='posCtl', period_in_ms=20)
# lg_posSet.add_variable('posCtl.targetX', 'float')
# lg_posSet.add_variable('posCtl.targetY', 'float')
# lg_posSet.add_variable('posCtl.targetZ', 'float')
lg_pm = LogConfig(name='pm', period_in_ms=20)
lg_pm.add_variable('pm.vbat', 'float')
def main():
# Initialize the low-level drivers (don't list the debug drivers)
cflib.crtp.init_drivers(enable_debug_driver=False)
# Scan for Crazyflies and use the first one found
print('Scanning interfaces for Crazyflies...')
available = cflib.crtp.scan_interfaces()
print('Crazyflies found:')
for i in available:
print(i[0])
if len(available) == 0:
print('No Crazyflies found, cannot run example')
else:
if (small_log):
logConfig_stab = LogConfig(name='AttitudeBeforePosCtrl',
period_in_ms=log_intervall)
logConfig_stab.add_variable('bposatt.roll', 'float')
logConfig_stab.add_variable('bposatt.pitch', 'float')
logConfig_stab.add_variable('bposatt.yaw', 'float')
logConfig_stab.add_variable('bposatt.actuatorThrust', 'float')
logConfig_state = LogConfig(name='AttitudeAfterPosCtrl',
period_in_ms=log_intervall)
logConfig_state.add_variable('aposatt.roll', 'float')
logConfig_state.add_variable('aposatt.pitch', 'float')
logConfig_state.add_variable('aposatt.yaw', 'float')
logConfig_state.add_variable('aposatt.actuatorThrust', 'float')
logConfig_all = LogConfig(name='PositionLog',
period_in_ms=log_intervall)
logConfig_all.add_variable('stateEstimate.x', 'float')
logConfig_all.add_variable('stateEstimate.y', 'float')
logConfig_all.add_variable('stateEstimate.z', 'float')
with SyncCrazyflie(URI) as scf:
cf = scf.cf
if (small_log):
logger_stab = SyncLogger(scf, logConfig_stab)
logger_state = SyncLogger(scf, logConfig_state)
logger_stab.connect()
logger_state.connect()
logger_all = SyncLogger(scf, logConfig_all)
logger_all.connect()
cf.param.set_value('kalman.resetEstimation', '1')
time.sleep(0.1)
cf.param.set_value('kalman.resetEstimation', '0')
time.sleep(2)
for y in range(maneuver_iterations):
cf.commander.send_hover_setpoint(0, 0, 0, y * maneuver_step)
time.sleep(time_step)
for i in range(balance_iterations):
print_message("Balancing out " + str(i))
cf.commander.send_hover_setpoint(0, 0, 0, height)
time.sleep(time_step)
log_start = logger_all._queue.qsize()
for i in range(log_iterations):
print_message("Logging " + str(i))
cf.commander.send_hover_setpoint(0, 0, 0, height)
time.sleep(time_step)
log_end = logger_all._queue.qsize()
log_count = log_end - log_start
print_message("Start:{}, end:{}".format(log_start, log_end))
print_message("To log {} lines.".format(log_end - log_start + 1))
for y in range(maneuver_iterations - 1):
cf.commander.send_hover_setpoint(0, 0, 0,
height - (y * maneuver_step))
time.sleep(time_step)
cf.commander.send_hover_setpoint(0, 0, 0, 0.1)
time.sleep(time_step)
time.sleep(time_step)
cf.commander.send_stop_setpoint() #land
time.sleep(1.5)
print_message("Landed, writing out log...")
if (small_log):
with open(log_file_name(logConfig_stab), "w") as f:
log_count = write_out_log(logger_stab, f, log_start,
log_end)
print_message("logged stabilizer")
with open(log_file_name(logConfig_state), "w") as f:
log_count = write_out_log(logger_state, f, log_start,
log_end)
print_message("logged state")
logger_stab.disconnect()
logger_state.disconnect()
with open(log_file_name(logConfig_all), "w") as f:
log_count = write_out_log(logger_all, f, log_start, log_end)
logger_all.disconnect()
print_message("Done logging.")
from cflib.crazyflie import Crazyflie
from cflib.crazyflie.log import LogConfig
from cflib.crazyflie.syncCrazyflie import SyncCrazyflie
from cflib.crazyflie.syncLogger import SyncLogger
URI = 'radio://0/40/2M/E7E7E7E7E7'
def position_callback(timestamp, data, logconf):
print(data)
if __name__ == '__main__':
# Initialize the low-level drivers (don't list the debug drivers)
cflib.crtp.init_drivers(enable_debug_driver=False)
log_conf = LogConfig(name='Position', period_in_ms=500)
log_conf.add_variable('ranging.distance2', 'float')
#log_conf.add_variable('kalman.stateX', 'float')
#log_conf.add_variable('kalman.stateZ', 'float')
#log_conf.add_variable('stabilizer.roll', 'float')
#log_conf.add_variable('stabilizer.pitch', 'float')
#log_conf.add_variable('stabilizer.yaw', 'float')
with SyncCrazyflie(URI, cf=Crazyflie(rw_cache='./cache')) as scf:
scf.cf.log.add_config(log_conf)
log_conf.data_received_cb.add_callback(position_callback)
log_conf.start()
for i in range(60):
time.sleep(1)
