MPU6050.cpp

Go to the documentation of this file.

#include "MPU6050.hh"

void
MPU6050::write(Register reg, uint8_t value)
{
  twi.acquire(this);
  twi.write((uint8_t) reg, &value, sizeof(value));
  twi.release();
}

void
MPU6050::write(Register reg, void* buffer, size_t count)
{
  twi.acquire(this);
  twi.write((uint8_t) reg, buffer, count);
  twi.release();
}

uint8_t
MPU6050::read(Register reg)
{
  uint8_t res;
  twi.acquire(this);
  twi.write((uint8_t) reg);
  twi.read(&res, sizeof(res));
  twi.release();
  return (res);
}

void
MPU6050::read(Register reg, void* buffer, size_t count)
{
  twi.acquire(this);
  twi.write((uint8_t) reg);
  twi.read(buffer, count);
  twi.release();
}

bool
MPU6050::begin(uint8_t clksel)
{
  // Sanity check the identity register value
  if (read(WHO_AM_I) != 0x68) return (false);

  // Set clock source (default PLL with X axis gyroscope reference)
  pwr_mgmt_1_t pwr;
  pwr.CLKSEL = clksel;
  write(PWR_MGMT_1, pwr);

  // Set accelerometer full range (default +/-2 g)
  accel_config_t accel;
  accel.AFS_SEL = AFS_RANGE_2G;
  write(ACCEL_CONFIG, accel);

  // Set gyroscope full range (default +/-250 dps)
  gyro_config_t gyro;
  gyro.FS_SEL = FS_RANGE_250;
  write(GYRO_CONFIG, gyro);

  return (true);
}

bool
MPU6050::end()
{
  return (true);
}

int16_t
MPU6050::read_temperature()
{
  int16_t temperature;
  read(TEMP_OUT, &temperature, sizeof(temperature));
  return (swap(temperature) + 12410) / 34;
}

void
MPU6050::read_motion(motion_t& m)
{
  read(ACCEL_OUT, &m, sizeof(m));
  swap<motion_t>(&m);
  m.temp = (m.temp + 12410) / 34;
}

void
MPU6050::read_accelerometer(sample_t& s)
{
  read(ACCEL_OUT, &s, sizeof(s));
  swap<sample_t>(&s);
}

void
MPU6050::read_gyroscope(sample_t& s)
{
  read(GYRO_OUT, &s, sizeof(s));
  swap<sample_t>(&s);
}

IOStream&
operator<<(IOStream& outs, MPU6050& mpu)
{
  MPU6050::motion_t value;
  mpu.read_motion(value);
  outs << PSTR("MPU6050::accelerometer(x = ") << value.accel.x
       << PSTR(", y = ") << value.accel.y
       << PSTR(", z = ") << value.accel.z
       << PSTR(")") << endl;
  outs << PSTR("MPU6050::temperature = ") << value.temp << endl;
  outs << PSTR("MPU6050::gyroscope(x = ") << value.gyro.x
       << PSTR(", y = ") << value.gyro.y
       << PSTR(", z = ") << value.gyro.z
       << PSTR(")") << endl;
  return (outs);
}