SITL for testing of custom code base

Hello Everyone,

I want to test my custom code ( involves using of filters,controllers, and generating trajectories). I want drone on the simulator to behave according to controller outputs from my piece of code. So, after I flcik in guided, the code generates a trajectory between two points, and my aircraft should follow that. How do I begin so that the drone behaves accordingly in SITL.
So, basically my aim is test the logic of my code and test how my drone will be expected to behave in real life scenario without physical testing it to avoid crashes and save time.

What would be the best way to approach this problem statement on high-level?

Regards,
Prarthana Sigedar