-
hello. thanks. import time
import threading
import cflib.crtp
from cflib.crazyflie import Crazyflie
from cflib.crazyflie.log import LogConfig
from cflib.crazyflie.syncCrazyflie import SyncCrazyflie
from cflib.crazyflie.syncLogger import SyncLogger
import logging
import numpy as np
import pandas as pd
logging.basicConfig(level=logging.ERROR)
# URI to the Crazyflie to connect to
uri_t = 'radio://0/125/2M/E7E7E7E7A2'
uri_a = 'radio://0/90/2M/E7E7E7E7CC'
sequence=[0,0,0]
a=[]
flytime = 20
height = 0.3
position = [4,4]
class TOC:
def __init__(self, cf):
self._cf = cf
self.log_conf = LogConfig(name='Position', period_in_ms=200)
#self.log_conf.add_variable('ranging.distance0', 'float')
self.log_conf.add_variable('ranging.distance1', 'float')
#self.log_conf.add_variable('ranging.distance2', 'float')
self.log_conf.add_variable('ranging.distance3', 'float')
#self.log_conf.add_variable('ranging.distance4', 'float')
#self.log_conf.add_variable('ranging.distance5', 'float')
self.log_conf.add_variable('tdoa3.hmDist', 'float')
self._cf.log.add_config(self.log_conf)
self.log_conf.data_received_cb.add_callback(self.position_callback)
self.log_conf.start()
def position_callback(self, timestamp, data, logconf):
#self.d0=data['ranging.distance0']
self.d1=data['ranging.distance1']
#self.d2=data['ranging.distance2']
self.d3=data['ranging.distance3']
#self.d4=data['ranging.distance4']
#self.d5=data['ranging.distance5']
self.d6=data['tdoa3.hmDist']
def wait_for_position_estimator(scf):
print('Waiting for estimator to find position...')
log_config = LogConfig(name='Kalman Variance', period_in_ms=100)
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 reset_estimator(scf):
global data
cf = scf.cf
cf.param.set_value('kalman.resetEstimation', '1')
time.sleep(0.1)
cf.param.set_value('kalman.resetEstimation', '0')
data = TOC(cf)
time.sleep(0.1)
wait_for_position_estimator(cf)
def take_off(cf, position, tot):
take_off_time = tot
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, lt):
landing_time = lt
sleep_time = 0.1
steps = int(landing_time / sleep_time)
vz = -position / landing_time
#print(vz)
for i in range(steps):
cf.commander.send_velocity_world_setpoint(0, 0, vz, 0)
time.sleep(sleep_time)
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)
def run_sequence(scf):
global a, height, flytime, delay
cf = scf.cf
end_time = time.time() + flytime
cf.param.set_value('flightmode.posSet', '1')
def TDOA(d):
d21 = d[1]-d[0]
d31 = d[2]-d[0]
d41 = d[3]-d[0]
d51 = d[4]-d[0]
return d21, d31, d41, d51
#take_off(cf, height, 3.0)
while time.time() < end_time:
#cf.commander.send_hover_setpoint(0, 0, 0, height)
#d = [data.d0, data.d1, data.d2, data.d3,data.d4,data.d5, data.d6]
print(data.d1, data.d3, data.d6)
#r1=TDOA(d)
# print(r1)
time.sleep(0.1)
#land(cf, height, 3.0)
time.sleep(0.1)
class trans_self:
def __init__(self):
self.xyz = 0
def trans_var(self):
global data
self.xyz = [[data.x, data.y, data.z]]
if __name__ == '__main__':
cflib.crtp.init_drivers(enable_debug_driver=False)
threads = []
with SyncCrazyflie(uri_t, cf=Crazyflie(rw_cache='./cache')) as scf_t, SyncCrazyflie(uri_a, cf=Crazyflie(rw_cache='./cache')) as scf_a:
reset_estimator(scf_a)
reset_estimator(scf_t)
#scf_t.cf.param.set_value('loco.mode', '3')
scf_t.cf.param.set_value('tdoa3.hmId', '253')
scf_t.cf.param.set_value('tdoa3.hmAnchLog', '254')
scf_t.cf.param.set_value('tdoa3.hmTwr', '1')
#scf_a.cf.param.set_value('loco.mode', '3')
scf_a.cf.param.set_value('tdoa3.hmId', '254')
scf_a.cf.param.set_value('tdoa3.hmAnchLog', '253')
scf_a.cf.param.set_value('tdoa3.hmTwr', '1')
#scf_a.cf.param.set_value('tdoa3.hmTwrTXPos', '2')
t1 = threading.Thread(target=run_sequence, args=(scf_a, ))
t2 = threading.Thread(target=run_sequence, args=(scf_t, ))
t1.start()
threads.append(t1)
t2.start()
threads.append(t2)
for thread in threads:
thread.join() |
Beta Was this translation helpful? Give feedback.
Replies: 1 comment 8 replies
-
Just for reference of others, these are the instructions to try out the experimental Hybrid TDOA mode: https://www.bitcraze.io/documentation/repository/crazyflie-firmware/master/functional-areas/loco-positioning-system/tdoa3_hybrid_mode/ In the sense of your issue, where there any loco anchors on and being received by the crazyflies as well? |
Beta Was this translation helpful? Give feedback.
I found the reason.
There seems to be a problem where the ID is not recognized properly if it is written in string type when writing the ID.
In the code above, I changed the string type to int, and the distance was displayed correctly.