Mavlink command sender race condition

[RomanBapst]

While working with camera trigger via mavlink I noticed that there is a race condition occurring between mavlink instances.

When a command from PX4 needs to be sent out each mavlink instance sends the command to it's destination and then tries to add an item into the queue of the "MavlinkCommandSender".
Even though multiple instances try to insert the command into the queue, it will only be inserted once as the logic checks if the timestamp already exists.
A command item is removed from the queue as soon as the corresponding command is acknowledged by the target system which communicates through one of the mavlink instances.
If a command item remains in the queue for longer than 500ms the corresponding mavlink command is resent on ALL mavlink instances.

The race condition takes place if an acknowledge messages is received before all other mavlink instance tried to put the corresponding command into the sending queue (via MavlinkCommandSender). In that case the same command will be put into the queue again. That itself is not a problem, however, if the command is not acknowledged on those instances then the command will be re-sent after the timeout has passed to ALL other instances (including the one which already got the ack).

I have reproduced the problem as follows:
Enable camera triggering via mavlink:
TRIG_MODE 2 -> time based triggering
TRIG_INTERVAL 2000 -> send trigger every 2000ms
TRIG_INTERFACE 3 -> send trigger message via mavlink

Enable Telem2:
MAV_1_MODE -> telem2

Power the vehicle via usb or make sure that there is a mavlink instance running via usb.
This is important because we need at least two mavlink instances which send camera trigger messages.
Make sure that there is something connected to telem2 which acknowledges the camera trigger commands.
Make sure that there is nothing connected to the usb mavlink instance which acknowledges the camera trigger commands.

You will notice that the trigger messages received on TELEM2 do not come in regular intervals.

[julianoes]

Could we fix that race condition with some sort of lock?