Out put yaw pitch roll command to quick test new drone design.

[ZhangBo-China]

To put it simply, I want to output yaw pitch roll command that solved by flight-controller with serial ports. In fact, I am working on making a new kind of drone, and I have packed up the yaw pitch roll command adapt to my drone. I want to test my design quickly, but I am not familiar with flight-controller.
For example, I want the drone maintain self-stability, but there is a disturbance, and the drone begin to spin right, flight-controller sense the spin, sent a command to motion-solver to make the drone spin left, and the command sent to the motion-solver is what I want.

Tom's vedio that iI whated is https://www.youtube.com/watch?v=Y31BhQToh_U (at about 3:20), in this vedio, He say that he do it with INAV, and I wonder how.

I am familiar with STM32, and I am using a board with F407VE to packed up the yaw pitch roll command adapting to my drone that I mentioned. I also have a hobbywing-XRotor-FlightController-F4-G3.
Thanks guys!

Thanks!

[DzikuVx]

All that data is available via the MSP protocol already