Custom firmware with range sensors support on a drone and avoid obstacles (APM Ardupilot 2.6)