Fixed bug when forming data packet to be sent