Fix for four-rotor testing, and readded stuff I accidentally deleted.
Stupid hands.