Read data with the AP_RobotisServo.cpp code in arducopter

Hi everyone,

I am currently working on a project that implies to connect a pixhawk directly with an open manipulator, without passing by an Arduino card (except for powering it).
Up until now, I have managed to replicate the instruction given by the documentation on ardupilot :
After that, I downloaded the ardupilot code and modified the AP_RobotisServo.cpp to be able to control the arm with a 3 stance switch on an RC and predefined angular positions for the servos.

Now here is my problem :
I can control the arm just fine, but I fail to read the data returned by the servos. I wrote the function below to communicate with the arm, by sending a package with the read instruction. However, I don’t receive anything and I fail to understand why…

Here is the list of the things I already checked :

-The function is added into the AP_RobotisServo.h
-The packaged sent is correct, I checked with U2D2 and DynamixelWizard2.0

I think the problem comes from this step in the configuration function, which is supposed to disable the status return and let me read, but it doesn’t seem to let me read…

Capture d’écran du 2023-10-24 10-50-55

When I modify it or remove it, the package I receive isn’t the one I asked for and the arm doesn’t obey properly to the command I send.

I would greatly appreciate a little help from the developer @Andrew_Tridgell

Thanks a lot in advance