Fixed priority scheduler code to not breka things.

Priority specific tests, except for alarm-priority, still fail.