changed status position check and moved sensor readings out of interrupt
Magic code.
Merge pull request #3 from vlantaca/bardia_branch

Working code