........... Sensor Reading Source Code .............

========= File-Name: 11_Sensor_Read_Stuff.ino ===========

//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
//+++++++++++++ Begin Sensor Reading functions ++++++++++++++++
//                Last Update: 24-12-2021  
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++

void readMPU6050(short * result)
{
  //Note: DUE and Teensy is int 4 byes, and short is 2 bytes
  int regAddress = 0x3B;    //first axis-acceleration-data register on the MPU6050
  byte howManyBytesToRead = 14;   //uint8_t means: give me an unsigned int of exactly 8 bits.
  byte _buff[14];
  short readings[7];  //short is 16bits=2bytes
  readFrom(MPU_addr, regAddress, howManyBytesToRead, _buff); //read the acceleration data from the ADXL345

  // each axis reading comes in 10 bit resolution, ie 2 bytes.  Least Significat Byte first!!
  // thus we are converting both bytes in to one int
  readings[0] = ((_buff[0]) << 8) | _buff[1];   // 0x3B (ACCEL_XOUT_H) & 0x3C (ACCEL_XOUT_L)
  readings[1] = ((_buff[2]) << 8) | _buff[3];   // 0x3D (ACCEL_YOUT_H) & 0x3E (ACCEL_YOUT_L)
  readings[2] = ((_buff[4]) << 8) | _buff[5];   // 0x3F (ACCEL_ZOUT_H) & 0x40 (ACCEL_ZOUT_L)

  readings[3] = ((_buff[6]) << 8) | _buff[7];   // 0x41 (TEMP_OUT_H) & 0x42 (TEMP_OUT_L)

  readings[4] = ((_buff[8]) << 8) | _buff[9];   // 0x43 (GYRO_XOUT_H) & 0x44 (GYRO_XOUT_L)
  readings[5] = ((_buff[10]) << 8) | _buff[11]; // 0x45 (GYRO_YOUT_H) & 0x46 (GYRO_YOUT_L)
  readings[6] = ((_buff[12]) << 8) | _buff[13]; // 0x47 (GYRO_ZOUT_H) & 0x48 (GYRO_ZOUT_L)
  delay(3);

//All results[i] in bit-values
  result[0] = -1*readings[0];  //Acc-X   Minus required to get the g-sign right.
  result[1] = -1*readings[1];  //Acc-Y   Minus required to get the g-sign right.
  result[2] = -1*readings[2];  //Acc-Z   Minus required to get the g-sign right.
  result[3] =    readings[3];  //temp
  result[4] =    readings[4];  //gyro-X
  result[5] =    readings[5];  //gyro-Y
  result[6] =    readings[6];  //gyro-Z

/*  
  //All results[i] in bit-values
  result[0] = -(int)readings[0];  //Acc-X   Minus required to get the g-sign right.
  result[1] = -(int)readings[1];  //Acc-Y   Minus required to get the g-sign right.
  result[2] = -(int)readings[2];  //Acc-Z   Minus required to get the g-sign right.
  result[3] = (int)readings[3];  //temp
  result[4] = (int)readings[4];  //gyro-X
  result[5] = (int)readings[5];  //gyro-Y
  result[6] = (int)readings[6];  //gyro-Z

  Serial.print("Raw-Gyro: ");
  Serial.print(result[4]);  //X  
  Serial.print(" ");
  Serial.print(result[5]);  //Y
  Serial.print(" ");
  Serial.print(result[6]); //Z
  Serial.println(" ");
*/  
}

//=============================================================
//=============================================================
void read_Mag_Data(short * result)
{
  byte howManyBytesToRead = 6;   //uint8_t means: give me an unsigned int of exactly 8 bits.
  byte _buff[6];
  short readings[3];  //short is 16bits=2bytes
  
  // each axis reading comes in 10 bit resolution, ie 2 bytes.  Least Significat Byte first!!
  // thus we are converting both bytes in to a 2byte short

  if (KOMPASS == 0x0E)
    {
  int regAddress = 0x01;    //first axis-Kompass-data register of MAG3110
  //read the Mag-data from the MAG3110  
  readFrom(KOMPASS, regAddress, howManyBytesToRead, _buff); //read the acceleration data from the MAG3110 
  readings[0] = ((_buff[0]) << 8) | _buff[1]; // X-as raw-data.
  readings[1] = ((_buff[2]) << 8) | _buff[3]; // Y-as raw-data.
  readings[2] = ((_buff[4]) << 8) | _buff[5]; // Zas raw-data.
  delay(3);
    }

  result[0] = readings[0];  //bit values
  result[1] = readings[1];  
  result[2] = readings[2]; 
  
//     Serial.print("Magnetometer raw readings: ");
//     Serial.print(result[0]);  //X  
//     Serial.print(" ");
//     Serial.print(result[1]);  //Y
//     Serial.print(" ");
//     Serial.print(result[2]); //Z
//     Serial.println();
}


//=============================================================
//=============================================================
// Reads num bytes starting from address register on ACC in to _buff array

void readFrom(int DEVICE, byte address, int num, byte _buff[])
{
  I2C.beginTransmission(DEVICE); // start transmission to device
  I2C.write(address);             // sends address to read from
  I2C.endTransmission();         // end transmission

  I2C.beginTransmission(DEVICE); // start transmission to device
  I2C.requestFrom(DEVICE, num);    // request 6 bytes from device

  int i = 0;
  while (I2C.available())        // device may send less than requested (abnormal)
  {
    _buff[i] = I2C.read();    // receive a byte
    i++;
  }
  I2C.endTransmission();         // end transmission
}

//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
//+++++++++++++++ End Sensor Reading functions ++++++++++++++++
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++