bool MPU6050::initialize() {
setClockSource(MPU6050_CLOCK_PLL_XGYRO);
setFullScaleGyroRange(MPU6050_GYRO_FS_250);
setFullScaleAccelRange(MPU6050_ACCEL_FS_2);
setSleepEnabled(false); // thanks to Jack Elston for pointing this one out!
return 1;
}
void LSM330DLC::init(int acc_addr, int gyro_addr) {
accAddr = acc_addr;
gyroAddr = gyro_addr;
setAccelDataRate(LSM330DLC_A_DATARATE_25HZ);
setFullScaleAccelRange(LSM330DLC_A_FULLSCALE_PM2G);
enableGyro();
setFullScaleGyroRange(LSM330DLC_G_FULLSCALE_250DPS);
}
void mpu_setup()
{
devAddr = MPU6050_DEFAULT_ADDRESS;
//switchSPIEnabled(true);
DELAY_US(1*1000);
setClockSource(MPU6050_CLOCK_PLL_XGYRO);
setFullScaleGyroRange(MPU6050_GYRO_FS_250);
setFullScaleAccelRange(MPU6050_ACCEL_FS_2);
setSleepEnabled(false); // thanks to Jack Elston for pointing this one out!
}
void MPU6XXX32::init(SPI2C_config * con)
{
i2cConfig = con;
comP = SPI2C::setSPIC2C(i2cConfig);
WR->setSPI2CConfig(i2cConfig);
if (comP->currentConfg->spi2ctype == SPI2C_SPI_TRANSACTIONAL)
spiMode = true;
setClockSource(MPU6050_CLOCK_PLL_XGYRO);
setFullScaleGyroRange(MPU6050_GYRO_FS_250);
setFullScaleAccelRange(MPU6050_ACCEL_FS_2);
setSleepEnabled(false); // thanks to Jack Elston for pointing this one out!
}
void CurieIMUClass::setAccelerometerRange(int range)
{
BMI160AccelRange bmiRange;
if (range <= 2) {
bmiRange = BMI160_ACCEL_RANGE_2G;
} else if (range <= 4) {
bmiRange = BMI160_ACCEL_RANGE_4G;
} else if (range <= 8) {
bmiRange = BMI160_ACCEL_RANGE_8G;
} else {
bmiRange = BMI160_ACCEL_RANGE_16G;
}
setFullScaleAccelRange(bmiRange);
}
/** Power on and prepare for general usage.
* This will activate the device and take it out of sleep mode (which must be done
* after start-up). This function also sets both the accelerometer and the gyroscope
* to their most sensitive settings, namely +/- 2g and +/- 250 degrees/sec, and sets
* the clock source to use the X Gyro for reference, which is slightly better than
* the default internal clock source.
*/
void MPU6050::initialize()
{
#ifdef useDebugSerial
debugSerial.printf("MPU6050::initialize start\n\r");
#endif
setClockSource(MPU6050_CLOCK_PLL_XGYRO);
setFullScaleGyroRange(MPU6050_GYRO_FS_500);
setFullScaleAccelRange(MPU6050_ACCEL_FS_16);
setSleepEnabled(false); // thanks to Jack Elston for pointing this one out!
#ifdef useDebugSerial
debugSerial.printf("MPU6050::initialize end\n\r");
#endif
}
/** Power on and prepare for general usage.
* This will activate the device and take it out of sleep mode (which must be done
* after start-up). This function also sets both the accelerometer and the gyroscope
* to their most sensitive settings, namely +/- 2g and +/- 250 degrees/sec, and sets
* the clock source to use the X Gyro for reference, which is slightly better than
* the default internal clock source.
*/
PUBLIC void initializeLSM6DS0(bool useAccel, bool useGyro) {
xAccelOffset = yAccelOffset = zAccelOffset = 0;
xGyroOffset = yGyroOffset = zGyroOffset = 0;
gyroEnabled = useGyro;
accelEnabled = useAccel;
if(gyroEnabled)
{
gScale = GYRO_FS_245DSP;
calcGyroRes();
}
if(accelEnabled)
{
aScale = ACC_FS_2G;
calcAccelRes();
}
if(testConnection())
{
resetLSM6DS0();
if(accelEnabled)
{
initAccel();
setFullScaleAccelRange(aScale);
setAccelODR(ACC_ODR_119Hz);
}
if(gyroEnabled)
{
initGyro();
setFullScaleGyroRange(gScale);
setGyroODR(GYRO_ODR_119Hz_CO_14Hz);
}
calibrateLSM6DS0(gbias,abias);
}
}
/******************************** MPU6050 Functions ********************************/
void initializeIMU()
{
//Init Interrupt Output Pin
P1REN |= BIT4;
P1OUT |= BIT4;
P1IFG &= ~BIT4; // P1.4 IFG cleared
P1IE |= BIT4; // P1.4 interrupt enabled
P4SEL |= BIT1 + BIT2; // P4.1 & P4.2 SDA/SCL Select
// Delays do not need to be this long. Delays are present to keep I2C line clear.
// This functions can be altered by the user. Check MPU6050.h file by Jeff Rowberg to determine the sensitivty variables.
setClockSource(MPU6050_CLOCK_PLL_XGYRO); // Set MPU6050 clock
msDelay(100);
setFullScaleGyroRange(MPU6050_GYRO_FS_2000); // Gyroscope sensitivity set to 2000 degrees/sec
msDelay(100);
setFullScaleAccelRange(MPU6050_ACCEL_FS_4); // Accelerometer sensitivity set to 4g
msDelay(100);
setDLPFConfig(MPU6050_DLPF_BW_5); // Digital Low Pass Filter Configuration
setInterruptPin();
setSleepEnabled(0); // Wake up device.
msDelay(100);
}
int main (void)
{
initI2C1 ();
initUsart ();
#if 0
/*
* +----------------+
* | Initialization |
* +----------------+
*/
accelgyro.initialize();
setClockSource(MPU6050_CLOCK_PLL_XGYRO/*0x01*/);
/*
* Excerpt from domcumentation : Upon power up, the MPU-60X0 clock source defaults to the internal oscillator.
* However, it is highly recommended that the device be configured to use one of the gyroscopes. Below is the code
* which does it:
*/
I2Cdev::writeBits(devAddr, MPU6050_RA_PWR_MGMT_1/*0x6B*/, MPU6050_PWR1_CLKSEL_BIT/*2*/, MPU6050_PWR1_CLKSEL_LENGTH/*3*/, source/*MPU6050_CLOCK_PLL_XGYRO 0x01*/);
setFullScaleGyroRange(MPU6050_GYRO_FS_250/*0x00*/);
/*
* 0x1b register is used to trigger gyroscope self-test and configure the gyroscopes’ full scale range. Below
* we set ful scale to be +/- 250 units (seconds?)
*/
I2Cdev::writeBits(devAddr, MPU6050_RA_GYRO_CONFIG/*0x1B*/, MPU6050_GCONFIG_FS_SEL_BIT/*4*/, MPU6050_GCONFIG_FS_SEL_LENGTH/*2*/, range/*0x00*/);
setFullScaleAccelRange(MPU6050_ACCEL_FS_2/*0x00*/);
/*
* Set accelerometer full scale to be +/- 2g.
*/
I2Cdev::writeBits(devAddr, MPU6050_RA_ACCEL_CONFIG/*0x1C*/, MPU6050_ACONFIG_AFS_SEL_BIT/*4*/, MPU6050_ACONFIG_AFS_SEL_LENGTH/*2*/, range/*0*/);
setSleepEnabled(false); // thanks to Jack Elston for pointing this one out!
/*
* By default MPU6050 is in sleep mode after powering up. Below we are waking it back on. This
* is done using the same register as in first line,
*/
I2Cdev::writeBit(devAddr, MPU6050_RA_PWR_MGMT_1/*0x6B*/, MPU6050_PWR1_SLEEP_BIT/*6*/, enabled/*false*/);
accelgyro.testConnection()
getDeviceID() == 0x34;
/*
* This register is used to verify the identity of the device. The contents of WHO_AM_I are
* the upper 6 bits of the MPU-60X0’s 7-bit I C address. The Power-On-Reset value of Bit6:Bit1 is 0b110100 == 0x34.
*/
I2Cdev::readBits(devAddr, MPU6050_RA_WHO_AM_I/*0x75*/, MPU6050_WHO_AM_I_BIT/*6*/, MPU6050_WHO_AM_I_LENGTH/*6*/, buffer);
return buffer[0];
/*
* +----------------+
* | Main loop |
* +----------------+
*/
int16_t ax, ay, az;
int16_t gx, gy, gz;
accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
/*
* In MPU-6000 and MPU-6050 Product Specification Rev 3.3 on pages 36 and 37 we read, that I²C reads and writes
* can be performed with single byte or multiple bytes. In single byte mode, we issue (after sending slave
* address ofcourse) a register address, and send or receive one byte of data. Multiple byte reads and writes, at the
* other hand consist of slave address, regiser address and multiple consecutive bytes od data. Slave puts or gets
* first byte from the register with the address we've just sent, and increases this addres by 1 after each byte.
*
* This is very useful in case of accelerometer and gyroscope because manufacturer has set up the apropriate registers
* cnsecutively, so one can read accel, internal temp and gyro data in one read command. Below is the code which does
* exactly this:
*/
I2Cdev::readBytes(devAddr, MPU6050_RA_ACCEL_XOUT_H/*0x3B*/, 14, buffer);
*ax = (((int16_t)buffer[0]) << 8) | buffer[1];
*ay = (((int16_t)buffer[2]) << 8) | buffer[3];
*az = (((int16_t)buffer[4]) << 8) | buffer[5];
*gx = (((int16_t)buffer[8]) << 8) | buffer[9];
*gy = (((int16_t)buffer[10]) << 8) | buffer[11];
*gz = (((int16_t)buffer[12]) << 8) | buffer[13];
#endif
// Configuration:
I2C_start (I2C1, MPU6050_ADDRESS_AD0_LOW, I2C_Direction_Transmitter); // start a transmission in Master transmitter mode
I2C_write_slow (I2C1, MPU6050_RA_PWR_MGMT_1); // Register address
I2C_write (I2C1, MPU6050_CLOCK_PLL_XGYRO); // Register value = 0x01. Which means, that DEVICE_RESET, SLEEP, CYCLE and TEMP_DIS are all 0.
I2C_stop (I2C1);
I2C_start (I2C1, MPU6050_ADDRESS_AD0_LOW, I2C_Direction_Transmitter);
I2C_write (I2C1, MPU6050_RA_GYRO_CONFIG);
I2C_write (I2C1, MPU6050_GYRO_FS_250); // All bits set to zero.
I2C_stop (I2C1);
I2C_start (I2C1, MPU6050_ADDRESS_AD0_LOW, I2C_Direction_Transmitter);
I2C_write (I2C1, MPU6050_RA_ACCEL_CONFIG);
I2C_write (I2C1, MPU6050_ACCEL_FS_2); // All bits set to zero.
I2C_stop (I2C1);
// Simple test if communication is working
I2C_start (I2C1, MPU6050_ADDRESS_AD0_LOW, I2C_Direction_Transmitter);
I2C_write (I2C1, MPU6050_RA_WHO_AM_I);
I2C_stop (I2C1);
I2C_start (I2C1, MPU6050_ADDRESS_AD0_LOW, I2C_Direction_Receiver);
uint8_t whoAmI = I2C_read_nack (I2C1); // read one byte and don't request another byte
I2C_stop (I2C1);
if (whoAmI == 0x34) {
usartSendString (USART1, "Accelerometer has been found!\r\n");
}
else {
usartSendString (USART1, "*NO* Accelerometer has been found!\r\n");
}
while (1) {
I2C_start (I2C1, MPU6050_ADDRESS_AD0_LOW, I2C_Direction_Transmitter);
I2C_write (I2C1, MPU6050_RA_ACCEL_XOUT_H);
I2C_stop (I2C1);
I2C_start (I2C1, MPU6050_ADDRESS_AD0_LOW, I2C_Direction_Receiver);
uint16_t ax = ((uint16_t)I2C_read_ack (I2C1) << 8) | I2C_read_ack (I2C1);
uint16_t ay = ((uint16_t)I2C_read_ack (I2C1) << 8) | I2C_read_ack (I2C1);
uint16_t az = ((uint16_t)I2C_read_ack (I2C1) << 8) | I2C_read_ack (I2C1);
uint16_t temp = ((uint16_t)I2C_read_ack (I2C1) << 8) | I2C_read_ack (I2C1);
uint16_t gx = ((uint16_t)I2C_read_ack (I2C1) << 8) | I2C_read_ack (I2C1);
uint16_t gy = ((uint16_t)I2C_read_ack (I2C1) << 8) | I2C_read_ack (I2C1);
uint16_t gz = ((uint16_t)I2C_read_ack (I2C1) << 8) | I2C_read_nack (I2C1);
I2C_stop (I2C1);
printf ("Accel : (%d, %d, %d), temperature : %d, gyro : (%d, %d, %d)\r\n", ax, ay, az, temp, gx, gy, gz);
}
}
void CurieIMUClass::setAccelerometerRange(int range)
{
setFullScaleAccelRange(range);
}
