I am working on a drone and I am using a raspberry pi 3b+ and Navio2 as the intelligence boards. My idea is to have a toggle switch on the RC transmitter toggle the drone between manual and autonomous flight modes. Now in autonomous mode, the drone will maintain “x” meters away from a wall and it will do this using a LiDAR (RPLIDAR A1) while following a predefined flight path which is up, right, down and up again as seen in the image. What I would like to ask from the community is, what advice can you all give me? Like where should I start? Where do I include the lidar configuration in the Ardupilot architecture? Can I even define the path outlined in the image in the ardupilot software architecture? Is my idea even possible with the current board that I am using? I have the ardupilot .cpp files built with waf outlined in https://ardupilot.org/dev/docs/building-for-navio2-on-rpi3.html. Just wondering what would be the best approach in diving into the .cpp files and adding new lines of code into them to complete my desired objective.