Thanks so much for replying.
But I have some questions for your controller.
(1) do you publish this throttle to the mavros topic ~setpoint_raw/target_attitude as its thrust？
（2） if we say a_cmd is the required thrust 6N, what is the a_curr?
(3) why do you put a throttle in the right side of the equation?
(4) the thrust for the setpoint_raw/target_attitude should be in (0~1), do you saturate it later?