PEEK: Sensors improvements

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

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