PEEK: Sensors improvements

- Bring some fixes to the algorithm thanks to @D4rKn3sSyS
- Cleanup

Signed-off-by: Carlo Savignano <stevewatersy@gmail.com>
Change-Id: I5c55fd5136f915d4f7c49006030ea019e3c02fb8