Implementing a new position and attitude controller

Hi everyone,
I am new to the community. As part of my master thesis, I am using a quadcopter and I am developing a new controller based on the sliding mode theory. I have already designed the controller using MATLAB and now I would like to test it on a real drone with a simple test flight. My controller takes as inputs position and speed in the six degrees of freedom, the desired trajectory and outputs the forces that the propellers should generate. Can someone help me and tell me which is the simplest way to do that? I was thinking to substitute the controller in the firmware my own code, but I do not know how to send commands to the four rotors.
Thank you

Try reading the arducopter documentation on how to add a new flight mode.

Probably it will be more complicated that that, but it is a good starting point.