Kalman Params callback

All discussions related to the Loco Positioning system
Post Reply
stupid_moron
Beginner
Posts: 22
Joined: Tue Jan 21, 2020 7:05 am

Kalman Params callback

Post by stupid_moron »

Code: Select all

import logging, time,sys,cflib.crtp

from cflib.crazyflie import Crazyflie
from cflib.crazyflie.log import LogConfig, Log, LogVariable
from cflib.crazyflie.syncCrazyflie import SyncCrazyflie
from cflib.crazyflie.syncLogger import SyncLogger

uris = ['radio://0/80/2M/E7E7E7E701','radio://0/80/2M/E7E7E7E702']
logging.basicConfig(level=logging.ERROR)

def callback0(timestamp,data,logconf):
    print("0:",data,logconf) #GUI Label
    
def callback1(timestamp,data,logconf):
    print("1:",data,logconf) #GUI Label
    
if __name__ == '__main__':
    crazyflie = []
    syncCrazyflie = []
    kalmanLog = []
    kalmanParams = [("kalman", "stateX", "float"),("kalman", "stateY", "float"),("kalman", "stateZ", "float")]
    cflib.crtp.init_drivers(enable_debug_driver=False)

    for y in range(0,2):
        crazyflie.append(Crazyflie(rw_cache='./cache'))
        syncCrazyflie.append(SyncCrazyflie(uris[y], cf=crazyflie[y]))
        kalmanLog.append(LogConfig("GetKalmanPos", 100))
        for param in kalmanParams:
            kalmanLog[y].add_variable('{}.{}'.format(param[0], param[1]), param[2])
        
        syncCrazyflie[y].open_link()
        crazyflie[y].log.add_config(kalmanLog[y])
        

    kalmanLog[0].data_received_cb.add_callback(callback0) #<-----
    kalmanLog[0].start()
    
    kalmanLog[1].data_received_cb.add_callback(callback1)#<-----
    kalmanLog[1].start()

    time.sleep(2)
    kalmanLog[0].stop()
    kalmanLog[1].stop()
I have a problem that have stumped me for days. The above code is the original that works

For the 2 lines marked "<-----" i would need to create 2x functions for the callback, and the function will do something(print in this case as an example)

I need hep to reduce the 2x function into a 1x function or a loop function style. The purpose is that, there will be multiple crazyflies and i hope to conect/disconnect them dynamically. Upon connecting them, their kalmanLog should start as well.
The idea is what if i have 15x Crazyflie connected, I would have to crease 15x function of def callback0 all the way to def callback 14. Im sure there's a elegant solution.

Potential solutions that i have tried but doesnt work:
TRY 1:

Code: Select all

def callback(timestamp,data,logconf,y):
    print("0:",data,logconf) #GUI Labe
    
    for y in range(number_of_crazyflies):
    kalmanLog[y].data_received_cb.add_callback(callback(y)) #<-----
    kalmanLog[y].start() 
TRY 2:

Code: Select all

def callback(timestamp,data,logconf,y):
    print("0:",data,logconf) #GUI Label

    for y in range(number_of_crazyflies):
    kalmanLog[y].data_received_cb.add_callback(partial(callback,y)) #<-----
    kalmanLog[y].start() 

Try 3:
Since the logconf will be different and unique everytime the Crazyflie is powered on.
i created a stored_logconf = [] to store the hex value upon connection of the crazyflie and using this callback style:

Code: Select all

def callback(timestamp,data,logconf):
    for y in range(number_of_crazyflie):
        while(str(logconf)) == stored_logconf[y]:
            print(data)

    for y in range(number_of_crazyflie):
    kalmanLog[y].data_received_cb.add_callback(callback) #<-----
    kalmanLog[y].start() 
So with the above 3 methods that i have tried, they didnt work due to either:

def(callback) expected 0 arguments but 3 was provided
def(callback) expected 3arguments but 4 was provided

kristoffer
Bitcraze
Posts: 280
Joined: Tue Jun 30, 2015 7:47 am

Re: Kalman Params callback

Post by kristoffer »

It should be possible to use the same callback for all log confs.
The last argument in the callback is logconf, use that to figure out which log configuration it originates from. The log conf also contains the Crazyflie object if needed.

stupid_moron
Beginner
Posts: 22
Joined: Tue Jan 21, 2020 7:05 am

Re: Kalman Params callback

Post by stupid_moron »

i have tried allocating to the same call back funct and differentiate the state estimates by using the LogConfig data. But the values still seems to be wrong. Suppose in a 2x Crazyflie setup with state estimates x0,y0,z0 and x1,y1,z1.

sometimes it prints the x0,y1,z0 and x1, y0,z1, etc etc.

I have a feeling that its becase the call back doesnt pump out the values together in an array like [x0,y0,z1]. Rather, it pumps out the value in x0,y0 etc?

Im not too sure about this. maybe im wrong?

kristoffer
Bitcraze
Posts: 280
Joined: Tue Jun 30, 2015 7:47 am

Re: Kalman Params callback

Post by kristoffer »

There is no guarantee of the order of the log data, it depends on when the Crazyflie transmits it. You must use the key(s) in the dictionary to know which log data you got.
All data in one call to the callback should belong to the same log config though.

I did a quick test by modifying the basiclog.py example and I think it looks OK

[79953][Stabilizer-cf1]: {'stabilizer.roll': 0.003590386128053069, 'stabilizer.pitch': 0.020818669348955154, 'stabilizer.yaw': 0.014612825587391853}
[570838][Stabilizer-cf2]: {'stabilizer.roll': 0.013573986478149891, 'stabilizer.pitch': 0.5746753811836243, 'stabilizer.yaw': -0.04228271543979645}
[79963][Stabilizer-cf1]: {'stabilizer.roll': 0.0047375536523759365, 'stabilizer.pitch': 0.02078910730779171, 'stabilizer.yaw': 0.015045677311718464}
[570848][Stabilizer-cf2]: {'stabilizer.roll': 0.012147619388997555, 'stabilizer.pitch': 0.5777081251144409, 'stabilizer.yaw': -0.04305123910307884}
[79973][Stabilizer-cf1]: {'stabilizer.roll': 0.0052658612839877605, 'stabilizer.pitch': 0.021292686462402344, 'stabilizer.yaw': 0.013424771837890148}
[570858][Stabilizer-cf2]: {'stabilizer.roll': 0.013016912154853344, 'stabilizer.pitch': 0.5779128074645996, 'stabilizer.yaw': -0.04344018176198006}
[79983][Stabilizer-cf1]: {'stabilizer.roll': 0.006973275914788246, 'stabilizer.pitch': 0.017849966883659363, 'stabilizer.yaw': 0.014487801119685173}
[570868][Stabilizer-cf2]: {'stabilizer.roll': 0.011799642816185951, 'stabilizer.pitch': 0.5811878442764282, 'stabilizer.yaw': -0.043568771332502365}
[79993][Stabilizer-cf1]: {'stabilizer.roll': 0.008892353624105453, 'stabilizer.pitch': 0.018246948719024658, 'stabilizer.yaw': 0.013562421314418316}
[570878][Stabilizer-cf2]: {'stabilizer.roll': 0.012033618055284023, 'stabilizer.pitch': 0.5852932333946228, 'stabilizer.yaw': -0.04329010844230652}
[80003][Stabilizer-cf1]: {'stabilizer.roll': 0.008192009292542934, 'stabilizer.pitch': 0.019324002787470818, 'stabilizer.yaw': 0.013894503936171532}
[570888][Stabilizer-cf2]: {'stabilizer.roll': 0.011935936287045479, 'stabilizer.pitch': 0.587138831615448, 'stabilizer.yaw': -0.043436020612716675}

stupid_moron
Beginner
Posts: 22
Joined: Tue Jan 21, 2020 7:05 am

Re: Kalman Params callback

Post by stupid_moron »

Hi sorry my mistake, i have to insert the logger entry into a dict type first before calling it. It works now

However, I cannot perform the actions: Connect, then disconnect, and connect again, it gives an error. Below is the code i have modified from the sync logger. The error comes during the 2nd connection status to the crazyflie.

Code: Select all

import logging,time,sys, 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
logging.basicConfig(level=logging.ERROR)

class main():
    def __init__(self):
        pass
    def run(self):
        cflib.crtp.init_drivers(enable_debug_driver=False)
        uris = ['radio://0/80/2M/E7E7E7E701']
        lg_stab = LogConfig(name='Stabilizer', period_in_ms=10)
        lg_stab.add_variable('stateEstimate.x', 'float')
        lg_stab.add_variable('stateEstimate.y', 'float')
        scf =  SyncCrazyflie(uris[0], cf=Crazyflie(rw_cache='./cache'))

        scf.open_link()
        logger = SyncLogger(scf, lg_stab)
        logger.connect()
        for log_entry in logger:
            data = log_entry[1]
            print(data)
            break
        logger.disconnect()
        scf.close_link()
        
        time.sleep(1)
        
        scf.open_link()
        logger = SyncLogger(scf, lg_stab)
        logger.connect()
        for log_entry in logger:
            data = log_entry[1]
            print(data)
            break
        logger.disconnect()
        scf.close_link()
            
        print("end")
        
if __name__ == '__main__':
    main().run()

kimberly
Bitcraze
Posts: 470
Joined: Fri Jul 06, 2018 11:13 am

Re: Kalman Params callback

Post by kimberly »

Hi,

The problem you are mentioning does not seem to be related to your initial problem with the logging callbacks, but it is indeed an interesting problem that we would like to discuss.

To increase search-ability for the other users, could you make a new thread about the connection issue you are having with a minimal example script?

Post Reply