Swarm Simulation Loses Flight Stability and Sinks When Scaling from 2 to 3 Drones

[Qassim69]

I am experiencing a severe flight stability issue when scaling a Crazyswarm2 simulation from 2 to 3 (or 4) drones in RViz. The system uses two custom logic scripts: one monitors the proximity of other drones and boundaries, and the other uses that data to calculate and send velocity commands via cmdVelocityWorld to repel them from collisions and drive them toward random points.
While a 2-drone setup correctly follows this velocity logic and maintains stable flight, adding a third drone causes the swarm to lose stability and sink to the floor immediately after the takeoff phase.

The simulation relies on the following Crazyswarm2 stack:

• src/crazyswarm2/crazyflie_py/crazyflie_py/crazyflie.py
• src/crazyswarm2/crazyflie_sim/crazyflie_sim/crazyflie_server.py
• src/crazyswarm2/crazyflie_sim/crazyflie_sim/crazyflie_sil.py
• src/crazyswarm2/crazyflie_sim/crazyflie_sim/backend/np.py
• src/crazyswarm2/crazyflie_sim/crazyflie_sim/visualization/rviz.py

Behaviour Observed with 2 Drones:
When simulating 2 drones, the simulation behaves properly in RViz. The drones successfully take off, navigate towards random points using cmdVelocityWorld, and properly repel from each other and the boundaries. Notably, with max_dt set to 0.001 for crazyflie_server.py, the CPU usage touches 95–97%, but the drones remain completely stable and execute the velocity logic perfectly.

Behaviour Observed with 3 Drones:
When simulating 3 or more drones, the initial takeoff phase succeeds (drones take off simultaneously and reach the desired altitude). However, the moment they are supposed to start moving toward the Random Point, they fail to follow the velocity logic. Instead, the drones start spinning on their z-axis (yaw instability) and eventually sink to the floor.

Troubleshooting & max_dt Testing:
To determine if the bottleneck was CPU exhaustion or physics backend timing, I tested several max_dt values in crazyflie_server.py (which triggers np.py and the simulation time). Note: omitting max_dt defaults it to 0.0, running as fast as the CPU allows.

• max_dt = 0.0005 and 0.001: CPU usage hits 100%. Drones take off but spin/sink upon transitioning to waypoint navigation.
• max_dt = 0.005 and 0.01: CPU usage for crazyflie_server.py successfully dropped. However, this did not fix the flight stability. The drones still spun and sank, it just delayed the time it took for them to fall.

I am struggling to identify exactly where the bottleneck lies when scaling to 3 drones, given that 2 drones fly perfectly even under high (97%) CPU load.

My primary suspicion revolves around the PID controller frequency:

1. Is the bottleneck in the PID controller (crazyflie_sil.py), the server (crazyflie_server.py), or the simulation backend?
2. My understanding is that the PID controllers require velocity values to be updated consistently at 1000Hz for stable flight. This is achievable if crazyflie_server.py fires at a max_dt of 0.001. Does increasing max_dt to 0.005 or 0.01 inherently break flight stability by delaying the timer and starving the PID controller of timely velocity updates?

would it possible for you to check my repository?
I have attached the video of 2 and 3 drones flying.

2.drones.mp4

3.Drones.mp4

[Qassim69]

I am implementing the APF logic:
APF_1_1_agent.py
APF_1_1_env.py

In order to use cmdVelocityWorld, I had done some changes in crazyflie_server.py:
crazyflie_server.py
crazyflie_sil.py

[knmcguire]

Hi Qassim,

Thanks for reporting this! I will make an issue to investigate this later.

Both me and @whoenig are very occupied in the next few weeks so we have limited availability to check this out. So if you are making any other observations in the mean time please update the issue.

[Qassim69]

Thank you for the feedback. As I'm working on my thesis, I was hoping to know where the issue could be in:
Control Logic: Specifically, the core PID controller constants.
Software Implementation: Potential bugs within crazyflie_server.py.
Physics Simulation: Inconsistencies in the np.py backend.
System Latency: Timing and synchronization issues between the server and the hardware.

Any insight into which of these areas is most likely the bottleneck would be incredibly helpful.

[knmcguire]

At this point I really need to see this on the unchanged crazyswarm2 repo to really say anthing about it. I'd rather should say I think that perhaps the timing on the firmware bindings of the controllers don't handle non-realtime simulations very well but I really need to look at this more closer.

[whoenig]

My guess is that this is a bug with the software-in-the-loop. Which controller are you using? Perhaps setting any state-based controller gains to 0 (i.e., I-gains) helps?

[Qassim69]

I'm using the PID Controller.
Where exactly do I make this state-based controller gains to 0 (i.e., I-gains) change?

I have tested my algorithm on the physical Crazyflie drones and I could make 5 drones fly and follow the velocity commands using cmdVelocityWorld.