Hi luisrodenas I have a question : Do I use the same values for whatever full scale I'm using at the gyro and accel ? What I mean is, if I have MPU9255_GYRO_FULL_SCALE_2000DPS or MPU9255_GYRO_FULL_SCALE_250DPS, the function works without changing anything ? I ask that since values oscilate within different scales at least for the Accelerometer.
Also I have a suggestion : When you do the average, you are adding all the samples and dividing by the number of samples at the end. That may lead to overflows easy if the sample number is too large. What I suggest is why not take a first read, and then, inside the loop, take another read, add it to the first and divide by 2, that result adds to another read and divides by 2 so on and so forth, that way you don't get to higher numbers to operate with.
for (int n=0;n<samples;n++)
a >> 1 ;