Fixed a bug and added some cosmetic changes to the debug output.

The codes works perfectly with pre-recorded data, need further work with actual radio…