有没有办法把MPU-6050的原始数据转换成0-359的旋转数据？
问题描述 投票：1回答：2
我这周正在搭建一个Oculus Quest。它的意思是一个有趣的（廉价）项目的工作，我想出了如何计算肢体位移，如果一个恒定的（脚的位置）位置被跟踪positonaly。我已经设法在同一个arduino上一次添加2个以上的MPU-6050。问题是，当我想从MPU的串行数据，并把它到统一。旋转数据非常错误，看起来是这样的。
gyro -8.00 -171.00 -148.00 ||| gyro 117.00 -639.00 -1105.00
gyro -4.00 -160.00 -125.00 ||| gyro -20.00 -640.00 -1081.00
gyro -9.00 -160.00 -128.00 ||| gyro -86.00 -694.00 -1074.00
gyro 13.00 -141.00 -120.00 ||| gyro -309.00 -713.00 -1040.00
gyro -8.00 -136.00 -134.00 ||| gyro -184.00 -683.00 -1045.00
gyro -6.00 -150.00 -158.00 ||| gyro -214.00 -696.00 -1058.00
gyro -12.00 -143.00 -147.00 ||| gyro -5.00 -644.00 -1086.00
gyro 16.00 -157.00 -140.00 ||| gyro 42.00 -662.00 -1153.00
gyro -9.00 -151.00 -136.00 ||| gyro 54.00 -644.00 -1154.00
gyro 8.00 -143.00 -143.00 ||| gyro 81.00 -662.00 -1149.00
gyro -17.00 -124.00 -134.00 ||| gyro -126.00 -698.00 -1088.00
gyro -11.00 -150.00 -138.00 ||| gyro -28.00 -703.00 -1169.00
gyro -1.00 -129.00 -161.00 ||| gyro -187.00 -744.00 -1125.00
gyro -23.00 -145.00 -133.00 ||| gyro -43.00 -713.00 -1117.00
gyro 1.00 -143.00 -139.00 ||| gyro -7.00 -662.00 -1160.00
gyro 11.00 -156.00 -136.00 ||| gyro 20.00 -697.00 -1178.00
gyro 21.00 -132.00 -116.00 ||| gyro 168.00 -651.00 -1162.00
gyro -1.00 -133.00 -153.00 ||| gyro -41.00 -688.00 -1151.00
gyro -5.00 -174.00 -144.00 ||| gyro 107.00 -699.00 -1157.00
gyro 2.00 -144.00 -146.00 ||| gyro -177.00 -710.00 -1105.00
gyro -31.00 -134.00 -133.00 ||| gyro -124.00 -687.00 -1091.00
gyro -8.00 -169.00 -121.00 ||| gyro -114.00 -695.00 -1133.00
gyro -11.00 -178.00 -151.00 ||| gyro -132.00 -662.00 -1074.00
gyro -20.00 -154.00 -132.00 ||| gyro 62.00 -659.00 -1136.00
gyro 22.00 -142.00 -135.00 ||| gyro -88.00 -665.00 -1116.00
gyro -30.00 -148.00 -131.00 ||| gyro 51.00 -683.00 -1144.00
gyro -2.00 -163.00 -129.00 ||| gyro -111.00 -699.00 -1119.00
gyro -9.00 -164.00 -128.00 ||| gyro -58.00 -706.00 -1114.00
gyro 4.00 -146.00 -146.00 ||| gyro -52.00 -699.00 -1121.00
gyro 12.00 -149.00 -138.00 ||| gyro -147.00 -707.00 -1092.00
gyro -24.00 -146.00 -131.00 ||| gyro -2.00 -701.00 -1164.00
gyro -7.00 -135.00 -142.00 ||| gyro -38.00 -700.00 -1137.00
gyro -2.00 -169.00 -139.00 ||| gyro 92.00 -636.00 -1156.00
gyro -12.00 -160.00 -145.00 ||| gyro -52.00 -658.00 -1094.00
gyro -12.00 -145.00 -131.00 ||| gyro -27.00 -689.00 -1137.00
gyro -22.00 -139.00 -134.00 ||| gyro -42.00 -723.00 -1122.00
gyro 0.00 -149.00 -133.00 ||| gyro -54.00 -682.00 -1096.00
gyro -15.00 -173.00 -124.00 ||| gyro 24.00 -688.00 -1127.00
gyro 9.00 -163.00 -139.00 ||| gyro -117.00 -682.00 -1080.00
gyro -21.00 -151.00 -128.00 ||| gyro 21.00 -662.00 -1137.00
gyro 20.00 -101.00 -159.00 ||| gyro -96.00 -636.00 -1051.00
gyro -5.00 -166.00 -132.00 ||| gyro -135.00 -693.00 -1128.00
gyro -10.00 -149.00 -123.00 ||| gyro -173.00 -699.00 -1112.00
gyro 14.00 -165.00 -135.00 ||| gyro -91.00 -723.00 -1134.00
gyro -12.00 -163.00 -151.00 ||| gyro 255.00 -670.00 -1163.00
gyro -10.00 -158.00 -144.00 ||| gyro 95.00 -612.00 -1163.00
gyro 0.00 -150.00 -130.00 ||| gyro 17.00 -693.00 -1195.00
gyro -18.00 -159.00 -146.00 ||| gyro -59.00 -728.00 -1161.00
gyro 2.00 -183.00 -114.00 ||| gyro -40.00 -740.00 -1148.00
gyro 3.00 -164.00 -107.00 ||| gyro 80.00 -729.00 -1201.00
gyro -17.00 -148.00 -128.00 ||| gyro -141.00 -682.00 -1147.00
gyro 14.00 -158.00 -147.00 ||| gyro 112.00 -696.00 -1145.00
gyro -1.00 -139.00 -136.00 ||| gyro -116.00 -731.00 -1091.00
gyro -11.00 -136.00 -128.00 ||| gyro -64.00 -735.00 -1111.00
gyro -2.00 -150.00 -133.00 ||| gyro -151.00 -757.00 -1092.00
gyro -21.00 -136.00 -129.00 ||| gyro -156.00 -714.00 -1139.00
gyro -20.00 -163.00 -111.00 ||| gyro 42.00 -738.00 -1116.00
gyro -16.00 -160.00 -141.00 ||| gyro -141.00 -681.00 -1108.00
gyro -8.00 -176.00 -129.00 ||| gyro 80.00 -691.00 -1142.00
gyro -6.00 -167.00 -159.00 ||| gyro -52.00 -725.00 -1119.00
gyro -10.00 -178.00 -135.00 ||| gyro -62.00 -706.00 -1069.00
gyro -16.00 -173.00 -136.00 ||| gyro -129.00 -735.00 -1112.00
gyro 9.00 -147.00 -158.00 ||| gyro -203.00 -734.00 -1132.00
gyro 12.00 -152.00 -131.00 ||| gyro -12.00 -720.00 -1158.00
gyro -24.00 -163.00 -146.00 ||| gyro -76.00 -737.00 -1141.00
gyro 27.00 -153.00 -128.00 ||| gyro 190.00 -676.00 -1162.00
gyro 1.00 -150.00 -129.00 ||| gyro 23.00 -669.00 -1123.00
gyro -14.00 -146.00 -131.00 ||| gyro 51.00 -672.00 -1180.00
gyro 0.00 -134.00 -115.00 ||| gyro -4.00 -706.00 -1171.00
gyro -6.00 -145.00 -139.00 ||| gyro -160.00 -730.00 -1147.00
gyro -16.00 -165.00 -152.00 ||| gyro -82.00 -705.00 -1112.00
gyro 14.00 -152.00 -147.00 ||| gyro -246.00 -696.00 -1089.00
gyro -14.00 -162.00 -137.00 ||| gyro 33.00 -705.00 -1162.00
gyro -22.00 -174.00 -152.00 ||| gyro -31.00 -664.00 -1161.00
gyro -1.00 -138.00 -131.00 ||| gyro 137.00 -642.00 -1141.00
gyro 4.00 -173.00 -134.00 ||| gyro 165.00 -652.00 -1168.00
gyro 9.00 -152.00 -133.00 ||| gyro -23.00 -666.00 -1111.00
gyro -12.00 -146.00 -134.00 ||| gyro 64.00 -697.00 -1128.00
gyro -20.00 -132.00 -141.00 ||| gyro -221.00 -695.00 -1046.00
gyro -7.00 -143.00 -141.00 ||| gyro -122.00 -684.00 -1116.00
gyro -7.00 -118.00 -150.00 ||| gyro -188.00 -699.00 -1097.00
gyro -9.00 -148.00 -126.00 ||| gyro -56.00 -686.00 -1107.00
gyro 6.00 -169.00 -130.00 ||| gyro 132.00 -665.00 -1152.00
gyro -1.00 -150.00 -119.00 ||| gyro 22.00 -615.00 -1144.00
gyro 12.00 -157.00 -120.00 ||| gyro 212.00 -678.00 -1163.00
gyro 5.00 -156.00 -153.00 ||| gyro -42.00 -745.00 -1169.00
gyro -2.00 -143.00 -130.00 ||| gyro 22.00 -732.00 -1171.00
gyro 3.00 -129.00 -142.00 ||| gyro -145.00 -730.00 -1123.00
gyro -2.00 -132.00 -154.00 ||| gyro -147.00 -705.00 -1140.00
gyro -9.00 -120.00 -132.00 ||| gyro -85.00 -691.00 -1143.00
gyro -12.00 -156.00 -153.00 ||| gyro -157.00 -696.00 -1087.00
gyro -11.00 -149.00 -141.00 ||| gyro 67.00 -681.00 -1095.00
gyro -21.00 -167.00 -140.00 ||| gyro -72.00 -673.00 -1100.00
gyro 19.00 -156.00 -138.00 ||| gyro 76.00 -687.00 -1139.00
gyro -17.00 -135.00 -144.00 ||| gyro -55.00 -693.00 -1107.00
gyro -6.00 -136.00 -129.00 ||| gyro -72.00 -721.00 -1091.00
gyro 26.00 -146.00 -146.00 ||| gyro -42.00 -681.00 -1084.00
gyro 5.00 -133.00 -125.00 ||| gyro -135.00 -694.00 -1101.00
gyro -1.00 -152.00 -124.00 ||| gyro 5.00 -674.00 -1173.00
gyro 1.00 -144.00 -116.00 ||| gyro -81.00 -676.00 -1110.00
gyro -8.00 -140.00 -127.00 ||| gyro 32.00 -671.00 -1144.00
gyro 17.00 -144.00 -144.00 ||| gyro 60.00 -682.00 -1163.00
gyro 8.00 -169.00 -122.00 ||| gyro 16.00 -671.00 -1151.00
gyro 16.00 -163.00 -128.00 ||| gyro 134.00 -703.00 -1200.00
gyro -16.00 -178.00 -133.00 ||| gyro -62.00 -671.00 -1127.00
gyro -2.00 -157.00 -141.00 ||| gyro 128.00 -738.00 -1190.00
gyro -32.00 -147.00 -134.00 ||| gyro -189.00 -664.00 -1139.00
在我看来，好像是两个MPU-6050的数据计算方式不同（它们现在应该没有任何功能）。最后，我希望它的旋转数据映射到0-359。我知道Jeff Rowberg有一个很好的库，但我想自己来做，这样我就能理解它，以便将来可以修改它。我的代码。
#include <Wire.h>
long accelX, accelY, accelZ;
float gForceX, gForceY, gForceZ, gyroX, gyroY, gyroZ,rotX, rotY, rotZ;
long accelX2, accelY2, accelZ2;
float gForceX2, gForceY2, gForceZ2;
void setup(){
//write all ADO's to high to raise thieR adress to 0x69
pinMode(22, OUTPUT);
digitalWrite(22, HIGH);
pinMode(23, OUTPUT);
digitalWrite(23, HIGH);
pinMode(24, OUTPUT);
digitalWrite(24, HIGH);
Wire.begin();
Wire.beginTransmission(0x69);
Wire.write(0x6B);
Wire.write(0b00000000);
Wire.endTransmission();
Wire.beginTransmission(0x69);
Wire.write(0x1B);
Wire.write(0x00000000);
Wire.endTransmission();
Wire.beginTransmission(0x69);
Wire.write(0x1C);
Wire.write(0b00000000);
Wire.endTransmission();
Wire.begin();
Wire.beginTransmission(0x68);
Wire.write(0x6B);
Wire.write(0b00000000);
Wire.endTransmission();
Wire.beginTransmission(0x68);
Wire.write(0x1B);
Wire.write(0x00000000);
Wire.endTransmission();
Wire.beginTransmission(0x68);
Wire.write(0x1C);
Wire.write(0b00000000);
Wire.endTransmission();
Serial.begin(115200);
}
void loop(){
GetMpuValue(22);
Serial.print("\t ||| \t");
GetMpuValue(23);
Serial.println("\t");
}
void GetMpuValue(int pin){
digitalWrite(pin, LOW);
Wire.beginTransmission(0x68);
Wire.write(0x3B);
Wire.endTransmission();
Wire.requestFrom(0x68,6);
Wire.beginTransmission(0x68);
Wire.write(0x43);
Wire.endTransmission();
Wire.requestFrom(0x68,6);
while(Wire.available() < 6);
gyroX = Wire.read()<<8|Wire.read();
gyroY = Wire.read()<<8|Wire.read();
gyroZ = Wire.read()<<8|Wire.read();
digitalWrite(pin, HIGH);
Serial.print("gyro\t");
Serial.print(gyroX);
Serial.print("\t");
Serial.print(gyroY);
Serial.print("\t");
Serial.print(gyroZ);
}
我的代码：
2个回答
1
投票
投票
陀螺仪的配置是由这些线给出的，所以它们必须有相同的配置。
Wire.beginTransmission(0x68); // Start communication with MPU-1
Wire.write(0x1B); // Request the GYRO_CONFIG register
Wire.write(0x00); // Apply the desired configuration to the register : + or -250°/s
Wire.endTransmission(); // End the transmission
Wire.beginTransmission(0x69); // Start communication with MPU-2
Wire.write(0x1B); // Request the GYRO_CONFIG register
Wire.write(0x00); // Apply the desired configuration to the register : + or -250°/s
Wire.endTransmission(); // End the transmission
所以它们必须有相同的配置... ...
顺便说一下，陀螺仪的配置是由......给出的。
所以在你的程序中，我看到MPU的微积分地址是0X68，而不是地址0x69，这是一个多路复用器结点吗？
另一件事我没有看到两个MPU的滤波器配置：(我不知道是否所有的MPU都有这个功能，请阅读你的MPU的数据表，见我的评论))
Wire.beginTransmission(0x68); // Start communication with MPU
Wire.write(0x1A); // Request the CONFIG register
Wire.write(0x03); // Apply the desired config to the register : DLPF about 43Hz
Wire.endTransmission(); // End the transmission
如果您想从原始数据中读取角度测量值，您只需除以131，如图所示：（您已将FS_SEL选为0（-+250degsec），所以您的值是rawvalue 131 = degseconde。
你看250 x 131 = 32750 (int max value = 32767)
只是有一件事，一个陀螺仪累积漂移看 此处,
所以，我的完整代码，我用我的设置。
我的完整样本
//----------------------------------------------------------------------------------------------------------------------
#include <Wire.h>
//----------------------------------------------------------------------------------------------------------------------
#define YAW 0
#define PITCH 1
#define ROLL 2
#define X 0 // X axis
#define Y 1 // Y axis
#define Z 2 // Z axis
#define MPU_ADDRESS 0x68 // I2C address of the MPU-6050
#define FREQ 250 // Sampling frequency
#define SSF_GYRO 65.5 // Sensitivity Scale Factor of the gyro from datasheet
//----------------------------------------------------------------------------------------------------------------------
// The RAW values got from gyro (in °/sec) in that order: X, Y, Z
int gyro_raw[3] = {0, 0, 0};
// Average gyro offsets of each axis in that order: X, Y, Z
long gyro_offset[3] = {0, 0, 0};
// Calculated angles from gyro's values in that order: X, Y, Z
float gyro_angle[3] = {0, 0, 0};
// The RAW values got from accelerometer (in m/sec²) in that order: X, Y, Z
int acc_raw[3] = {0 , 0 , 0};
// Calculated angles from accelerometer's values in that order: X, Y, Z
float acc_angle[3] = {0, 0, 0};
// Total 3D acceleration vector in m/s²
long acc_total_vector;
/**
* Real measures on 3 axis calculated from gyro AND accelerometer in that order : Yaw, Pitch, Roll
* - Left wing up implies a positive roll
* - Nose up implies a positive pitch
* - Nose right implies a positive yaw
*/
float measures[3] = {0, 0, 0};
// MPU's temperature
int temperature;
// Init flag set to TRUE after first loop
boolean initialized;
unsigned int period; // Sampling period
unsigned long loop_timer;
//----------------------------------------------------------------------------------------------------------------------
void setup() {
// Serial.begin(57600); Only for debug
Wire.begin();
TWBR = 24; // 400kHz I2C clock (200kHz if CPU is 8MHz)
setupMpu6050Registers();
calibrateMpu6050();
loop_timer = micros();
period = (1000000 / FREQ) ; // Sampling period in µs
}
void loop() {
readSensor();
calculateAngles();
while (micros() - loop_timer < period);
loop_timer = micros();
}
0
投票
投票
代码末尾。
/**
* Configure gyro and accelerometer precision as following:
* - accelerometer: ±8g
* - gyro: ±500°/s
*
* @see https://www.invensense.com/wp-content/uploads/2015/02/MPU-6000-Register-Map1.pdf
* @return void
*/
void setupMpu6050Registers() {
// Configure power management
Wire.beginTransmission(MPU_ADDRESS); // Start communication with MPU
Wire.write(0x6B); // Request the PWR_MGMT_1 register
Wire.write(0x00); // Apply the desired configuration to the register
Wire.endTransmission(); // End the transmission
// Configure the gyro's sensitivity
Wire.beginTransmission(MPU_ADDRESS); // Start communication with MPU
Wire.write(0x1B); // Request the GYRO_CONFIG register
Wire.write(0x08); // Apply the desired configuration to the register : ±500°/s
Wire.endTransmission(); // End the transmission
// Configure the acceleromter's sensitivity
Wire.beginTransmission(MPU_ADDRESS); // Start communication with MPU
Wire.write(0x1C); // Request the ACCEL_CONFIG register
Wire.write(0x10); // Apply the desired configuration to the register : ±8g
Wire.endTransmission(); // End the transmission
// Configure low pass filter if exists
Wire.beginTransmission(MPU_ADDRESS); // Start communication with MPU
Wire.write(0x1A); // Request the CONFIG register
Wire.write(0x03); // Set Digital Low Pass Filter about ~43Hz
Wire.endTransmission(); // End the transmission
}
/**
* Calibrate MPU6050: take 2000 samples to calculate average offsets.
* During this step, the quadcopter needs to be static and on a horizontal surface.
*
* This function also sends low throttle signal to each ESC to init and prevent them beeping annoyingly.
*
* This function might take ~2sec for 2000 samples.
*
* @return void
*/
void calibrateMpu6050()
{
int max_samples = 2000;
for (int i = 0; i < max_samples; i++) {
readSensor();
gyro_offset[X] += gyro_raw[X];
gyro_offset[Y] += gyro_raw[Y];
gyro_offset[Z] += gyro_raw[Z];
// Just wait a bit before next loop
delay(3);
}
// Calculate average offsets
gyro_offset[X] /= max_samples;
gyro_offset[Y] /= max_samples;
gyro_offset[Z] /= max_samples;
}
/**
* Request raw values from MPU6050.
*
* @return void
*/
void readSensor() {
Wire.beginTransmission(MPU_ADDRESS); // Start communicating with the MPU-6050
Wire.write(0x3B); // Send the requested starting register
Wire.endTransmission(); // End the transmission
Wire.requestFrom(MPU_ADDRESS, 14); // Request 14 bytes from the MPU-6050
// Wait until all the bytes are received
while (Wire.available() < 14);
acc_raw[X] = Wire.read() << 8 | Wire.read(); // Add the low and high byte to the acc_raw[X] variable
acc_raw[Y] = Wire.read() << 8 | Wire.read(); // Add the low and high byte to the acc_raw[Y] variable
acc_raw[Z] = Wire.read() << 8 | Wire.read(); // Add the low and high byte to the acc_raw[Z] variable
temperature = Wire.read() << 8 | Wire.read(); // Add the low and high byte to the temperature variable
gyro_raw[X] = Wire.read() << 8 | Wire.read(); // Add the low and high byte to the gyro_raw[X] variable
gyro_raw[Y] = Wire.read() << 8 | Wire.read(); // Add the low and high byte to the gyro_raw[Y] variable
gyro_raw[Z] = Wire.read() << 8 | Wire.read(); // Add the low and high byte to the gyro_raw[Z] variable
}
/**
* Calculate real angles from gyro and accelerometer's values
*/
void calculateAngles()
{
calculateGyroAngles();
calculateAccelerometerAngles();
if (initialized) {
// Correct the drift of the gyro with the accelerometer
gyro_angle[X] = gyro_angle[X] * 0.9996 + acc_angle[X] * 0.0004;
gyro_angle[Y] = gyro_angle[Y] * 0.9996 + acc_angle[Y] * 0.0004;
} else {
// At very first start, init gyro angles with accelerometer angles
gyro_angle[X] = acc_angle[X];
gyro_angle[Y] = acc_angle[Y];
initialized = true;
}
// To dampen the pitch and roll angles a complementary filter is used
measures[ROLL] = measures[ROLL] * 0.9 + gyro_angle[X] * 0.1;
measures[PITCH] = measures[PITCH] * 0.9 + gyro_angle[Y] * 0.1;
measures[YAW] = -gyro_raw[Z] / SSF_GYRO; // Store the angular motion for this axis
}
/**
* Calculate pitch & roll angles using only the gyro.
*/
void calculateGyroAngles()
{
// Subtract offsets
gyro_raw[X] -= gyro_offset[X];
gyro_raw[Y] -= gyro_offset[Y];
gyro_raw[Z] -= gyro_offset[Z];
// Angle calculation using integration
gyro_angle[X] += (gyro_raw[X] / (FREQ * SSF_GYRO));
gyro_angle[Y] += (-gyro_raw[Y] / (FREQ * SSF_GYRO)); // Change sign to match the accelerometer's one
// Transfer roll to pitch if IMU has yawed
gyro_angle[Y] += gyro_angle[X] * sin(gyro_raw[Z] * (PI / (FREQ * SSF_GYRO * 180)));
gyro_angle[X] -= gyro_angle[Y] * sin(gyro_raw[Z] * (PI / (FREQ * SSF_GYRO * 180)));
}
/**
* Calculate pitch & roll angles using only the accelerometer.
*/
void calculateAccelerometerAngles()
{
// Calculate total 3D acceleration vector : √(X² + Y² + Z²)
acc_total_vector = sqrt(pow(acc_raw[X], 2) + pow(acc_raw[Y], 2) + pow(acc_raw[Z], 2));
// To prevent asin to produce a NaN, make sure the input value is within [-1;+1]
if (abs(acc_raw[X]) < acc_total_vector) {
acc_angle[X] = asin((float)acc_raw[Y] / acc_total_vector) * (180 / PI); // asin gives angle in radian. Convert to degree multiplying by 180/pi
}
if (abs(acc_raw[Y]) < acc_total_vector) {
acc_angle[Y] = asin((float)acc_raw[X] / acc_total_vector) * (180 / PI);
}
}
最新问题
- 如何将 hazelcast 节点绑定到特定 IP 地址？
- 包中的名称与同名依赖项冲突
- 验证数据的输入形状无效
- 在 C 中的字符串数组中分配字符分段错误
- 我找不到使用 grype 出现但在 docker 镜像中找不到的包
- Android 资源包 - gradlew 错误地连接资源目录
- 构建具有多个入口点和集中（配置）文件的Python项目
- 单元测试：断言不起作用？
- expo 应用程序未在开发版本上运行：EXPO 流意外结束
- 如何编辑assets/notifications/styles.css？
- 如何停止 for 循环中的无限循环？ [已关闭]
- java代码中的问题：如何停止for循环中的无限循环？ [已关闭]
- 在 ggplot (plotnine) 中使用position_dodge2 分组条形图列定位，以防丢失观察结果
- 如何停止 for 循环中的无限循环？ [已关闭]
- 如何防止实体在 Pekko/Akka 中多次实体化？
- 通过引用传递字符串在 Atmel Studio 中不起作用
- Eclipse 关于 Java 中私有静态嵌套类的合成访问器的警告？
- 目录内数据的符号链接
- nano Firmware Flasher 中是否有 STM32F042K6T6 的实现
- 如何为 FakeItEasy 构建 HttpResponseHeaders
© www.soinside.com 2019 - 2024. All rights reserved.