Working on HIL. Bugs in init fixed.