Embedded code don't run with the desired sample time

Hello to all;

I'm trying to make a controller to run in TI launchpad at 500hz or 0.002s of sample time. The program is very simple, the most complex part is to call the information of an IMU by I2C and use that values to calculate some angles, PID's and finally PWM outputs.

To measure the exact sample time I integrate in the S-function of the IMU a micro () code to measure the exact time between cycles, and the minimum sample time was 0.004, the double of the required. After configuring the model with the objectives of execution efficiency and different fundamental sample time of the model from 1e-5 to 0.002, the best result that I can obtain is 0.0035s.

I tried with the Embedded Code Quick Start, and after the configuration with the recommended values the sample time begin to change erratically from 0.0003 (10 times lower) to 4085s, and the program collapse after some time. I need to obtain this sample time, but I can't find the way to configure correctly the Code generation. Can you help me to configure them?

Thanks