Init hardware function ported from pixHawk Code