Added missing locks to some i2c accesses. Changed command execution to run in just one i2c bus access.