added median filter and tilt compensated sonar

created a general median filter using bubble sort and a maximum of 7
elements to sort through. Also enabled -Os optimisations as code no
longer fits 64k flash