text: #include "mpu6050.h"
  
  int output_counter;
  
  float dtTotal;
  
  struct OffsetParams
  {
      void *x;
      void (*callback)(void *, float, float, float, float, float, float);
  };
  
  struct SensorParams
  {
      void *x;
      void (*callback)(void *);
  };
  
  void get_gyro_raw(float *roll, float *pitch, float *yaw)
  {
      int16_t X = i2c_smbus_read_byte_data(f_dev, 0x43) << 8 | i2c_smbus_read_byte_data(f_dev, 0x44); //Read X registers
      int16_t Y = i2c_smbus_read_byte_data(f_dev, 0x45) << 8 | i2c_smbus_read_byte_data(f_dev, 0x46); //Read Y registers
      int16_t Z = i2c_smbus_read_byte_data(f_dev, 0x47) << 8 | i2c_smbus_read_byte_data(f_dev, 0x48); //Read Z registers
      *roll = (float)X;                                                                               //Roll on X axis
      *pitch = (float)Y;                                                                              //Pitch on Y axis
      *yaw = (float)Z;                                                                                //Yaw on Z axis
  }
  
  void get_gyro(float *roll, float *pitch, float *yaw)
  {
      get_gyro_raw(roll, pitch, yaw);                                       //Store raw values into variables
      *roll = round((*roll - offset_gyro_r) * 1000.0 / gyro_sens) / 1000.0; //Remove the offset and divide by the gyroscope sensetivity (use 1000 and round() to round the value to three decimal places)
      *pitch = round((*pitch - offset_gyro_p) * 1000.0 / gyro_sens) / 1000.0;
      *yaw = round((*yaw - offset_gyro_y) * 1000.0 / gyro_sens) / 1000.0;
  }
  
  void get_accel_raw(float *x, float *y, float *z)
  {
      int16_t X = i2c_smbus_read_byte_data(f_dev, 0x3b) << 8 | i2c_smbus_read_byte_data(f_dev, 0x3c); //Read X registers
      int16_t Y = i2c_smbus_read_byte_data(f_dev, 0x3d) << 8 | i2c_smbus_read_byte_data(f_dev, 0x3e); //Read Y registers
      int16_t Z = i2c_smbus_read_byte_data(f_dev, 0x3f) << 8 | i2c_smbus_read_byte_data(f_dev, 0x40); //Read Z registers
      *x = (float)X;
      *y = (float)Y;
      *z = (float)Z;
  }
  
  void get_accel(float *x, float *y, float *z)
  {
      get_accel_raw(x, y, z);                                       //Store raw values into variables
      *x = round((*x - offset_acc_x) * 1000.0 / acc_sens) / 1000.0; //Remove the offset and divide by the accelerometer sensetivity (use 1000 and round() to round the value to three decimal places)
      *y = round((*y - offset_acc_y) * 1000.0 / acc_sens) / 1000.0;
      *z = round((*z - offset_acc_z) * 1000.0 / acc_sens) / 1000.0;
  }
  
  int get_angle(int axis, float *result)
  {
      if (axis >= 0 && axis <= 2)
      {                          //Check that the axis is in the valid range
          *result = angle[axis]; //Get the result
          return 0;
      }
      else
      {
          post("ERR (MPU6050.cpp:getAngle()): 'axis' must be between 0 and 2 (for roll, pitch or yaw)");
          *result = 0; //Set result to zero
          return 1;
      }
  }
  
  void *calculate_offsets(void *arg)
  {
      running = 0;
      sleep(1);
      sys_lock();
      post("Calculating the offsets. Please keep the accelerometer level and still. This could take a couple of minutes...");
      sys_unlock();
      struct OffsetParams *offsetParams = arg;
  
      float ax_off = 0;
      float ay_off = 0;
      float az_off = 0;
      float gr_off = 0;
      float gp_off = 0;
      float gy_off = 0;
  
      float gyro_off[3]; //Temporary storage
      float accel_off[3];
  
      for (int i = 0; i < 10000; i++)
      {                                                                                                //Use loop to average offsets
          get_gyro_raw(&gyro_off[0], &gyro_off[1], &gyro_off[2]);                                      //Raw gyroscope values
          gr_off = gr_off + gyro_off[0], gp_off = gp_off + gyro_off[1], gy_off = gy_off + gyro_off[2]; //Add to sum
  
          get_accel_raw(&accel_off[0], &accel_off[1], &accel_off[2]);                                     //Raw accelerometer values
          ax_off = ax_off + accel_off[0], ay_off = ay_off + accel_off[1], az_off = az_off + accel_off[2]; //Add to sum
      }
  
      gr_off = gr_off / 10000, gp_off = gp_off / 10000, gy_off = gy_off / 10000; //Divide by number of loops (to average)
      ax_off = ax_off / 10000, ay_off = ay_off / 10000, az_off = az_off / 10000;
  
      az_off = az_off - acc_sens; //Remove 1g from the value calculated to compensate for gravity)
  
      offsetParams->callback(offsetParams->x, ax_off, ay_off, az_off, gr_off, gp_off, gy_off);
  
      free(offsetParams);
  
      return 0;
  }
  
  void get_offsets(void *x, void (*callback)(void *, float, float, float, float, float, float))
  {
      struct OffsetParams *offsetParams = (struct OffsetParams *)malloc(sizeof(struct OffsetParams));
      offsetParams->x = x;
      offsetParams->callback = callback;
      pthread_t calc_offset_thread;
      pthread_create(&calc_offset_thread, NULL, calculate_offsets, (void *)offsetParams);
  }
  
  void set_offsets(float acc_x, float acc_y, float acc_z, float gyro_r, float gyro_p, float gyro_y)
  {
      offset_acc_x = acc_x;
      offset_acc_y = acc_y;
      offset_acc_z = acc_z;
      offset_gyro_r = gyro_r;
      offset_gyro_p = gyro_p;
      offset_gyro_y = gyro_y;
  }
  
  static float wrap(float angle_to_wrap, float limit)
  {
      while (angle_to_wrap > limit)
          angle_to_wrap -= 2 * limit;
      while (angle_to_wrap < -limit)
          angle_to_wrap += 2 * limit;
      return angle_to_wrap;
  }
  
  void stop_thread()
  {
      running = 0;
  }
  
  void setup()
  {
      int MPU6050_addr = 0x68;
  
      f_dev = open("/dev/i2c-1", O_RDWR);
  
      if (f_dev < 0)
      { //Catch errors
          post("ERROR: Failed to open /dev/i2c-1. Please check that I2C is enabled with raspi-config");
      }
  
      int status = ioctl(f_dev, I2C_SLAVE, MPU6050_addr);
  
      if (status < 0)
      {
          post("ERROR: Could not get I2C bus with adress: %i", MPU6050_addr); //Print error message
      }
  
      i2c_smbus_write_byte_data(f_dev, 0x6b, 0b00000000); //Take MPU6050 out of sleep mode - see Register Map
  
      i2c_smbus_write_byte_data(f_dev, 0x1a, 0b00000011); //Set DLPF (low pass filter) to 44Hz (so no noise above 44Hz will pass through)
  
      i2c_smbus_write_byte_data(f_dev, 0x19, 0b00000100); //Set sample rate divider (to 200Hz) - see Register Map
  
      i2c_smbus_write_byte_data(f_dev, 0x1b, gyro_config); //Configure gyroscope settings - see Register Map (see MPU6050.h for the GYRO_CONFIG parameter)
  
      i2c_smbus_write_byte_data(f_dev, 0x1c, acc_config);
  
      //Set offsets to zero
      i2c_smbus_write_byte_data(f_dev, 0x06, 0b00000000),
          i2c_smbus_write_byte_data(f_dev, 0x07, 0b00000000),
          i2c_smbus_write_byte_data(f_dev, 0x08, 0b00000000),
          i2c_smbus_write_byte_data(f_dev, 0x09, 0b00000000),
          i2c_smbus_write_byte_data(f_dev, 0x0A, 0b00000000),
          i2c_smbus_write_byte_data(f_dev, 0x0B, 0b00000000),
          i2c_smbus_write_byte_data(f_dev, 0x00, 0b10000001),
          i2c_smbus_write_byte_data(f_dev, 0x01, 0b00000001),
          i2c_smbus_write_byte_data(f_dev, 0x02, 0b10000001);
  }
  
  void *sensor_loop(void *arg)
  {
      struct SensorParams *sensorParams = arg;
      clock_gettime(CLOCK_REALTIME, &start);
      while (running > 0)
      {
          get_gyro(&gr, &gp, &gy);
          get_accel(&ax, &ay, &az);
          sgZ = (az >= 0) - (az < 0); // allow one angle to go from -180° to +180°
  
          angle_acc_x = atan2(ay, sgZ * sqrt(az * az + ax * ax)) * RAD_T_DEG; // [-180°,+180°]
          angle_acc_y = -atan2(ax, sqrt(az * az + ay * ay)) * RAD_T_DEG;      // [- 90°,+ 90°]
  
          angle[0] = wrap(filter_coeff * (angle_acc_x + wrap(angle[0] + gr * dt - angle_acc_x, 180)) + (1.0 - filter_coeff) * angle_acc_x, 180);
          angle[1] = wrap(filter_coeff * (angle_acc_y + wrap(angle[1] + sgZ * gp * dt - angle_acc_y, 90)) + (1.0 - filter_coeff) * angle_acc_y, 90);
          angle[2] += gy * dt;
  
          dtTotal += dt;
  
          if (dtTotal >= output_secs)
          {
              if(running) sensorParams->callback(sensorParams->x);
              dtTotal = 0;
          }
  
          clock_gettime(CLOCK_REALTIME, &end);
          dt = (end.tv_sec - start.tv_sec) + (end.tv_nsec - start.tv_nsec) / 1e9; //Calculate new dt
          clock_gettime(CLOCK_REALTIME, &start);                                  //Save time to start clock
      }
      post("sensor thread stopped");
      free(sensorParams);
      return 0;
  }
  
  void start_thread(
      pthread_attr_t thread_attr,
      pthread_t x_threadid,
      void (*callback)(void *),
      void *x)
  {
      if (running == 1){
          stop_thread();
      }
  
      running = 1;
  
      struct SensorParams *sensorParams = (struct SensorParams *)malloc(sizeof(struct SensorParams));
      sensorParams->x = x;
      sensorParams->callback = callback;
  
      if (pthread_create(&x_threadid, &thread_attr, sensor_loop, (void *)sensorParams) < 0)
      {
          error("threadtemplate: could not launch receive thread");
          return;
      }
      else
      {
          post("sensor thread started");
      }
  }
  
  void set_ranges(int acc_range, int gyro_range)
  {
  
      if (acc_range == 1)
      {
          acc_sens = 8192.0;
          acc_config = 0b00001000;
      }
      else if (acc_range == 2)
      {
          acc_sens = 4096.0;
          acc_config = 0b00010000;
      }
      else if (acc_range == 3)
      {
          acc_sens = 2048.0;
          acc_config = 0b00011000;
      }
      else
      {
          acc_sens = 16384.0;
          acc_config = 0b00000000;
      }
  
      if (gyro_range == 1)
      {
          gyro_sens = 65.5;
          gyro_config = 0b00001000;
      }
      else if (gyro_range == 2)
      {
          gyro_sens = 32.8;
          gyro_config = 0b00010000;
      }
      else if (gyro_range == 3)
      {
          gyro_sens = 16.4;
          gyro_config = 0b00011000;
      }
      else
      {
          gyro_sens = 131.0;
          gyro_config = 0b00000000;
      }
  }
  
  void init_mpu6050(int acc_range, int gyro_range, float output_freq, float f_coeff)
  {
      filter_coeff = f_coeff;
      output_secs = 1. / output_freq;
      set_ranges(acc_range, gyro_range);
      setup();
  }