Distance Between Two Crazyflie using LPS
Posted: Tue May 26, 2020 12:50 pm
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.
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!
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!