MPU9250 - Serial senor read

Greetings to the team. So my main question has to do with the following task. I am trying to read the sensor data in a serial monitor and try to calibrate that in (e.g. MATLAB). Right now I am reading only raw values. At this point my code looks like the following:

#include <Wire.h>

#define MPU9250_ADDRESS  0x68 
#define AK8963_ADDRESS   0x0C
#define AK8963_XOUT_H    0x04
#define PWR_MGMT_1       0x6B
#define GYRO_CONFIG      0x1B
#define ACCEL_CONFIG     0x1C
#define AK8963_ADDRESS   0x0C
#define AK8963_CNTL      0x0A
#define AK8963_XOUT_H    0x04
#define INT_PIN_CFG      0x37

float ax, ay, az;
float gx, gy, gz;
float mx, my, mz;
int mode = -1;
void setup()
{
//Initialize MPU9250 write in power mgmt1
  Serial.begin(500000);
  Wire.begin();                      // Initialize comunication
  Wire.beginTransmission(MPU9250_ADDRESS);       // Start communication with MPU6050 // MPU=0x68
  Wire.write(0x6B);                  // Talk to the register 6B
  Wire.write(0x00);                  // Make reset - place a 0 into the 6B register
  Wire.endTransmission(true);  
  delay(100);

//Get stable time source 
  Wire.beginTransmission(MPU9250_ADDRESS);
  Wire.write(PWR_MGMT_1);
  Wire.write(0x01);                  //Auto select clock source to be PLL gyroscope 
  Wire.endTransmission(true);
  delay(200);    

//Set bypass mode in MPU9250
  Wire.beginTransmission(MPU9250_ADDRESS);
  Wire.write(INT_PIN_CFG);
  Wire.write(0x02);                  //Auto select clock source to be PLL gyroscope
  Wire.endTransmission(true); 
  delay(20);

//Configure Accelerometer Sensitivity - Full scale range( +/- 8G)
  Wire.beginTransmission(MPU9250_ADDRESS);
  Wire.write(ACCEL_CONFIG);                  //Talk to the ACCEL_CONFIG register (1C hex)
  Wire.write(0x10);                  //Set the register bits as 00010000 (+/- 8g full scale range)
  Wire.endTransmission(true);
  delay(20);
  
// Configure Gyro Sensitivity - Full Scale Range ( +/- 1000deg/s)
  Wire.beginTransmission(MPU9250_ADDRESS);
  Wire.write(GYRO_CONFIG);                   // Talk to the GYRO_CONFIG register (1B hex)
  Wire.write(0x10);                   // Set the register bits as 00010000 (1000deg/s full scale)
  Wire.endTransmission(true);
  delay(20);

// Configure Magnetometer for continuous readings at 16-bit
  Wire.beginTransmission(AK8963_ADDRESS);
  Wire.write(AK8963_CNTL);
  Wire.write(0x00);
  Wire.endTransmission(true);
  delay(10);
  Wire.beginTransmission(AK8963_ADDRESS);
  Wire.write(AK8963_CNTL);
  Wire.write(0x16);                   // Or set it to 0x12 for 8Hz refresh rate
  Wire.endTransmission(true);
  delay(10);
}

void loop()
{
  if(Serial.available()>0)
  {
    mode = Serial.read();
     if(mode == 'R')
    { 
// Read accelerometer and gyroscope data
  uint8_t Buf[14];
  I2Cread(MPU9250_ADDRESS,0x3B,14,Buf);
// Create 16 bits values from 8 bits data

// Accelerometer
   ax = ((Buf[0]<<8) | Buf[1])*(8.0/32768.0);
   ay = ((Buf[2]<<8) | Buf[3])*(8.0/32768.0);
   az = ((Buf[4]<<8) | Buf[5])*(8.0/32768.0);
  
// Gyroscope
   gx = ((Buf[8] <<8) | Buf[9])*(1000.0/32768.0);
   gy = ((Buf[10]<<8) | Buf[11])*(1000.0/32768.0);
   gz = ((Buf[12]<<8) | Buf[13])*(1000.0/32768.0);
  
// Display values

// Accelerometer
  Serial.print(ax,6);
  Serial.print("\t");
  Serial.print (ay,6);
  Serial.print("\t");
  Serial.print(az,6);
  Serial.print("\t");
// Gyroscope
  Serial.print(gx,6);
  Serial.print("\t");
  Serial.print (gy,6);
  Serial.print("\t");
  Serial.print(gz,6); 
  Serial.print("\t");
// _____________________
// ::: Magnetometer :::

// Read register Status 1 and wait for the DRDY: Data Ready  

  uint8_t ST1;
do
{
  I2Cread(AK8963_ADDRESS,0x02,1,&ST1);
}
  while (!(ST1&0x01));
// Read magnetometer data
  uint8_t Mag[7];
  I2Cread(AK8963_ADDRESS,0x03,7,Mag);
  
// Create 16 bits values from 8 bits data

// Magnetometer
    mx = ((Mag[3]<<8) | Mag[2])*(10.*4912./32760.0);
    my = ((Mag[1]<<8) | Mag[0])*(10.*4912./32760.0);
    mz = ((Mag[5]<<8) | Mag[4])*(10.*4912./32760.0);

// Magnetometer
 Serial.print(mx,6);
 Serial.print("\t"); 
 Serial.print(my,6);
 Serial.print("\t");
 Serial.println(mz,6);
 delay(10);
}
 }
}


void I2Cread(uint8_t Address, uint8_t Register, uint8_t Nbytes, uint8_t* Data)
{
// Set register address
Wire.beginTransmission(Address);
Wire.write(Register);
Wire.endTransmission(false);
uint8_t index=0;
Wire.requestFrom(Address, Nbytes);
while (Wire.available()){
Data[index++]=Wire.read();
} 
 }

I didn’t want yo use any already existing libraries regarding that sensor (because most if not all of them they include sensor calibration , DMP, FIFO and all these registers activated). I kept that simple.
So at this point my main idea is the following. I want to send some type of special character while the serial s active. When the code sees that character it willl perform a specific task. There are two options to achieve that:
** Firstly I can send all data into the serial port and then using the other program, I should be into the position of reading only specific columns.

**Secondly, as I described above I can send a special character and have a specific loop section present only these data. Lets say for example. ‘R’: will loop and present only accelerometer data.
‘L’: will loop and present only gyroscppic data etc…

Trying to follow that way, I have the following problem. When I send (in my posted code ‘R’), a specific character I read only one line of data. If I make the following change:

while (mode=='R')
{
}

Then I am in position of reading continuously but following the ‘while’ way I cannot include more “modes” in my main loop.

--------------------------------------------- SECOND QUESTION ----------------------------------------

That has to do with the serial data representation. Following the picture below, you can see that structured data do not follow the same format. When i move the sensor fast enough I keep getting these white areas and results fall out of their predefined order.

Please if you have any suggestions reply. Enything would be much appreciated.
Ssorry for any mistakes in my post. This is my first one.
Thanks in advance,
Best wishes,
Koutsoukos Theodosis

Screenshot_1.jpg