The control loop has been implemented in Simulink. The physics and attitude controller are yet to be finished.The total required thrust comes from two sources: the thrust from the position controller and the thrust required for the attitude controller. The following image shows the control loop in Simulink.