mirror of
https://gitlab.com/yikestone/qt_pi.git
synced 2025-08-03 21:54:12 +05:30
44 lines
1.5 KiB
C++
44 lines
1.5 KiB
C++
#include "qt_pi.h"
|
|
|
|
void initIMU(){
|
|
|
|
Wire.begin();
|
|
|
|
uint8_t buf;
|
|
|
|
I2Cdev::writeBits(MPU6050_DEFAULT_ADDRESS, MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_CLKSEL_BIT, MPU6050_PWR1_CLKSEL_LENGTH, MPU6050_CLOCK_PLL_XGYRO);
|
|
|
|
I2Cdev::writeBits(MPU6050_DEFAULT_ADDRESS, MPU6050_RA_GYRO_CONFIG, MPU6050_GCONFIG_FS_SEL_BIT, MPU6050_GCONFIG_FS_SEL_LENGTH, MPU6050_GYRO_FS_250);
|
|
|
|
I2Cdev::writeBits(MPU6050_DEFAULT_ADDRESS, MPU6050_RA_ACCEL_CONFIG, MPU6050_ACONFIG_AFS_SEL_BIT, MPU6050_ACONFIG_AFS_SEL_LENGTH, MPU6050_ACCEL_FS_2);
|
|
|
|
I2Cdev::writeBit(MPU6050_DEFAULT_ADDRESS, MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_SLEEP_BIT, false);
|
|
|
|
I2Cdev::readBits(MPU6050_DEFAULT_ADDRESS, MPU6050_RA_WHO_AM_I, MPU6050_WHO_AM_I_BIT, MPU6050_WHO_AM_I_LENGTH, &buf);
|
|
|
|
// use the code below to change accel/gyro offset values
|
|
/*
|
|
Serial.println("Updating internal sensor offsets...");
|
|
// -76 -2359 1688 0 0 0
|
|
Serial.print(accelgyro.getXAccelOffset()); Serial.print("\t"); // -76
|
|
Serial.print(accelgyro.getYAccelOffset()); Serial.print("\t"); // -2359
|
|
Serial.print(accelgyro.getZAccelOffset()); Serial.print("\t"); // 1688
|
|
accelgyro.setXGyroOffset(220);
|
|
accelgyro.setYGyroOffset(76);
|
|
accelgyro.setZGyroOffset(-85);
|
|
*/
|
|
}
|
|
|
|
void sendIMU() {
|
|
|
|
static uint8_t buf[14 + com_preamble_len];
|
|
|
|
I2Cdev::readBytes(MPU6050_DEFAULT_ADDRESS, MPU6050_RA_ACCEL_XOUT_H, 14, buf + DATASTRB);
|
|
|
|
buf[COMB] = IMU_DATA;
|
|
buf[CIDB] = BID;
|
|
|
|
sendData(14 + com_preamble_len, buf);
|
|
|
|
}
|