fix early return in critical section entry
fix early return in critical section entry

madcowswe/ODrive,madcowswe/ODrive,madcowswe/ODriveFirmware,madcowswe/ODrive,madcowswe/ODrive,madcowswe/ODriveFirmware,madcowswe/ODriveFirmware,madcowswe/ODrive,madcowswe/ODriveFirmware