PEEK: Sensors improvements

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

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