SITL with Simulink

Hi everyone!
I have assembled a simulink model to simulate a quadcopter, and built my own Mavlink TX/RX blocks to communicate with the PX4 Firmware in SITL. The version of the Firmware is 1.11.
But I cannot get it to work. Somehow, when the vehicle rotates, the yaw estimation extracted from the logs goes crazy. I have checked that all the inputs and their units to sensors when building the Mavlink message are correct by checking the sensors data logged.
Any one have any idea of what could be happening?
Thanks in advance!

Hi. In case anyone had the same problem, I could solve the issue using px4 firmware v 1.9. In v1.11 there is a huge jump in yaw value after a few seconds from take-off