## Distance Between Two Crazyflie using LPS

Discussions about autonomous flight in general, regardless of positioning method
youngbin1130
Member
Posts: 42
Joined: Thu Dec 12, 2019 4:28 am

### Distance Between Two Crazyflie using LPS

Hello again.

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
#
#
#  This program is free software; you can redistribute it and/or
#  modify it under the terms of the GNU General Public License
#
#  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

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)

var_y_history =  * 10
var_x_history =  * 10
var_z_history =  * 10

threshold = 0.001

with SyncLogger(scf, log_config) as logger:
endTime = time.time() + 10

for log_entry in logger:
data = log_entry

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

while not scf.cf.param.is_updated:
time.sleep(1.0)

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 / 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 / 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')
line_count = 0
distance = float(row) * 0.3
#print(row+' ' + row)
mc.forward(0.2,distance)
line_count += 1
mc.stop

try:
cf = scf.cf

take_off(cf, sequence)
for position in sequence:
print('Setting position {}'.format(position))
end_time = time.time() + position
while time.time() < end_time:
cf.commander.send_position_setpoint(position,
position,
position, 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.start()

# Get Roll, Pitch, Yaw
def GetStabilizer(scf):
log_conf = LogConfig(name='Stablizer',period_in_ms=500)

log_conf.start()

# Calculate Distance Between Two Agents
def calculate_distance(p1, p2):
diff = [0,0,0]
diff = p1-p2
diff = p1-p2
diff = p1-p2
return  math.sqrt( (diff*diff) + (diff*diff) + (diff*diff) )

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.

swarm.parallel(start_position_printing)
#swarm.parallel(calculate_distance)
#swarm.parallel(GetStabilizer)
swarm.parallel(run_sequence, args_dict=seq_args)

``````
In my code, I used p1 and p2 to be the location of each drone (which is going to be expanded in the future to maybe 4 or 5) and I have added a function which calculates the distance between two points. However, I'm unsure how to how to make sure that p1 belongs to URI1 and p2 to URI2. I'm not sure if I'm making myself clear. If I'm able to do this, I would theoretically find the difference in the YAW as well.

Always appreciate the help.

Stay safe!
kimberly
Bitcraze
Posts: 518
Joined: Fri Jul 06, 2018 11:13 am

### Re: Distance Between Two Crazyflie using LPS

Hmm it doesn't seem that your script would be able to work at all, but it is difficult to see...

What swarm.parallel does, is to create a thread for that function for each crazyflie in python. It means that you will not be able to access the position of the other drone in the same thread. So you would need to make a global variable for p1 and p2 and first fill those with the position by swarm.parallel(get position) , and once that thread is finished (to be sure that the positions have been copied) calculate the the distance between the two in a non thread normal python function.

So with the get_position function, you can put in an if-statement that looks at the URI of the crazyflie and if its URI1, fill p1 and when its URI2, fill p2. So I hope that works.

It might be handy to also read up on the thread handling in python in general. It can be a bit messy sometimes if not handled properly
youngbin1130
Member
Posts: 42
Joined: Thu Dec 12, 2019 4:28 am

### Re: Distance Between Two Crazyflie using LPS

Hey Kimberly.

That's exactly what I did for my first iteration. I had further improved on it after some more thinking.

Code: Select all

``````# -*- coding: utf-8 -*-
#
#     ||          ____  _ __
#  +------+      / __ )(_) /_______________ _____  ___
#  | 0xBC |     / __  / / __/ ___/ ___/ __ `/_  / / _ \
#  +------+    / /_/ / / /_/ /__/ /  / /_/ / / /_/  __/
#   ||  ||    /_____/_/\__/\___/_/   \__,_/ /___/\___/
#
#  Copyright (C) 2017-2018 Bitcraze AB
#
#
#  This program is free software; you can redistribute it and/or
#  modify it under the terms of the GNU General Public License
#
#  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 numpy
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

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
}

location = {} # Dictionary of URI and Location
rotation = {} # Dictionaty of URI and rotation

def wait_for_position_estimator(scf):
print('Waiting for estimator to find position...')

log_config = LogConfig(name='Stabilizer', period_in_ms=100)

var_y_history =  * 10
var_x_history =  * 10
var_z_history =  * 10

threshold = 0.001

with SyncLogger(scf, log_config) as logger:
endTime = time.time() + 10

for log_entry in logger:
data = log_entry

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

while not scf.cf.param.is_updated:
time.sleep(1.0)

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 / 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 / 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(10)

"""
# 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')
line_count = 0
distance = float(row) * 0.3
#print(row+' ' + row)
mc.forward(0.2,distance)
line_count += 1
mc.stop

try:
cf = scf.cf

take_off(cf, sequence)
for position in sequence:
print('Setting position {}'.format(position))
end_time = time.time() + position
while time.time() < end_time:
cf.commander.send_position_setpoint(position,
position,
position, 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))

point = (x,y,z) # (-39.528446197509766, -8.916569709777832, -0.5112752318382263)\

location[uri] = point # {'radio://0/33/1M': (-3.2653462886810303, -1.9112612009048462, 0.020919203758239746), 'radio://0/40/1M': (-4.532137870788574, -0.9649770855903625, 0.047263506799936295)}

calculate_distance(location[URI1],location[URI3])

def stablizer_callback(uri, timestamp, data, logconf):
roll = data['stabilizer.roll']
pitch = data['stabilizer.pitch']
yaw = data['stabilizer.yaw']

# print("Uri: {}, Data: {}".format(uri, data))

stabilizer = (roll, pitch, yaw)

rotation[uri] = stabilizer

CalculateStabDiff(rotation[URI1],rotation[URI3])

# Get Position Data
def start_position_printing(scf):

log_conf = LogConfig(name='Position', period_in_ms=100)

log_conf.start()

# Get Roll, Pitch, Yaw
def GetStabilizer(scf):
log_conf = LogConfig(name='Stablizer',period_in_ms=100)

log_conf.start()

# Calculate Distance Between Two Agents
def calculate_distance(p1, p2):
diff = [0,0,0]
diff = p1-p2
diff = p1-p2
diff = p1-p2
print(math.sqrt( (diff*diff) + (diff*diff) + (diff*diff) ))

def OrientationDifference(p1,p2):
print("aa")

# Calculate Bearing with Drone Data
def CalculateBearing(p1,p2):
u_vector1= p1/numpy.linalg.norm(p1)
u_vector2= p2/numpy.linalg.norm(p2)
dot_product = numpy.dot(u_vector1, u_vector2)
angle = numpy.arccos(dot_product)

print(angle)

# Calculate difference in YAW relative to P1
def CalculateStabDiff(p1,p2):
bearingDiff = [0,0,0]
bearingDiff = p1-p2
bearingDiff = p1-p2
bearingDiff = p1-p2
print(bearingDiff)

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.

swarm.parallel(start_position_printing)
swarm.parallel(calculate_distance)
swarm.parallel(GetStabilizer)
swarm.parallel(run_sequence, args_dict=seq_args)

``````
However the current code allows for 1 to 1 drone (thus a maximum of two). If I were to scale this into more drones (ex. 1 drone to 3 thus calculating the distance from A to B, A to C and A to D), would there be an drastic hit in performance? I would assume not so much since the calculation is done on PC (if I'm not mistaken).
kimberly
Bitcraze
Posts: 518
Joined: Fri Jul 06, 2018 11:13 am

### Re: Distance Between Two Crazyflie using LPS

I don't think that there will be a problem with performance, however (depended on how many crazyflies you will use) communication might be a bottle neck.

Also what you need to know about threads, is that it difficult to control when a certain position is copied from the function to the global variable. If you would do it all in a loop (getting the log, calculate distance and react to it), it might be that for some crazyflies is still has the old variable in there from the previous iteration. Its a bit tricky but maybe just restarting the thread again, like you are doing in the main function (every swarm.parallel opens up the thread and closes it again once the sequences are finished) might be the way to go, but it might also slow down the overall performance

It is though to say how the performance will be affected exactly, but just go ahead and try it out.