Oscillating yaw in copter swarm

[SebastianK123]

Issue details

When using the swarm feature, the follower copter performs rapid yaw oscillation. These oscillations seem to be between the initial heading when entering guided mode and the current heading of the leader.

For example, if both leader and follower begin in the same orientation and the swarming feature is started, the behaviour is as expected. Translating the leaders location using pitch and roll leads to the expected behaviour from the follower. If you then change the heading of the leader, the follower moves to its correct position but begins oscillating its yaw between the original heading and the leaders current heading. This was observed in flight testing.

This behaviour can be replicated in SITL.

I've been having a dig around in the mission planner swarm code, and it appears to be a conflict between the setPositionTargetGlobalInt command and MAV_CMD.CONDITION_YAW. Originally it looks like the setPositionTargetGloballnt had the YAW bitmask disabled, yet somehow this was still causing a conflict( This is the part that confuses me). Adding the leaders yaw into this command and enabling the YAW bitmask as well as removing the MAV_CMD.CONDITION_YAW seems to correct the behaviour( see the attached picture)

I'd be happy to contribute this as a fix, but might need some guidance as I haven't contributed before. These changes are currently in my own fork.

Version

Mission planner 1.3.80 build 1.2.8574.30089 Copter V4.5(dev) -- (used in SITL) Copter V4.3 -- observed in hardware

Platform

[ ] All [ ] AntennaTracker [1] Copter [ ] Plane [ ] Rover [ ] Sub

Airframe type

Copter v4.3 - Y6b Copter v4.5 - generic SITL quad

Hardware type

Cube orange

Logs