flight: Control refactored to deal with pitchroll, yaw sp directly