Add the secondary Heading PID

[kevinfrei]

@Kooolpool this one you could do at Colin's place.

The idea is that you have a cutoff, so the first PID gets you "close enough" and then the secondary PID can be super well tuned for being close to the target heading, but not quite there.