AP_HAL_SMACCM: Implement new scheduler methods.

- Set the system initialized after call to "setup" in main.
AP_Baro_MS5611: Don't panic if taking semaphore fails during init.

- The MPU6000 holds on to the I2C semaphore for quite some time during
  init, which caused a panic when the MS5611 is also on I2C.