Fixed an issue where the data used was from the serial buffer rather then real time. This also fixed a mem-leak issue. Bug was found using the RaspberryPi.