fixed fairly large buffer overrun

[markwilkie]

Found this bug while chasing down crashes in my own fork. Thanks a ton for writing this, it's great stuff and I've got my ArduinoBLE version working.

Cheers