Added compensation for heading into the write command, more robust heading calcualtion