Implementing my own control loops in the source code

Hello,
I am in the process of implementing a custom flight mode to the PX4 firmware as part of my master’s thesis. In short, I will be creating a flight mode called ‘Position 2’ that will use my own control loops for velocity and attitude control and use PX4’s position and rate controllers for the quadcopter.
I already tried UAV Toolbox for Simulink and successfully flew SITL with my autopilot; however, that approach gets rid of pre-flight checks, sensor calibration and safety features, which will be essential when I upload the code to the real drone.
I am having difficulty adding another flight mode to the source code as existing tutorials are outdated. Has anybody succeeded on doing that and by change can share any resources that I may have missed?

Thanks.

Try sharing your code, maybe someone from the development team will have a look.
I’m also interested in looking at it but I’m not part of said team so I can’t really add something of value.