Crazyfly gets communicated the wrong position

[raphael-jms]

Hi!
We are trying out our new crazyflies, but so far I could not get them to fly. When I start the hello_world, they mostly flip over (sometimes they also jump around on the ground).

Our current setup:

• We use
  • Qualisys using motion capture (not libobjecttracker)
  • mellinger controller
  • cflib backend
• For now, we only use one Crazyflie, but we want to have more later
• I tried to fiddle around with the configuration of the local frame of the crazyflie in Qualisys, and it should be okay now

My investigations so far found the following:

• manually moving the crazyflie and reading the /poses topic looks correct
• Using the kalman filter: When opening rviz, there are two frames, one called cf04_mocap which is at the correct place. There is also another one only called cf04. This one periodically starts at the origin mocap and moves outwards for ca 25s before it starts at the origin again.The /cf04/pose topic shows the state of the cf04 frame that is moving outwards; this is the one that is sent to the Crazyflie, isn't it?
• Using the complementary estimation: The cf04_mocap is correct as before, but the cf04 frame is somewhere at x: 0.0; y: 0.0; z: 262.7536315917969

~/crazyswarm_ws$ ros2 launch crazyflie launch.py backend:=cflib
[INFO] [launch]: All log files can be found below /home/sml-laptop-1/.ros/log/2025-06-24-13-13-34-287041-sml-laptop-1-Latitude-7450-45232
[INFO] [launch]: Default logging verbosity is set to INFO
/opt/ros/jazzy/lib/python3.12/site-packages/launch/conditions/launch_configuration_equals.py:53: UserWarning: The 'LaunchConfigurationEquals' and 'LaunchConfigurationNotEquals' Conditions are  deprecated. Use the 'EqualsSubstitution' and 'NotEqualsSubstitution' substitutions instead! E.g.:
  IfCondition(
  	EqualsSubstitution(LaunchConfiguration('some_launch_arg'), "some_equality_check")
  )
  warnings.warn(
[INFO] [motion_capture_tracking_node-1]: process started with pid [45235]
[INFO] [crazyflie_server.py-2]: process started with pid [45236]
[INFO] [teleop-3]: process started with pid [45237]
[INFO] [joy_node-4]: process started with pid [45238]
[INFO] [gui.py-5]: process started with pid [45239]
[teleop-3] [INFO] [1750763614.372149541] [teleop]: Mode changed to high_level
[motion_capture_tracking_node-1] logClouds=0
[crazyflie_server.py-2] [INFO] [1750763615.400020293] [crazyflie_server]: [cf04] is connected!
[crazyflie_server.py-2] [INFO] [1750763615.400783487] [crazyflie_server]: All Crazyflies are connected! It took 0.6828279495239258 seconds
[motion_capture_tracking_node-1] [WARN] [1750763615.412157369] [motion_capture_tracking]: New subscription discovered on topic '/poses', requesting incompatible QoS. No messages will be sent to it. Last incompatible policy: DEADLINE_QOS_POLICY
[crazyflie_server.py-2] [INFO] [1750763615.414726100] [crazyflie_server]: [cf04] setup logging for pose at freq 10
[crazyflie_server.py-2] [INFO] [1750763615.415753981] [crazyflie_server]: [cf04] setup logging for status at freq 1
[crazyflie_server.py-2] [INFO] [1750763615.417204854] [crazyflie_server]: All Crazyflies logging are initialized.
[crazyflie_server.py-2] [INFO] [1750763615.417508392] [crazyflie_server]: [cf04] commander.enHighLevel is set to 1
[crazyflie_server.py-2] [INFO] [1750763615.418067518] [crazyflie_server]: [cf04] kalman.pNAcc_xy is set to 1.0
[crazyflie_server.py-2] [INFO] [1750763615.419014707] [crazyflie_server]: [cf04] locSrv.extPosStdDev is set to 0.001
[crazyflie_server.py-2] [INFO] [1750763615.419870857] [crazyflie_server]: [cf04] locSrv.extQuatStdDev is set to 0.05
[crazyflie_server.py-2] [INFO] [1750763615.420718641] [crazyflie_server]: [cf04] stabilizer.controller is set to 2
[crazyflie_server.py-2] [INFO] [1750763615.421670011] [crazyflie_server]: [cf04] stabilizer.estimator is set to 2
[crazyflie_server.py-2] [INFO] [1750763615.421947129] [crazyflie_server]: All Crazyflies parameters are initialized.
[crazyflie_server.py-2] [WARN] [1750763615.423984869] [crazyflie_server]: New publisher discovered on topic '/poses', offering incompatible QoS. No messages will be received from it. Last incompatible policy: DEADLINE
[crazyflie_server.py-2] [INFO] [1750763616.377151751] [crazyflie_server]: [cf04] is fully connected!
[crazyflie_server.py-2] [INFO] [1750763616.377784486] [crazyflie_server]: All Crazyflies are fully connected! It took 1.6600008010864258 seconds
[crazyflie_server.py-2] [INFO] [1750763649.270280163] [crazyflie_server]: [cf04] takeoff(height=1.0 m,duration=2.5 s,group_mask=0)
[crazyflie_server.py-2] [INFO] [1750763656.771163700] [crazyflie_server]: [cf04] land(height=0.03999999910593033 m,duration=2.5 s,group_mask=0)

The message about the incompatible QoS appears even after a full restart of the system.

fileversion: 3

robots:
  cf04:
    enabled: true
    uri: radio://0/80/2M/E7E7E7E704
    initial_position: [0.0, 0.0, 0.1]
    type: cf21  # see robot_types

robot_types:
  cf21:
    motion_capture:
      tracking: "vendor" # one of "vendor", "librigidbodytracker"
      marker: my_new_arrangement
      dynamics: default
    big_quad: false
    battery:
      voltage_warning: 3.8  # V
      voltage_critical: 3.7 # V

# global settings for all robots
all:
  firmware_logging:
    enabled: true
    default_topics:
      pose:
        frequency: 10 # Hz
      status:
        frequency: 1 # Hz
  firmware_params:
    commander:
      enHighLevel: 1
    stabilizer:
      estimator: 2 # 1: complementary, 2: kalman
      controller: 2 # 1: PID, 2: mellinger
    locSrv:
      extPosStdDev: 1e-3
      extQuatStdDev: 0.5e-1
  reference_frame: "mocap"
  broadcasts:
    num_repeats: 15 # number of times broadcast commands are repeated
    delay_between_repeats_ms: 1 # delay in milliseconds between individual repeats

/motion_capture_tracking:
  ros__parameters:
    # one of "optitrack", "optitrack_closed_source", "vicon", "qualisys", "nokov", "vrpn", "motionanalysis"
    type: "qualisys"
    # Specify the hostname or IP of the computer running the motion capture software
    hostname: "10.0.0.10"

    topics:
      frame_id: "mocap"
      poses:
        qos:
          mode: "sensor"
          # deadline: 100.0 # Hz
          deadline: 50.0 # Hz
      tf:
        child_frame_id: "{}_mocap"

Can this be a setup problem? If you have an idea of what is wrong, I would be very grateful.

[whoenig]

I think the server is hardcoded to a deadline of 100Hz, see https://github.com/IMRCLab/crazyswarm2/blob/a68652755af4e17735556643cf10e2d20c600fe3/crazyflie/scripts/crazyflie_server.py#L340. in motion_capture_tracking.yaml, change the deadline setting accordingly.

The issue you are seeing is that the server can't receive /poses and thus it doesn't send the qualisys information to the drone. The on-board Kalman filter detects divergence and resets itself every 25s.

[raphael-jms]

Hello whoenig, thank you a lot for you answer! The communication issue is solved now.
Unfortunately, the drone still immediately flips. I will investigate further and update if I find something.