last_letter + APM SITL network diagram on synthetic clock

[GeorgeZP]

Hello Tridge and everyone else interested in the topic,

This post describes the efforts to make the last_letter + SITL simulation solution as accurate as possible, by providing a synthetic clock and a regulation system for the simulation runtime.

The main, defining parameters are the /world/simRate, /world/deltat and /world/timeControls. Let's go ahead and explain what each one does before we go into the meat of the discussion.

• simRate applies only when timeControls=0. This is the update rate at which the last_letter simulator runs.
• deltat is the step at which the simulation time advances every simulation step.

Thus, if simRate = 500 and deltat=0.002, the simulation runs at 500fps and at real-time.
If simRate = 500 and deltat=0.001, the simulation runs at 500fps and at x0.5 speed.
If simRate = 1000 and deltat=0.004, the simulation runs at 500fps and at x2 speed.

• timeControls selects between the two schemes below, which will be described below.
  

Right now, the SITL + last_letter solution works with the first scheme:

1. The node Simulation clock triggers off of the Wallclock at the specified simRate and in-turn triggers the modelPlane node (the physics simulation)
2. modelPlane produces a simulation state (ie fdm packet content) and sends it to the fdmUDPSend node, which stores it. modelPlane uses the last known control input while performing calculations.
3. fdmUDPSend polls for a SITL control output by the APM SITL at a rate equal to simRate governed by the simulated clock. If a controls packet is received, then it sends an fdm packet packet back to the APM SITL. Also, it interprets the SITL ctrl output and publishes it to the ctrlPWM topic for the simulation to use.
   

In this way, the simulation rate is exactly equal to the predefined simRate (as long as the processing power of the system allows it). However, there is no guarantee that the fdmUDPSend node will consume SITL ctrl output as fast as it is produced, nor that the APM SITL will be able to produce SITL ctrl output / request fdm packet as fast as the simulation simRate.
This may result in the simulation producing multiple steps/deltat's before the APM SITL captures the fdm packet, thus missing simulation information.
With this scheme, I have successfully done a SITL, piloted the aircraft using a joystick via the MAVProxy joystick module. Simulation would run at the nominal 500Hz solid.


When using timeControls=1, things work a bit differently.

1. The Simulation clock triggers off of the ctrlPWM topic. Each time a new ctrlPWM message is published, the Simulation clock advances by deltat. This triggers the modelPlane node as usual.
2. This step is identical to the previous case. modelPlane produces a new simulation state and sends it to the fdmUDPSend which stores it.
3. Once again, this step is identical to the previous case. fdmUDPSend polls for SITL ctrl output and when it receives one, it communicates the simulation state back to the APM SITL.

This structure guarantees that the simulation will not step unless it receives a SITL ctrl output. Exactly one simulation step will happen when exactly one SITL ctrl output produced. Essentially, the APM controller governs the simulation rate.
However, this creates a chicken-and-egg problem: APM SITL waits for an fdm packet, while last_letter wait for a SITL ctrl output. This deadlock is broken by the new Packet injector node (ctrlInjector.cpp). This node listens to the ctrlPWM topic and if more than 1 wallclock second passes without a message being published, then it publishes a dummy one, to break the deadlock. This can be used to kick-start the communication circle.

I have managed to make SITL run with this scheme as well, albeit with a much lower rate of 380-480Hz (out of 500Hz nominal). This is unacceptably low, and the reason why the loop runs that much lower should be investigated.
Moreover, it should be verified that the APM SITL actually produces SITL ctrl output packet constantly, even during initialization, in order to maintain the loop rate and that it also sends exactly 1 SITL ctrl output for each fdm packet it receives.

Thank you for your attention,
George

[Andrew Tridgell]

Hi George,

Thanks for the detailed explanation. One extra bit may be useful:

> However, this creates a chicken-and-egg problem: /APM SITL /waits for an
> /fdm packet/, while /last_letter /wait for a /SITL ctrl output./ This
> deadlock is broken by the new /Packet injector /node (ctrlInjector.cpp).
> This node listens to the /ctrlPWM /topic and if more than 1 *wallclock
> *second passes without a message being published, then it publishes a

> dummy one, to break the deadlock. This can be used to kick-start the
> communication circle.

ArduPilot has a similar deadlock breaking mechanism on its side:

https://github.com/diydrones/ardupilot/blob/master/libraries/SITL/SIM_last_letter.cpp#L98

that is the code that is waiting for a FDM packet (sensor data,
simulation state etc) from last_letter. Notice that it waits for at most
0.1 seconds. If a new FDM packet isn't received in that time then it
re-sends a control packet to last_letter. That allows it to cope with
the possibility of lost UDP packets.

So last_letter shouldn't need any deadlock breaking code of its own, as
this is already builtin.

The reason for doing this on the ArduPilot side is that it will be quite
common for developers to be using last_letter for debugging, where they
attach gdb to ArduPlane.elf while it is flying with last_letter. With
gdb running there will be long pauses in the simulation while the
developer is inspecting variables etc, and we just need last_letter to
patiently wait for the next control input.

We have similar deadlock breaking code with each of the simulator
interfaces to cope with this problem for each of them.

Cheers, Tridge

[GeorgeZP]

Hello Tridge,

I just tried again to launch SILT+l_l with timeControls=1, with the
ctrlInjector node disabled.
The simulation would not kick-start, no matter how long I waited.
After I manually inserterd some /ctrlPWM messages with `rostopic echo
/skywalker_2013/ctrlPWM ...` the simulation kickstarted, physics took
over and APM initialized. Something doesn't work as expected, obviously.
We'll need to see whose end is the problem at.

Your comment on the debugers though is very valid. I will withdraw the
packet injector on my end.

Today the whole setup (APM+l_l) with timeControls=1 ran at 480Hz. Still
sub-optimal.
Does SITL adjust its time-step if the simulation is lagging, with the
speedup/down utilities?

Good night,
George