ardupilot icon indicating copy to clipboard operation
ardupilot copied to clipboard

Ensure stable IMU timing on RealFlight

Open andyp1per opened this issue 8 months ago • 8 comments

The current RealFlight approach means that we get highly variable loops and inter-loop dt's. This seems to significantly impact attitude control under arduous conditions. This PR generates high rate synthetic gyro samples that are sync'ed to the main loop's requests for samples. This results in very even loop dts whilst still ensuring that the latest data is always being used and appears to significantly help with attitude control.

Here is the updated, technically robust description. It now accounts for both the transport jitter (variable arrival time) and the variable decimation (skipped frames/variable physics steps).


The Core Argument: This PR upgrades the SIM_FlightAxis backend from a "sampling" model to an "integrating" model. It resolves the critical instability caused by RealFlight’s variable frame decimation and transport jitter, where the simulator might run at 1200Hz but deliver frames irregularly (e.g., ~600Hz, skipping intermediate steps).

1. The Problem: Variable Decimation & Time Aliasing

The interface faces two distinct challenges that the legacy fixed-schedule IMU could not handle:

  • Irregular Decimation: RealFlight updates physics at high rates (~1200Hz) but drops frames non-deterministically before sending. Sometimes we receive adjacent frames; other times we skip intermediates.
  • The Failure Mode: The old fixed-rate IMU timer was "blind" to these skips. If a frame was skipped, the state would jump significantly between updates. The fixed-rate IMU would interpret this jump as an instantaneous high-G event or snap rotation, feeding massive noise into the EKF and control loops.

2. The Solution: Adaptive Upsampling (Elastic Time)

The PR shifts to an event-driven model that reconstructs the missing data:

  • Slave to the Frame: We wait for a frame to arrive (wait_for_sample).
  • Delta-Based Interpolation: Instead of assuming a constant timestep, the code calculates the true simulation dt between the current frame and the last received frame.
    • If we receive adjacent frames, it generates a standard batch of synthetic samples.
    • If frames were skipped (larger dt), it automatically scales the number of synthetic samples to fill that larger gap smoothly.
  • Ignoring Arrival Jitter: By relying on the simulation time delta rather than the wall clock arrival time, we effectively filter out the OS/Network latency jitter.

3. The Engineering Win

This mechanism acts as a smart interpolation layer.

  • It smooths over the "stutter" of skipped frames by generating a continuous stream of IMU data that matches the average velocity over the gap.
  • It ensures the Autopilot always sees a coherent timeline, regardless of whether the simulator sent every frame or every other frame.

Summary: We cannot control RealFlight's output variability, but we can control how we digest it. This PR allows ArduPilot to "ride" the variable stream of data without aliasing, ensuring the EKF receives a smooth, physically accurate timeline essential for precision tuning.

andyp1per avatar Apr 29 '25 07:04 andyp1per

I have been running this commit for a couple of weeks, while overall it's a big improvement, especially in challenging flight scenarios, I suspect 100us samples are a bit too fast, especially when running SITL and Realflight on separate computers. We're seeing some interesting networking issues, especially on startup of SITL. Reducing to #define SAMPLE_INTERVAL_S 1.0e-3 has made a pretty big improvement, without really seeing any downsides (yet). Possibly something in between is ideal

lbullard-c2 avatar Jun 10 '25 03:06 lbullard-c2

Thanks for testing @lbullard-c2 ! It probably works well on my machine because it is using RF and SITL co-located and it is a Ryzen 9 9950X with an RTX 5080OC graphics card - so on the limits of consumer hardware right now. I agree there is probably an ideal value, although in theory the sample interval should not affect the network performance, since the network round trips are going at whatever rate RF gives us and we are just subdividing to give even timing. 1e-3 has the disadvantage of being quite close to the other sampling intervals going on and I suspect there is therefore a decent amount of jitter. Maybe 5e-4 would be better as that is quite representative of the real world with gyros running at 2KHz.

andyp1per avatar Jun 10 '25 07:06 andyp1per

I'd say my particular network environment might be the culprit, SITL is running on a server with a few enterprise switches / firewalls etc in between that probably don't like the traffic

lbullard-c2 avatar Jun 11 '25 00:06 lbullard-c2

Shoot, this does break the Cygwin SITL somehow. It freezes up right after linking up with RealFlight. I'll need to see if I can get to the bottom of it. This is important because this is how people run SITL from Mission Planner.

image

Update: Interesting, I left it running for an hour and it must have briefly unfrozen a couple times. image

robertlong13 avatar Jun 13 '25 04:06 robertlong13

Shoot, this does break the Cygwin SITL somehow. It freezes up right after linking up with RealFlight. I'll need to see if I can get to the bottom of it. This is important because this is how people run SITL from Mission Planner.

image

Update: Interesting, I left it running for an hour and it must have briefly unfrozen a couple times. image

It has to be something to do with the socket handling IMO, be good to get to the bottom of this!

andyp1per avatar Jun 13 '25 08:06 andyp1per

Ok rebased on Peter's change that was responsible for the slowdown and now everything looks good

andyp1per avatar Nov 25 '25 14:11 andyp1per