How can I measure the distance between drones using tdoa3 hybrid mode?

[phUR99]

hello.
We are currently trying to use one drone as an anchor to measure the distance to another drone. In this process, the firmware was updated to use hmDist in tdoa3 hybrid mode supported by bitcraze (hybrid mode setting was turned on and applied to the drone).
I configured the code and measured the distance as shown in the example below.
However, hmDist keeps showing only 0 and the distance does not seem to be measured properly.
What should I do?
If anyone knows, I would appreciate some help.

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

[knmcguire]

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?

[phUR99]

Thank you for the response.
I thought it was an example that only required two Crazyflies and a receiver without needing a Loco Positioning Anchor, but it seems that's not the case. I will connect the Loco Positioning Anchor and try again.

[phUR99]

I've connected the anchor and ran the code, but still, hmDist only outputs 0 when checked. Also, I've checked other log variables in hybrid mode, but there seems to be no change. How can I resolve this?

[knmcguire]

hmm okay. Eventhough it is experimental code, it would be nice to keep it working. Then we have to investigate this in our own setup, I hope this week or else the next. Currently I'm working from home and no one at the office is available to try this.

I've made an issue ticket to investigate this as soon as we can: bitcraze/crazyflie-firmware#1375

Is this is blocking you, I perhaps can advise you to draw some inspiration of this closed PR: bitcraze/crazyflie-firmware#580. It wasn't accepted as it was a bit too brute force of replacing the full driver, but it might be sufficient. It is based on a paper publication though by the author of this PR.

[phUR99]

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.

        input("press enter to start the flight")
        scf_t.cf.param.set_value('tdoa3.hmId', 254)
        scf_t.cf.param.set_value('tdoa3.hmAnchLog', 253)
        scf_t.cf.param.set_value('tdoa3.hmTwr', '1')
        scf_t.cf.param.set_value('tdoa3.hmTwrEstPos', '1')
        time.sleep(0.5)
        scf_a.cf.param.set_value('tdoa3.hmId', 253)        
        scf_a.cf.param.set_value('tdoa3.hmAnchLog', 254)
        scf_a.cf.param.set_value('tdoa3.hmTwr', '1') 
        scf_a.cf.param.set_value('tdoa3.hmTwrEstPos', '1')
        time.sleep(0.5)
        print(scf_a.cf.param.get_value(complete_name='tdoa3.hmId'))        
        print(scf_a.cf.param.get_value(complete_name='tdoa3.hmAnchLog'))        
        print(scf_a.cf.param.get_value(complete_name='tdoa3.hmTwr'))
        print(scf_a.cf.param.get_value(complete_name='tdoa3.hmTwrEstPos'))

        print(scf_t.cf.param.get_value(complete_name='tdoa3.hmId'))
        print(scf_t.cf.param.get_value(complete_name='tdoa3.hmAnchLog'))
        print(scf_t.cf.param.get_value(complete_name='tdoa3.hmTwr'))
        print(scf_t.cf.param.get_value(complete_name='tdoa3.hmTwrEstPos'))

[knmcguire]

Ah yes good find! And weird that you didn't get an error on this... there should though so I'll make a ticket for that!

[knmcguire]

made an issue here: bitcraze/crazyflie-lib-python#455. Hopefully we'll fix it soon