Use constrain on radio readings
Very simple mean filter
First try at auto-leveling