PX4 SITL with Simulink based Plant Model

I'm trying to perform SITL with my custom controller with PX4 toolchain for trajectory tracking for quadcopter.I designed the controller with the MIL(model-in-the-loop) taking into account this link as reference:https://uk.mathworks.com/help/supportpkg/px4/ref/simulator-plant-model-example.html. The controller I designed in MIL works(no code generation everything done in simulink). So I moved to SITL(on PX4 host target) on Simulink based plant model. I press "monitor and tune", wait untill the controller is compiled and it says to run(with pacing active on 1) the model plant, so I did, but it doesn't work. I noticed that the controller and the palnt are running at complitly different times. The drone goes crazy. Is this the problem maybe? Should I syncronize the plant and the controller?

I tried also jmavsim simulator, but it does not work either.

How is possible that the MIl works and the SITL doesn't?

Answer 1

[Arun Mathamkode]

Depends on when the plant and controller start running there can be an offset between controller and plant. And as per the current design plant will run two times for each controller loop, hence typically plant simulation time will be two times controller simulation time plus additional offset. I dont think this might be the reson behind the issue you are facing. There might be some issues in the plant that is getting exposed with your controller design. I recommend you to give a try with the plant model used for HITL (example link) which is more robust. You need to replace the serial based MAVLink Bridge Source and Sink blocks in this model with TCP block to establish connection with PX4 SITL.

To answer your question on why it worked in MIL, I need to know how did you run the MIL. Did you run the MIL with the same plant you are using for SITL? PX4 have multiple module like navigator commander estimator and its only a part is designed in Simulink so technically just with MIL, you cant really verify how the controller runs with other modules in PX4. in SITL along with the PX4 desined controller other modules also will be runnign and they will interact and its lot different from MIL.

[Salvatore Matteo Mennea]

Thank you for your reply. I'm running MIL and SITL with the same plant I found on the MIL demo.(https://uk.mathworks.com/help/supportpkg/px4/ref/simulator-plant-model-example.html). The only thing I've changed is the order of the euler angles.The vector in the demo is [yaw,pitch,roll] and I've change it with the standard one [roll,pitch,yaw] but I don't think is a problem also becouse in MIl it works.

I have a doubt. I need to feed back not only the position,velocities,acceleration and euler angles but also the jerk(acceleration derivative) and the euler angles derivative. So I'm doing a discrite derivative with the acceleration that I read from the vehicle_local_position message and the derivative of the euler angles that I get from the vehicle_attitude after converting them from quaternions to euler angles. Is this ok?

This is the feedback block.

This is the controller. The inner loop creates the references phi and theta and controls the altitude, while the inner is the attitude controller.