I was able to get location of each Crazyflie thanks to the forums here. I would like to take this a step further and calculate the distance between the the crazyflie (and hopefully more). While I know the formula to calculate the distance between the two drones, I'm unsure how to implement it.
Code: Select all
# -*- coding: utf-8 -*-
#
# || ____ _ __
# +------+ / __ )(_) /_______________ _____ ___
# | 0xBC | / __ / / __/ ___/ ___/ __ `/_ / / _ \
# +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/
# || || /_____/_/\__/\___/_/ \__,_/ /___/\___/
#
# Copyright (C) 2017-2018 Bitcraze AB
#
# Crazyflie Nano Quadcopter Client
#
# This program is free software; you can redistribute it and/or
# modify it under the terms of the GNU General Public License
# as published by the Free Software Foundation; either version 2
# of the License, or (at your option) any later version.
#
# This program is distributed in the hope that it will be useful,
# but WITHOUT ANY WARRANTY; without even the implied warranty of
# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
# GNU General Public License for more details.
# You should have received a copy of the GNU General Public License
# along with this program; if not, write to the Free Software
# Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston,
# MA 02110-1301, USA.
"""
Version of the AutonomousSequence.py example connecting to 10 Crazyflies.
The Crazyflies go straight up, hover a while and land but the code is fairly
generic and each Crazyflie has its own sequence of setpoints that it files
to.
The layout of the positions:
x2 x1 x0
y3 10 4
^ Y
|
y2 9 6 3
|
+------> X
y1 8 5 2
y0 7 1
"""
import time
import math
import cflib.crtp
from cflib.crazyflie import Crazyflie
from cflib.crazyflie.log import LogConfig
from cflib.crazyflie.swarm import CachedCfFactory
from cflib.crazyflie.swarm import Swarm
from cflib.crazyflie.syncLogger import SyncLogger
from cflib.crazyflie.syncCrazyflie import SyncCrazyflie
# Import MotionCommander
from cflib.positioning.motion_commander import MotionCommander
# Bring in CSV
import csv
# Change uris and sequences according to your setup
URI1 = 'radio://0/40/1M'
URI2 = 'radio://0/80/2M'
URI3 = 'radio://0/70/2M/E7E7E7E703'
URI4 = 'radio://0/70/2M/E7E7E7E704'
URI5 = 'radio://0/70/2M/E7E7E7E705'
URI6 = 'radio://0/70/2M/E7E7E7E706'
URI7 = 'radio://0/70/2M/E7E7E7E707'
URI8 = 'radio://0/70/2M/E7E7E7E708'
URI9 = 'radio://0/70/2M/E7E7E7E709'
URI10 = 'radio://0/70/2M/E7E7E7E70A'
z0 = 0.4
z = 1.0
x0 = 0.7
x1 = 0
x2 = -0.7
y0 = -1.0
y1 = -0.4
y2 = 0.4
y3 = 1.0
# x y z time
sequence1 = [
(0.3, 0.3, 0.5, 3.0),
(0.3, 0.3, 0.5, 5.0),
(0.3, 0.3, 0.5, 3.0),
]
sequence2 = [
(0.8, 0.8, 0.5, 3.0),
(0.8, 0.8, 0.5, 5.0),
(0.8, 0.8, 0.5, 3.0),
]
sequence3 = [
(x0, y2, z0, 3.0),
(x0, y2, z, 30.0),
(x0, y2, z0, 3.0),
]
sequence4 = [
(x0, y3, z0, 3.0),
(x0, y3, z, 30.0),
(x0, y3, z0, 3.0),
]
sequence5 = [
(x1, y1, z0, 3.0),
(x1, y1, z, 30.0),
(x1, y1, z0, 3.0),
]
sequence6 = [
(x1, y2, z0, 3.0),
(x1, y2, z, 30.0),
(x1, y2, z0, 3.0),
]
sequence7 = [
(x2, y0, z0, 3.0),
(x2, y0, z, 30.0),
(x2, y0, z0, 3.0),
]
sequence8 = [
(x2, y1, z0, 3.0),
(x2, y1, z, 30.0),
(x2, y1, z0, 3.0),
]
sequence9 = [
(x2, y2, z0, 3.0),
(x2, y2, z, 30.0),
(x2, y2, z0, 3.0),
]
sequence10 = [
(x2, y3, z0, 3.0),
(x2, y3, z, 30.0),
(x2, y3, z0, 3.0),
]
seq_args = {
URI1: [sequence1],
URI2: [sequence2],
URI3: [sequence3],
URI4: [sequence4],
URI5: [sequence5],
URI6: [sequence6],
URI7: [sequence7],
URI8: [sequence8],
URI9: [sequence9],
URI10: [sequence10],
}
# List of URIs, comment the one you do not want to fly
uris = {
URI1,
URI2,
# URI3,
# URI4,
# URI5,
# URI6,
# URI7,
# URI8,
# URI9,
# URI10
}
def wait_for_position_estimator(scf):
print('Waiting for estimator to find position...')
log_config = LogConfig(name='Stabilizer', period_in_ms=100)
log_config.add_variable('stabilizer.roll', 'float')
log_config.add_variable('stabilizer.pitch', 'float')
log_config.add_variable('stabilizer.yaw', '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:
endTime = time.time() + 10
for log_entry in logger:
data = log_entry[1]
var_x_history.append(data['kalman.varPX'])
var_x_history.pop(0)
var_y_history.append(data['kalman.varPY'])
var_y_history.pop(0)
var_z_history.append(data['kalman.varPZ'])
var_z_history.pop(0)
min_x = min(var_x_history)
max_x = max(var_x_history)
min_y = min(var_y_history)
max_y = max(var_y_history)
min_z = min(var_z_history)
max_z = max(var_z_history)
print("{} {} {}".
format(max_x - min_x, max_y - min_y, max_z - min_z))
if (max_x - min_x) < threshold and (
max_y - min_y) < threshold and (
max_z - min_z) < threshold:
break
def wait_for_param_download(scf):
while not scf.cf.param.is_updated:
time.sleep(1.0)
print('Parameters downloaded for', scf.cf.link_uri)
def reset_estimator(scf):
cf = scf.cf
cf.param.set_value('kalman.resetEstimation', '1')
time.sleep(0.1)
cf.param.set_value('kalman.resetEstimation', '0')
wait_for_position_estimator(cf)
def take_off(cf, position):
take_off_time = 1.0
sleep_time = 0.1
steps = int(take_off_time / sleep_time)
vz = position[2] / take_off_time
print(vz)
for i in range(steps):
cf.commander.send_velocity_world_setpoint(0, 0, vz, 0)
time.sleep(sleep_time)
def land(cf, position):
landing_time = 1.0
sleep_time = 0.1
steps = int(landing_time / sleep_time)
vz = -position[2] / landing_time
print(vz)
for _ in range(steps):
cf.commander.send_velocity_world_setpoint(0, 0, vz, 0)
time.sleep(sleep_time)
cf.commander.send_stop_setpoint()
# 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)
def run_sequence(scf, sequence):
time.sleep(20)
"""
# We take off when the commander is created
with MotionCommander(scf, default_height = 1.0) as mc:
with open('D:\DSO-AISwarm\InitialSampleData\ActionTest(1AgtforSUTD_test)(MovForward) - Copy.txt') as csv_file:
print('brought in forward')
csv_reader = csv.reader(csv_file, delimiter=' ')
line_count = 0
for row in csv_reader:
distance = float(row[0]) * 0.3
#print(row[0]+' ' + row[1])
mc.forward(0.2,distance)
line_count += 1
mc.stop
try:
cf = scf.cf
take_off(cf, sequence[0])
for position in sequence:
print('Setting position {}'.format(position))
end_time = time.time() + position[3]
while time.time() < end_time:
cf.commander.send_position_setpoint(position[0],
position[1],
position[2], 0)
time.sleep(0.1)
land(cf, sequence[-1])
except Exception as e:
print(e)
"""
def position_callback(uri, timestamp, data, logconf):
#x = data['kalman.stateX']
#y = data['kalman.stateY']
#z = data['kalman.stateZ']
#print("Uri: {}, Data: {}".format(uri, data))
print(data)
def stablizer_callback(timestamp,data,logconf):
x = data['stabilizer.roll']
y = data['stabilizer.pitch']
z = data['stabilizer.yaw']
print('orientation: ({},{},{})'.format(x,y,z))
# Get Position Data
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(lambda timestamp, data, logconf: position_callback(scf.cf.link_uri, timestamp, data, logconf))
log_conf.start()
# Get Roll, Pitch, Yaw
def GetStabilizer(scf):
log_conf = LogConfig(name='Stablizer',period_in_ms=500)
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(lambda timestamp, data, logconf: position_callback(scf.cf.link_uri, timestamp, data, logconf))
log_conf.start()
# Calculate Distance Between Two Agents
def calculate_distance(p1, p2):
diff = [0,0,0]
diff[0] = p1[0]-p2[0]
diff[1] = p1[1]-p2[1]
diff[2] = p1[2]-p2[2]
return math.sqrt( (diff[0]*diff[0]) + (diff[1]*diff[1]) + (diff[2]*diff[2]) )
def CalculateBearing(self,p1,p2):
#logic here
return
if __name__ == '__main__':
#logging.basicConfig(level=logging.DEBUG)
factory = CachedCfFactory(rw_cache='./cache')
cflib.crtp.init_drivers(enable_debug_driver=False)
with Swarm(uris,factory=factory) as swarm:
# If the copters are started in their correct positions this is
# probably not needed. The Kalman filter will have time to converge
# any way since it takes a while to start them all up and connect. We
# keep the code here to illustrate how to do it.
swarm.parallel(reset_estimator)
# The current values of all parameters are downloaded as a part of the
# connections sequence. Since we have 10 copters this is clogging up
# communication and we have to wait for it to finish before we start
# flying.
print('Waiting for parameters to be downloaded...')
swarm.parallel(wait_for_param_download)
swarm.parallel(start_position_printing)
#swarm.parallel(calculate_distance)
#swarm.parallel(GetStabilizer)
swarm.parallel(run_sequence, args_dict=seq_args)
Always appreciate the help.
Stay safe!