Fixing issue #24

Hi

I fixed issue #24 but I've only tested on my SGSI9000 and It's working fine. Please review the changes.

Thanks