Question about modifying the current control structure

Hi, all.
I am currently modifying the existing low-level control structure of Arducopter and trying to implement my own control structure to fit with the rest of the modules.

Basically what I have is a simple 1-level controller which contains four PID controllers for the altitude and the three angles phi, theta and psi corresponding to roll, pitch and yaw. Then, besides the PID controllers, there is a lookuptable connected to the four controller outputs and some matrix calculations are conducted. After that the final control signals (required overall thrust and and torques for roll, pitch and yaw) are converted into PWM signals according to a quadratic relation which I have acquired thru calibration for my built drone and fed into the ESCs.

According to the developer’s guide and some skimming through the source files, I found that in the Ardupilot, the altitude controller is regarded as a higher level control and put into the position control level, and that the control outputs of the altitude controller is directly sent to throttle (so actuator).

So what I am wondering is that, whether there is a way to hold the control output of the altitude controller and fuse it with the outputs of the attitude controllers so that the matrix calculations could be done on the four controller outputs and the lookup table data?

Also, implementing the thrust/torque to PWM conversion would be helpful in my case for a more accurate model, yet I do not really understand how this is done in Ardupilot as to cope with different drone models, and where should I look into if I want to modify it.