question about log period_in_ms and commander.send_setpoint function

Discussions and questions about the Crazyflie Nano Quadcopter
Post Reply
Baoasi
Beginner
Posts: 9
Joined: Mon Feb 08, 2021 10:52 am

question about log period_in_ms and commander.send_setpoint function

Post by Baoasi »

Hallo,
i have made an off- board controll, which means the position controller in PC and send the control commanders(roll, pitch, yaw and thrust) into crazyflie throught crazyradio. I can accomplish the goal now, but there is still a problem. When I shorten the log sampling time, the flight becomes very unstable. This problem occurs when the sampling time is less than 15ms, and the shorter the time, the greater the problem, until the flight is completely out of control. I tried to monitor the code running time and found that when the log sampling time is less than 15ms, the running time of function send_setpoint(roll, pitch, yaw, thrust) will be shortened as the sampling time decreases.
somebody seeing the same problem?
Thank you in advance.

The following is just a sample code

Code: Select all

import time 
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 
from pid import *

state_estimate = state_s()


SUM = 0

thrust_actual = 0

calculate = 0


# URI to the Crazyflie to connect to
uri = 'radio://0/80/2M'

# Change the sequence according to your setup
#             x    y    z  YAW
sequence = [
    (0.0, 0.0, 0.5, 0)
]

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

    log_config = LogConfig(name='Kalman Variance', period_in_ms=500)
    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):
    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 position_callback(timestamp, data, logconf):
    global state_estimate
    state_estimate.position.x = data['kalman.stateX']


def start_position_printing(scf):
    log_conf = LogConfig(name='Position', period_in_ms= 150)   # actual 15ms
    log_conf.add_variable('kalman.stateY', 'float')

    scf.cf.log.add_config(log_conf)
    log_conf.data_received_cb.add_callback(position_callback)
    log_conf.start()

def run_test(scf, sequence, SUM ):
    cf = scf.cf

    for position in sequence:
        print('Setting position {}'.format(position))

        for i in range(350):  # 3.5s
            time_start2 = time.time()
            cf.commander.send_setpoint(0, -0, 0, 0)
            time_end2 = time.time()
            SUM += time_end2 - time_start2
 
            time.sleep(0.01)

    print('SUM: {} '.format((SUM) * 1000))

    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)



if __name__ == '__main__':
    cflib.crtp.init_drivers(enable_debug_driver=False)

    with SyncCrazyflie(uri, cf=Crazyflie(rw_cache='./cache')) as scf:
        reset_estimator(scf)

        # Unlock startup thrust protection
        scf.cf.commander.send_setpoint(0, 0, 0, 0 )
        start_position_printing(scf)
        time_start = time.time()
        run_test(scf, sequence, SUM )

        time_end = time.time()
        print('total time: {} '.format((time_end - time_start)))

arnaud
Bitcraze
Posts: 2538
Joined: Tue Feb 06, 2007 12:36 pm

Re: question about log period_in_ms and commander.send_setpoint function

Post by arnaud »

I am not sure what is happening exactly but I have a note about the logging subsystem: When you setup a log block, the period is set by step of 10ms, so what I guess is happening is that if you ask for 15ms you are actually getting 20ms period and when you go bellow that you are getting 10ms period (100Hz).

This does not explain the instability though. It is possible, if you are logging more than one packet at 100Hz, that you are reaching the bandwidth limit of the radio. With full packets, the radio can transfer ~350packets per seconds, so 3 logs at 100Hz should work, more might actually queue up in the Crazyflie and cause problems.
wydmynd
Member
Posts: 87
Joined: Thu Dec 06, 2018 9:25 am

Re: question about log period_in_ms and commander.send_setpoint function

Post by wydmynd »

this is good information to have in the documentation. maximum recommended bytes or blocks per second. Also on USB (I assume it is higher)
arnaud
Bitcraze
Posts: 2538
Joined: Tue Feb 06, 2007 12:36 pm

Re: question about log period_in_ms and commander.send_setpoint function

Post by arnaud »

No, it is not higher on USB since it is using exactly the same log protocol.

The minimum logging period is already documented in the lib (https://www.bitcraze.io/documentation/r ... nd-logging) however it could indeed be more clean on the detail. What documentation did you look at for the log? I guess this is the one that we should improve.
Baoasi
Beginner
Posts: 9
Joined: Mon Feb 08, 2021 10:52 am

Re: question about log period_in_ms and commander.send_setpoint function

Post by Baoasi »

arnaud wrote: Thu Mar 11, 2021 9:37 am I am not sure what is happening exactly but I have a note about the logging subsystem: When you setup a log block, the period is set by step of 10ms, so what I guess is happening is that if you ask for 15ms you are actually getting 20ms period and when you go bellow that you are getting 10ms period (100Hz).

This does not explain the instability though. It is possible, if you are logging more than one packet at 100Hz, that you are reaching the bandwidth limit of the radio. With full packets, the radio can transfer ~350packets per seconds, so 3 logs at 100Hz should work, more might actually queue up in the Crazyflie and cause problems.
sorry, maybe i didn’t make it clear. Because i need faster logs, so I hack it by removing the *10 in log block in the firmware:

Code: Select all

case CONTROL_START_BLOCK:
      // ret = logStartBlock( p.data[1], p.data[2]*10);
      ret = logStartBlock( p.data[1], p.data[2]);                // Modified
      break;
so the sampling time is now 15ms.
arnaud
Bitcraze
Posts: 2538
Joined: Tue Feb 06, 2007 12:36 pm

Re: question about log period_in_ms and commander.send_setpoint function

Post by arnaud »

Ho, I see, then this is a bit of a different problem.

My guess is that things are queuing-up in the Crazyflie because of a lack of bandwidth and you are getting delayed measurement. I am not sure how this can be verified though, maybe one way would be to timestamp the log packet when they arrive in python and to calculate the offset between the log timestamp in the Crazyflie and the PC clock. This will not allow to get relative delay, only absolute, but if my assumption is right the difference between the Crazyflie and PC clock should increase and then plateau when the Crazyflie queues are full.

In theory you should be able to get ~400 to 800 packets per seconds from the Crazyflie, so assuming you do not have too many of these high-frequency logs, I would expect it to work fine, so I am not sure what can be going-on. One idea would be to disable the relaxation timer in the radio communication loop, this can be done by setting waitTime to 0: https://github.com/bitcraze/crazyflie-l ... er.py#L638. This should reduce the jitter when receiving packet and could help.
Post Reply