Change the flight control

Hi guys,
i’m a PhD student in aerial robotics and i work with omnidirectional drones.
I’m trying to modify the PX4 firmware in order to implement a new flight controller.
I’ve changed the equations in the “update” functions in “mc_pos_controller”, “mc_att_controller” and “mc_rate_controller” files. Now i need to also to control 4 servo motors and i’m stuck with that.

It is really difficult to modify the Firmware controller and i didn’t find any guide/tutorial.
Can you give me some advice please? have you ever have implemented a new controller in the firmware?

Thanks

No one has changed the flight control?