IMUupdate姿态解算的问题 (amobbs.com 阿莫电子技术论坛) -

caiming1234567 发表于 2014-5-19 13:49:35

IMUupdate姿态解算的问题

最近玩用Arduino,用到MPU6050,解算算法如下：
//*****************????**********************/
//***************
//???????
//***************
void init_BaseIMU(void) {
for (byte axis = XAXIS; axis <= ZAXIS; axis++) {
EularAngle = 0.0;
}
}

////////////////////////////////////////////////////////////////////////////////
// Initialize IMU
////////////////////////////////////////////////////////////////////////////////

void IMU_Init(void)
{
init_BaseIMU();
q0 = 1.0;
q1 = 0.0;
q2 = 0.0;
q3 = 0.0;
exInt = 0.0;
eyInt = 0.0;
ezInt = 0.0;

previousEx = 0;
previousEy = 0;
previousEz = 0;

Kp = 0.2; // 2.0;
Ki = 0.0005; //0.005;
}
////////////////////////////////////////////////////////////////////////////////
// IMU_Update
////////////////////////////////////////////////////////////////////////////////
void IMU_Update(float gx, float gy, float gz, float ax, float ay, float az, float G_Dt) {

float norm;
float vx, vy, vz;
float q0i, q1i, q2i, q3i;
float ex, ey, ez;

halfT = G_Dt/2;

// normalise the measurements
norm = sqrt(ax*ax + ay*ay + az*az);
ax = ax / norm;
ay = ay / norm;
az = az / norm;

// estimated direction of gravity and flux (v and w)
vx = 2*(q1*q3 - q0*q2);
vy = 2*(q0*q1 + q2*q3);
vz = q0*q0 - q1*q1 - q2*q2 + q3*q3;

// error is sum of cross product between reference direction of fields and direction measured by sensors
ex = (vy*az - vz*ay);
ey = (vz*ax - vx*az);
ez = (vx*ay - vy*ax);

// integral error scaled integral gain
exInt = exInt + ex*Ki;
if (isSwitched(previousEx,ex)) {
exInt = 0.0;
}
previousEx = ex;

eyInt = eyInt + ey*Ki;
if (isSwitched(previousEy,ey)) {
eyInt = 0.0;
}
previousEy = ey;

ezInt = ezInt + ez*Ki;
if (isSwitched(previousEz,ez)) {
ezInt = 0.0;
}
previousEz = ez;

// adjusted gyroscope measurements
gx = gx + Kp*ex + exInt;
gy = gy + Kp*ey + eyInt;
gz = gz + Kp*ez + ezInt;

// integrate quaternion rate and normalise
q0i = (-q1*gx - q2*gy - q3*gz) * halfT;
q1i = ( q0*gx + q2*gz - q3*gy) * halfT;
q2i = ( q0*gy - q1*gz + q3*gx) * halfT;
q3i = ( q0*gz + q1*gy - q2*gx) * halfT;
q0 += q0i;
q1 += q1i;
q2 += q2i;
q3 += q3i;

// normalise quaternion
norm = sqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3);
q0 = q0 / norm;
q1 = q1 / norm;
q2 = q2 / norm;
q3 = q3 / norm;
}
////////////////////////////////////////////////////////////////////////////////
// Calculate IMU
////////////////////////////////////////////////////////////////////////////////
void IMU_Cal(float roll,float pitch, float yaw, float longitudinalAccel, float lateralAccel, float verticalAccel, float G_DT) {
IMU_Update(roll,pitch,yaw,longitudinalAccel,lateralAccel,verticalAccel,G_DT);
/* Angle = atan2(2 * (q0*q1 + q2*q3), 1 - 2 *(q1*q1 + q2*q2)); //ROLL
Angle = asin(2 * (q0*q2 - q1*q3));                         //PITCH
Angle = atan2(2 * (q0*q3 + q1*q2), 1 - 2 *(q2*q2 + q3*q3)); //YAW*/

EularAngle = atan2(2 * (q0*q1 + q2*q3), 1 - 2 *(q1*q1 + q2*q2)); //ROLL
EularAngle = asin(2 * (q0*q2 - q1*q3));                         //PITCH
EularAngle = atan2(2 * (q0*q3 + q1*q2), 1 - 2 *(q2*q2 + q3*q3));
}
用到的是 Madgwick的算法，但是数据输出会出现几个问题：
1.较好的情况：上电打印输出都是三轴都是0.0,但是有时候，ROLL会从180左右降到零，YAW值有偏差。静止的状态。
2.动态，如果我只在X轴上转动固定住某个角度，当然会输出角度值，其他两轴的角度几乎是0，但是过一段时间其他两轴会增至3.14（弧度），我猜测是不是和KP，KI有关的。
求解答！

BLACKBLUE007 发表于 2014-6-14 10:27:26

正好我也在试用这个算法,也是在ARDUINO上!

你的代码只是其中的一段，所以无从知道你的HALFT取值是多大？另外你的四元素初值是用了（1，0，0，0）？应该先用加计的值算出欧拉（roll,pitch),然后由欧拉出初始的四元素.......

如果KP，KI不能让你快速收敛，回到初值，改half T试试吧........
（我和你用的代码不一样，我是用了另外一个有MAG数据的 Madgwick算法，但是没有接入地磁，用16M晶振的ARDUINO MINI， half T我试了一下，实际在6-7ms的一半！

caiming1234567 发表于 2014-6-17 11:26:22

本帖最后由 caiming1234567 于 2014-6-17 11:37 编辑

BLACKBLUE007 发表于 2014-6-14 10:27
正好我也在试用这个算法,也是在ARDUINO上!

你的代码只是其中的一段，所以无从知道你的HALFT取值是多大？另 ...

已经没问题了，是加速度值校准问题，算出来没问题，除了偏航有点问题，没有校正。

小菜鸟001 发表于 2014-7-19 13:28:56

caiming1234567 发表于 2014-6-17 11:26
已经没问题了，是加速度值校准问题，算出来没问题，除了偏航有点问题，没有校正。 ...

偏航似乎不能矫正，因为加速度计测不到

小菜鸟001 发表于 2014-7-19 13:33:00

我也是用的这个算法，pitch角超过80度就会变成0，你的是这样吗？

caiming1234567 发表于 2014-7-25 11:35:41

小菜鸟001 发表于 2014-7-19 13:33
我也是用的这个算法，pitch角超过80度就会变成0，你的是这样吗？

没有，是0~89~0

幽灵盾 发表于 2014-7-25 13:37:25

BLACKBLUE007 发表于 2014-6-14 10:27
正好我也在试用这个算法,也是在ARDUINO上!

你的代码只是其中的一段，所以无从知道你的HALFT取值是多大？另 ...

请教，我看到过用加计算出欧拉角，再用欧拉角初始化四元数的，也有直接用（1,0，0，0）初始化四元数的，但是用（1,0，0，0）的，有一个初始化200次的采样，不知这两种有什么区别，请不吝赐教。

幽灵盾 发表于 2014-7-25 13:41:24

BLACKBLUE007 发表于 2014-6-14 10:27
正好我也在试用这个算法,也是在ARDUINO上!

你的代码只是其中的一段，所以无从知道你的HALFT取值是多大？另 ...

还有，有的程序中注释这个halft必须是采样周期的一半，不能随便调整这个参数吧？难道这是理论和实际的差别？

BLACKBLUE007 发表于 2014-7-25 14:56:42

幽灵盾发表于 2014-7-25 13:41
还有，有的程序中注释这个halft必须是采样周期的一半，不能随便调整这个参数吧？难道这是理论和实际的差 ...

1,{1,0,0,0}初值也可以的,好象前提是你的IMU放置要和代码中规定的一致!
2,half T可以变的,据原文讲应该是IMUUPDATA的代码执行时间的一半,我试过,用比这"一半"时间大一点的数进去,没啥问题.......
3,人家取200次ACC值然后再做初始四元,可能仅仅是为校准加计吧!但无论如何,MPU6050,9150都一个德行,零飘就是无论怎么校,起码有+/-0.8%的数值误码率差!

我也是门外汉,不瞎折腾的,以上可能有不对.......

另外,求教一下,罗盘椭圆球拟合有资料么？给我资料吧如果有.....

幽灵盾 发表于 2014-7-25 17:06:53

非常感谢你的热心回答，我大二，研究四轴也就一个多月，也是门外汉。然后发现如果不进行yaw的控制四轴也能飞起来，不过会一直旋转，然后就飞天花板上去了{:titter:}。单纯依靠6050的陀螺仪，四轴不旋转的飞几秒，会突然不受控的掉下来，我猜想是yaw角漂走了，导致PID拉不会来。我现在也正在弄HMC5883L也想搞椭圆拟合，或者加进IMU,做成AHRSupdate。我这里有椭圆拟合的资料，正准备研究，还有AHRS的算法，希望对你有所帮助。

幽灵盾 发表于 2014-7-25 17:08:04

BLACKBLUE007 发表于 2014-7-25 14:56
1,{1,0,0,0}初值也可以的,好象前提是你的IMU放置要和代码中规定的一致!
2,half T可以变的,据原文讲应该是 ...

非常感谢你的热心回答，我大二，研究四轴也就一个多月，也是门外汉。然后发现如果不进行yaw的控制四轴也能飞起来，不过会一直旋转，然后就飞天花板上去了。单纯依靠6050的陀螺仪，四轴不旋转的飞几秒，会突然不受控的掉下来，我猜想是yaw角漂走了，导致PID拉不会来。我现在也正在弄HMC5883L也想搞椭圆拟合，或者加进IMU,做成AHRSupdate。我这里有椭圆拟合的资料，正准备研究，还有AHRS的算法，希望对你有所帮助。

caiming1234567 发表于 2014-7-27 14:17:19

BLACKBLUE007 发表于 2014-7-25 14:56
1,{1,0,0,0}初值也可以的,好象前提是你的IMU放置要和代码中规定的一致!
2,half T可以变的,据原文讲应该是 ...

我有matlab的程序，精确度要求不高，可以试试！

caiming1234567 发表于 2014-7-27 14:19:55

幽灵盾发表于 2014-7-25 13:41
还有，有的程序中注释这个halft必须是采样周期的一半，不能随便调整这个参数吧？难道这是理论和实际的差 ...

我程序里设置100HZ采样，halfT = 1/(2 * smaplefre) samplefre = 100.0f

caiming1234567 发表于 2014-7-27 14:21:47

BLACKBLUE007 发表于 2014-7-25 14:56
1,{1,0,0,0}初值也可以的,好象前提是你的IMU放置要和代码中规定的一致!
2,half T可以变的,据原文讲应该是 ...

可以发过来你的罗盘三轴的原始数据，要绕Z轴转两个360度的数据

BLACKBLUE007 发表于 2014-7-28 08:17:58

caiming1234567 发表于 2014-7-27 14:21
可以发过来你的罗盘三轴的原始数据，要绕Z轴转两个360度的数据

最小二乘法3D校准参数：
float mag_bais = {-26.1372, 78.1749,32.0521}, sclaeFactory_x ={ 227.5400,-4.1942, 5.3723}, sclaeFactory_y ={-4.1942,191.4929,2.2442} , sclaeFactory_z ={5.3723,2.2442,220.0720};

我的罗盘是AK8975:校准输出格式magdata[],0,1,2=x,y,z:

mx = (float)mx_raw - mag_bais
my = (float)my_raw - mag_bais
mz = (float)mz_raw - mag_bais

magdata = sclaeFactory_x * mx +sclaeFactory_x * my + sclaeFactory_x*mz;
magdata = sclaeFactory_y * mx +sclaeFactory_y * my + sclaeFactory_y*mz;
magdata = sclaeFactory_z * mx + sclaeFactory_z * my + sclaeFactory_z*mz;

lianx325 发表于 2014-7-28 18:21:54

楼主你好，我刚开始看这个算法，想问下下载的代码中，哪些部分是需要根据情况修改的
halfT如何确定
Kp，Ki如何得到
我现在得到的角度，无论怎么动，两个轴很小，一个轴接近180度。
用该算法需要注意些什么啊？
谢谢！

caiming1234567 发表于 2014-7-29 10:43:14

lianx325 发表于 2014-7-28 18:21
楼主你好，我刚开始看这个算法，想问下下载的代码中，哪些部分是需要根据情况修改的
halfT如何确定
Kp，Ki ...

1.你代码中执行解算函数的频率，即采样频率，我是这么理解的
2.PI的参数，看感觉，数据大了容易过调，就是说，你实际转了45度，输出数据显示先是出现大于45度的值，然后有慢慢回到45，这样是不行的，PI参数大了，2和0.005感觉还行，已经用到飞机上了，主要是准确性和灵敏度；
3.由于坐标系的不同，转欧拉角的公式不同

caiming1234567 发表于 2014-7-29 10:45:09

BLACKBLUE007 发表于 2014-7-28 08:17
最小二乘法3D校准参数：
float mag_bais = {-26.1372, 78.1749,32.0521}, sclaeFactory_x ={ 227. ...

我用的HMC5883l,看你的代码，对数据的处理不一样

lianx325 发表于 2014-7-29 11:37:23

caiming1234567 发表于 2014-7-29 10:43
1.你代码中执行解算函数的频率，即采样频率，我是这么理解的
2.PI的参数，看感觉，数据大了容易过调，就 ...

你初始化四元数了吗怎么初始化

BLACKBLUE007 发表于 2014-7-29 13:45:06

lianx325 发表于 2014-7-29 11:37
你初始化四元数了吗怎么初始化

1，先校准你的传感器，然后用ACCEL和COMPASS求你当前位置的欧拉角，再由欧拉角求当前位置的四元素即是你当前位置的初始四元素！
(论坛上有主方面的贴的，有详细介绍）
2，所有的工作，都必须先从校准原始传感器值开始，第二步是传感器值的单位化。
3，HALF T要尽可能让它有多快设多快，否则YPR数据出来会慢慢飘.......
4，我的GYRO和ACCEL校准是参照DATASHEET做的，试过很多方法，感觉这种最好！AK8975，则是使用了其他地方找来的方法，目前还没有能力把它写在代码中进行自校准，这个数据用在代码中，YAW数据出来，无论ROLL，PITCH是不是同有时有输出，YAW不会跳变，所以感觉3D球形拟合对三轴罗盘的确有明显作用！

附上我用的传感器自校代码，也是网上找来的，但找的资料很多，记不得出处了（对作者表示感谢）！
（我改写的代码是用在LEGO NXT玩具上的，请自行修改）

void calibrateMPU9150(float * dest1, float * dest2)
{
ubyte data;
int ii, packet_count, fifo_count;
long gyro_bias= {0, 0, 0}, accel_bias = {0, 0, 0};

// reset device, reset all registers, clear gyro and accelerometer bias registers
writeByte(mpu9150,MPU9150_ADDRESS, PWR_MGMT_1, 0x80); // Write a one to bit 7 reset bit; toggle reset device
wait1Msec(200);

// get stable time source
// Set clock source to be PLL with x-axis gyroscope reference, bits 2:0 = 001
writeByte(mpu9150,MPU9150_ADDRESS, PWR_MGMT_1, 0x01);
writeByte(mpu9150,MPU9150_ADDRESS, PWR_MGMT_2, 0x00);
wait1Msec(200);

// Configure device for bias calculation
writeByte(mpu9150,MPU9150_ADDRESS, INT_ENABLE, 0x00); // Disable all interrupts
writeByte(mpu9150,MPU9150_ADDRESS, FIFO_EN, 0x00);    // Disable FIFO
writeByte(mpu9150,MPU9150_ADDRESS, PWR_MGMT_1, 0x00); // Turn on internal clock source
writeByte(mpu9150,MPU9150_ADDRESS, I2C_MST_CTRL, 0x00); // Disable I2C master
writeByte(mpu9150,MPU9150_ADDRESS, USER_CTRL, 0x00); // Disable FIFO and I2C master modes
writeByte(mpu9150,MPU9150_ADDRESS, USER_CTRL, 0x0C); // Reset FIFO and DMP
wait1Msec(100);

// Configure MPU6050 gyro and accelerometer for bias calculation
writeByte(mpu9150,MPU9150_ADDRESS, CONFIG, 0x01);    // Set low-pass filter to 188 Hz
writeByte(mpu9150,MPU9150_ADDRESS, SMPLRT_DIV, 0x00);// Set sample rate to 1 kHz
writeByte(mpu9150,MPU9150_ADDRESS, GYRO_CONFIG, 0x00);// Set gyro full-scale to 250 degrees per second, maximum sensitivity
writeByte(mpu9150,MPU9150_ADDRESS, ACCEL_CONFIG, 0x00); // Set accelerometer full-scale to 2 g, maximum sensitivity

intgyrosensitivity= 131; // = 131 LSB/degrees/sec
intaccelsensitivity = 16384;// = 16384 LSB/g

// Configure FIFO to capture accelerometer and gyro data for bias calculation
writeByte(mpu9150,MPU9150_ADDRESS, USER_CTRL, 0x40); // Enable FIFO
writeByte(mpu9150,MPU9150_ADDRESS, FIFO_EN, 0x78); // Enable gyro and accelerometer sensors for FIFO(max size 1024 bytes in MPU-6050)
wait1Msec(200);

// At end of sample accumulation, turn off FIFO sensor read
writeByte(mpu9150,MPU9150_ADDRESS, FIFO_EN, 0x00);    // Disable gyro and accelerometer sensors for FIFO
readBytes(mpu9150,MPU9150_ADDRESS, FIFO_COUNTH, 2, &data); // read FIFO sample count
fifo_count = ((int)data << 8) | data;
packet_count = fifo_count/12;// How many sets of full gyro and accelerometer data for averaging

for (ii = 0; ii < packet_count; ii++) {
int accel_temp = {0, 0, 0}, gyro_temp = {0, 0, 0};
readBytes(mpu9150,MPU9150_ADDRESS, FIFO_R_W, 12, &data); // read data for averaging
accel_temp = (int) (((int)data << 8) | data) ;// Form signed 16-bit integer for each sample in FIFO
accel_temp = (int) (((int)data << 8) | data) ;
accel_temp = (int) (((int)data << 8) | data) ;
gyro_temp= (int) (((int)data << 8) | data) ;
gyro_temp= (int) (((int)data << 8) | data) ;
gyro_temp= (int) (((int)data << 8) | data) ;

accel_bias += (long) accel_temp; // Sum individual signed 16-bit biases to get accumulated signed 32-bit biases
accel_bias += (long) accel_temp;
accel_bias += (long) accel_temp;
gyro_bias+= (long) gyro_temp;
gyro_bias+= (long) gyro_temp;
gyro_bias+= (long) gyro_temp;

}
accel_bias /= (long) packet_count; // Normalize sums to get average count biases
accel_bias /= (long) packet_count;
accel_bias /= (long) packet_count;
gyro_bias/= (long) packet_count;
gyro_bias/= (long) packet_count;
gyro_bias/= (long) packet_count;

if(accel_bias > 0.0) {accel_bias -= (long) accelsensitivity;}// Remove gravity from the z-axis accelerometer bias calculation
else {accel_bias += (long) accelsensitivity;}

// Construct the gyro biases for push to the hardware gyro bias registers, which are reset to zero upon device startup
data = (-gyro_bias/4>> 8) & 0xFF; // Divide by 4 to get 32.9 LSB per deg/s to conform to expected bias input format
data = (-gyro_bias/4)    & 0xFF; // Biases are additive, so change sign on calculated average gyro biases
data = (-gyro_bias/4>> 8) & 0xFF;
data = (-gyro_bias/4)    & 0xFF;
data = (-gyro_bias/4>> 8) & 0xFF;
data = (-gyro_bias/4)    & 0xFF;

// Push gyro biases to hardware registers
writeByte(mpu9150,MPU9150_ADDRESS, XG_OFFS_USRH, data);
writeByte(mpu9150,MPU9150_ADDRESS, XG_OFFS_USRL, data);
writeByte(mpu9150,MPU9150_ADDRESS, YG_OFFS_USRH, data);
writeByte(mpu9150,MPU9150_ADDRESS, YG_OFFS_USRL, data);
writeByte(mpu9150,MPU9150_ADDRESS, ZG_OFFS_USRH, data);
writeByte(mpu9150,MPU9150_ADDRESS, ZG_OFFS_USRL, data);

// Output scaled gyro biases for display in the main program
dest1 = (float) gyro_bias/(float) gyrosensitivity;
dest1 = (float) gyro_bias/(float) gyrosensitivity;
dest1 = (float) gyro_bias/(float) gyrosensitivity;

// Construct the accelerometer biases for push to the hardware accelerometer bias registers. These registers contain
// factory trim values which must be added to the calculated accelerometer biases; on boot up these registers will hold
// non-zero values. In addition, bit 0 of the lower byte must be preserved since it is used for temperature
// compensation calculations. Accelerometer bias registers expect bias input as 2048 LSB per g, so that
// the accelerometer biases calculated above must be divided by 8.

long accel_bias_reg = {0, 0, 0}; // A place to hold the factory accelerometer trim biases
readBytes(mpu9150,MPU9150_ADDRESS, XA_OFFSET_H, 2, &data); // Read factory accelerometer trim values
accel_bias_reg = (int) ((int)data << 8) | data;
readBytes(mpu9150,MPU9150_ADDRESS, YA_OFFSET_H, 2, &data);
accel_bias_reg = (int) ((int)data << 8) | data;
readBytes(mpu9150,MPU9150_ADDRESS, ZA_OFFSET_H, 2, &data);
accel_bias_reg = (int) ((int)data << 8) | data;

long mask = 1; // Define mask for temperature compensation bit 0 of lower byte of accelerometer bias registers
ubyte mask_bit = {0, 0, 0}; // Define array to hold mask bit for each accelerometer bias axis

for(ii = 0; ii < 3; ii++) {
if(accel_bias_reg & mask) mask_bit = 0x01; // If temperature compensation bit is set, record that fact in mask_bit
}

// Construct total accelerometer bias, including calculated average accelerometer bias from above
accel_bias_reg -= (accel_bias/8); // Subtract calculated averaged accelerometer bias scaled to 2048 LSB/g (16 g full scale)
accel_bias_reg -= (accel_bias/8);
accel_bias_reg -= (accel_bias/8);

data = (accel_bias_reg >> 8) & 0xFF;
data = (accel_bias_reg)    & 0xFF;
data = data | mask_bit; // preserve temperature compensation bit when writing back to accelerometer bias registers
data = (accel_bias_reg >> 8) & 0xFF;
data = (accel_bias_reg)    & 0xFF;
data = data | mask_bit; // preserve temperature compensation bit when writing back to accelerometer bias registers
data = (accel_bias_reg >> 8) & 0xFF;
data = (accel_bias_reg)    & 0xFF;
data = data | mask_bit; // preserve temperature compensation bit when writing back to accelerometer bias registers

// Push accelerometer biases to hardware registers
writeByte(mpu9150,MPU9150_ADDRESS, XA_OFFSET_H, data);
writeByte(mpu9150,MPU9150_ADDRESS, XA_OFFSET_L_TC, data);
writeByte(mpu9150,MPU9150_ADDRESS, YA_OFFSET_H, data);
writeByte(mpu9150,MPU9150_ADDRESS, YA_OFFSET_L_TC, data);
writeByte(mpu9150,MPU9150_ADDRESS, ZA_OFFSET_H, data);
writeByte(mpu9150,MPU9150_ADDRESS, ZA_OFFSET_L_TC, data);

// Output scaled accelerometer biases for display in the main program
dest2 = (float)accel_bias/(float)accelsensitivity;
dest2 = (float)accel_bias/(float)accelsensitivity;
dest2 = (float)accel_bias/(float)accelsensitivity;
}

// I2C functions to get easier all values
bool waitForI2CBus(tSensors link)
{
while (true)
{
switch (nI2CStatus)
{
   case NO_ERR:
      return true;
   case STAT_COMM_PENDING:
      break;
   case ERR_COMM_CHAN_NOT_READY:
      break;
   case ERR_COMM_BUS_ERR:
   return false;
}
EndTimeSlice();
}
}

bool writeByte(tSensors link, ubyte DevAdd, ubyte RegAddr, ubyte Command)
{
sOutput.nMsgSize = 3;
sOutput.nDeviceAddress = DevAdd;
sOutput.nLocationPtr = RegAddr;
sOutput.nCommand = Command;
sendI2CMsg(link, &sOutput.nMsgSize,0);
if (waitForI2CBus(link))
   return true;
else
return false;
}

ubytereadByte( tSensors link, ubyte DevAdd,ubyte RegAddr)
{
ubyte buffer;
sReadmsg.nMsgSize = 2;
sReadmsg.nDeviceAddress = DevAdd;
sReadmsg.nLocationPtr = RegAddr;

   sendI2CMsg(link,&sReadmsg.nMsgSize, 1 );
   if (waitForI2CBus(link))
   {
   readI2CReply(link,&buffer,1 );
   //nxtDisplayTextLine(6, "who:%d",buffer);
   }
   return buffer;
}

void readBytes(tSensors link,ubyte DevAdd,ubyte RegAddr,ubyte count,ubyte *dest){
ubyte buffer;
sReadmsg.nMsgSize = 2;
sReadmsg.nDeviceAddress = DevAdd;
sReadmsg.nLocationPtr = RegAddr;

sendI2CMsg(link,&sReadmsg.nMsgSize,count);
   if (waitForI2CBus(link))
   {
   readI2CReply(link, &buffer,count);
   }
   for (ubyte i=0;i<count;i++){
      dest = buffer;}
}

lianx325 发表于 2014-7-29 14:58:44

BLACKBLUE007 发表于 2014-7-29 13:45
1，先校准你的传感器，然后用ACCEL和COMPASS求你当前位置的欧拉角，再由欧拉角求当前位置的四元素即是你 ...

你好，我刚接触这个东西，可以加个好友聊聊吗？

caiming1234567 发表于 2014-7-30 10:06:23

lianx325 发表于 2014-7-29 11:37
你初始化四元数了吗怎么初始化

不是｛1，0，0，0｝吗？

时光发表于 2014-8-28 10:32:57

BLACKBLUE007 发表于 2014-6-14 10:27
正好我也在试用这个算法,也是在ARDUINO上!

你的代码只是其中的一段，所以无从知道你的HALFT取值是多大？另 ...

楼主，，请问那个Halft到底是怎么算出来的呀？？指的到底是什么的周期啊

nhw1234 发表于 2014-8-28 18:31:34

我是来学习学习的

Puppey 发表于 2014-9-9 18:56:49

caiming1234567 发表于 2014-7-27 14:19
我程序里设置100HZ采样，halfT = 1/(2 * smaplefre) samplefre = 100.0f

我也大三了 0 0 话说做的怎样了~~~~~~

zhao_chunyu9257 发表于 2016-5-10 17:19:12

BLACKBLUE007 发表于 2014-7-29 13:45
1，先校准你的传感器，然后用ACCEL和COMPASS求你当前位置的欧拉角，再由欧拉角求当前位置的四元素即是你 ...

请问，你还有3D校准的程序吗？我看你给出的程序是加速度计和陀螺仪的

kyle4812 发表于 2018-3-13 16:41:24

加速度计+陀螺仪能得到欧拉角吗

页: [1]

amobbs.com 阿莫电子技术论坛's Archiver

IMUupdate姿态解算的问题