debugging cmdFullState tracking delay

[mygo5]

Hello!

I've recently been using CrazySwarm to perform various experiments and overall having much success. I'm using the low-level command mode cmdFullState() in order to track trajectories I have defined on the host PC. I've implemented this using a state machine which runs at about 100Hz. The tracking performance of the path is nearly perfect but considering the tracking of the trajectory is where the issues start to arise. Below is a visualization of the desired command sent in cmdFullState() as well as the "actual" positions coming in as read from VICON pose information:

My particular application sees me iteratively building trajectories mid-flight (on a finite time horizon), so this has only recently become an issue due to discrepancies in the expected vs actual position of the cf. I've taken some steps towards debugging this delay:

• I am sending the full trajectory.eval.pos, vel, acc, yaw, and omega components to ensure these feedforward terms can be properly utilized by the onboard mellinger controller. These trajectories are built by applying virtual snap "inputs" to a simulated vehicle before flight and so should be at least C4 continuous (continuous in jerk)
• I've tried tracking down publisher/subscriber calls from what gets called, all the way from calling cmdFullState() to the cflib's radio driver and printing out delays and haven't found anything too concerning (on the order of 5 ms)
• Additionally, using cfclient to manually control the cfs using a joystick doesn't yield the same delays, so I am not confident in saying is it a delay happening purely over the radio
• The delay is approximately 0.62 seconds and this is constant whether I am commanding a particularly fast or slow trajectory, which makes me feel as though this is not a simple steady state error in the tracking.

You can see if I shift the data over by 0.62 seconds then the desired and actual nearly perfectly overlap.

This excellent path, but not trajectory, tracking strongly suggests (at least to me) that there is something causing a delay in commanded inputs, a queue build-up somewhere, or something similar. However I wanted to reach out and inquire if this was a known phenemona or if there was perhaps an obvious place I hadn't yet looked.

Thanks in advance for any help anyone can give, CrazySwarm has been a great tool!

[knmcguire]

Hi!

What version of the crazyflie are you using? the 2.1 or the 2.1+ (with upgraded motors)

Have you tried any of the other controllers as well?

[mygo5]

We unfortunately don't have a uSD card deck, but I'm going to try and investigate the control/communication issue type because that makes a lot of sense. I haven't done any direct logging of variables directly from the crazyflie so trying to find my way around. Do you know if it would be possible to get the fullState command and the state estimate as the crazyflie firmware is storing them? Currently scouring the docks and found this for position:

https://www.bitcraze.io/documentation/repository/crazyflie-firmware/master/api/logs/#stateestimate

and this for some desired state:

https://www.bitcraze.io/documentation/repository/crazyflie-firmware/master/api/logs/#ctrltarget

if I could have the crazyflie send its own state estimate as well as fullState setpoint (both with the crazyflie's internal timestamps) I think that would give me a clue as to the communication/control nature of the delay. Thanks for the lead!

[whoenig]

Since you see the problem already in x, you can log "stateEstimate.x" and "ctrltarget.x" together. As you said, you would get both of them on the same clock and it would be easy to plot.

[mygo5]

This was a great lead to follow!

So, I learned I was able to add the logging to the crazyflies.yaml like this:

    enabled: true   
    uri: radio://0/80/2M/E7E7E7E7ED
    initial_position: [0, 0, 0]
    type: cf21  # see robot_types
    firmware_logging:
      enabled: true
      custom_topics:
        tracking_data: 
          frequency: 50
          vars: ["stateEstimate.x", "ctrltarget.x"]

I subscribed to the topic, plotted the variables "stateEstimate.x" and "ctrltarget.x" with their associated timestamps and got the following result:

This accounts for a ~0.5 second delay, the majority of the 0.62 second delay I've been seeing overall. I wonder if this is simply a limitation of the hardware? I thought perhaps the motion capture information feeding into the crazyflie's EKF could be delayed, but intuitively I feel that if that were the case the quad would have the opposite problem and exhibit overshoot. Accounting for the delay we see:

Just from a qualitative observation standpoint, it seems like the "actual" position has some delay before it starts tracking the desired states. I've also tried to reduce the frequency I send cmdFullState() from 100Hz to 20Hz to see if maybe I was overloading some command queue but didn't see a change in performance.

Thanks so much for the suggestion of what to look into here. Do you think an in-depth analysis of how the firmware deals with command setpoints would be appropriate given this information I've gathered? Or maybe I'm chasing the wrong thread? Much appreciated!

[whoenig]

Which controller are you using? I think such a delay is normal for the default PID controller, but Mellinger or Lee shouldn't have such large delays for such a relatively slow trajectory.

[mygo5]

Well... I thought I was using the Mellinger controller, but at some point it must've got switched to PID. This fixed everything, definitely an oversight on my part. Thanks for the support, I definitely learned a lot along the way even though it was right under my nose the entire time. Here's a plot of the x position over time now:

Looking great!

The only thing I noticed is the ability for the z-tracking. While in a "hover mode" I command:

zero_vec = [0.0, 0.0, 0.0]
cf.cmdFullState(hover_state, zero_vec, zero_vec, 0.0, zero_vec]

and that works great.

However when I start my trajectory tracking I notice an instant dip:

I'm not sure why exactly this is occurring... I remember reading here that the crazyflies use the mellinger controller with an integral term added. Perhaps an older version of the firmware doesn't include the integral and maybe isn't tuned properly due to that? Although then I would think my hover mode would fall down which I'm not observing. Is this a known issue?

Thanks!

edit: just read the part about the platform mass, its probably that since I've added the mocap deck

[whoenig]

This is weird - it can happen during very fast flight, but your plots don't look like there is anything very aggressive going on. The parameters/controller hasn't really changed since almost a decade. Nevertheless, it can't hurt to tune the z-part a bit. I think you could safely increase P, D, and I gains on z.

Use the parameters for tuning:
https://github.com/bitcraze/crazyflie-firmware/blob/1e40a14d9f112fbb4e7440250d52ae41b9810ccf/src/modules/src/controller/controller_mellinger.c#L369

Default values:
https://github.com/bitcraze/crazyflie-firmware/blob/1e40a14d9f112fbb4e7440250d52ae41b9810ccf/src/modules/src/controller/controller_mellinger.c#L61-L63

[mygo5]

accounting for the added mass of the mocap deck fixed the issue