|
|
发表于 2014-7-29 13:45:06
|
显示全部楼层
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[12];
int ii, packet_count, fifo_count;
long gyro_bias[3] = {0, 0, 0}, accel_bias[3] = {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
int gyrosensitivity = 131; // = 131 LSB/degrees/sec
int accelsensitivity = 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[0]); // read FIFO sample count
fifo_count = ((int)data[0] << 8) | data[1];
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[3] = {0, 0, 0}, gyro_temp[3] = {0, 0, 0};
readBytes(mpu9150,MPU9150_ADDRESS, FIFO_R_W, 12, &data[0]); // read data for averaging
accel_temp[0] = (int) (((int)data[0] << 8) | data[1] ) ; // Form signed 16-bit integer for each sample in FIFO
accel_temp[1] = (int) (((int)data[2] << 8) | data[3] ) ;
accel_temp[2] = (int) (((int)data[4] << 8) | data[5] ) ;
gyro_temp[0] = (int) (((int)data[6] << 8) | data[7] ) ;
gyro_temp[1] = (int) (((int)data[8] << 8) | data[9] ) ;
gyro_temp[2] = (int) (((int)data[10] << 8) | data[11]) ;
accel_bias[0] += (long) accel_temp[0]; // Sum individual signed 16-bit biases to get accumulated signed 32-bit biases
accel_bias[1] += (long) accel_temp[1];
accel_bias[2] += (long) accel_temp[2];
gyro_bias[0] += (long) gyro_temp[0];
gyro_bias[1] += (long) gyro_temp[1];
gyro_bias[2] += (long) gyro_temp[2];
}
accel_bias[0] /= (long) packet_count; // Normalize sums to get average count biases
accel_bias[1] /= (long) packet_count;
accel_bias[2] /= (long) packet_count;
gyro_bias[0] /= (long) packet_count;
gyro_bias[1] /= (long) packet_count;
gyro_bias[2] /= (long) packet_count;
if(accel_bias[2] > 0.0) {accel_bias[2] -= (long) accelsensitivity;} // Remove gravity from the z-axis accelerometer bias calculation
else {accel_bias[2] += (long) accelsensitivity;}
// Construct the gyro biases for push to the hardware gyro bias registers, which are reset to zero upon device startup
data[0] = (-gyro_bias[0]/4 >> 8) & 0xFF; // Divide by 4 to get 32.9 LSB per deg/s to conform to expected bias input format
data[1] = (-gyro_bias[0]/4) & 0xFF; // Biases are additive, so change sign on calculated average gyro biases
data[2] = (-gyro_bias[1]/4 >> 8) & 0xFF;
data[3] = (-gyro_bias[1]/4) & 0xFF;
data[4] = (-gyro_bias[2]/4 >> 8) & 0xFF;
data[5] = (-gyro_bias[2]/4) & 0xFF;
// Push gyro biases to hardware registers
writeByte(mpu9150,MPU9150_ADDRESS, XG_OFFS_USRH, data[0]);
writeByte(mpu9150,MPU9150_ADDRESS, XG_OFFS_USRL, data[1]);
writeByte(mpu9150,MPU9150_ADDRESS, YG_OFFS_USRH, data[2]);
writeByte(mpu9150,MPU9150_ADDRESS, YG_OFFS_USRL, data[3]);
writeByte(mpu9150,MPU9150_ADDRESS, ZG_OFFS_USRH, data[4]);
writeByte(mpu9150,MPU9150_ADDRESS, ZG_OFFS_USRL, data[5]);
// Output scaled gyro biases for display in the main program
dest1[0] = (float) gyro_bias[0]/(float) gyrosensitivity;
dest1[1] = (float) gyro_bias[1]/(float) gyrosensitivity;
dest1[2] = (float) gyro_bias[2]/(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[3] = {0, 0, 0}; // A place to hold the factory accelerometer trim biases
readBytes(mpu9150,MPU9150_ADDRESS, XA_OFFSET_H, 2, &data[0]); // Read factory accelerometer trim values
accel_bias_reg[0] = (int) ((int)data[0] << 8) | data[1];
readBytes(mpu9150,MPU9150_ADDRESS, YA_OFFSET_H, 2, &data[0]);
accel_bias_reg[1] = (int) ((int)data[0] << 8) | data[1];
readBytes(mpu9150,MPU9150_ADDRESS, ZA_OFFSET_H, 2, &data[0]);
accel_bias_reg[2] = (int) ((int)data[0] << 8) | data[1];
long mask = 1; // Define mask for temperature compensation bit 0 of lower byte of accelerometer bias registers
ubyte mask_bit[3] = {0, 0, 0}; // Define array to hold mask bit for each accelerometer bias axis
for(ii = 0; ii < 3; ii++) {
if(accel_bias_reg[ii] & mask) mask_bit[ii] = 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[0] -= (accel_bias[0]/8); // Subtract calculated averaged accelerometer bias scaled to 2048 LSB/g (16 g full scale)
accel_bias_reg[1] -= (accel_bias[1]/8);
accel_bias_reg[2] -= (accel_bias[2]/8);
data[0] = (accel_bias_reg[0] >> 8) & 0xFF;
data[1] = (accel_bias_reg[0]) & 0xFF;
data[1] = data[1] | mask_bit[0]; // preserve temperature compensation bit when writing back to accelerometer bias registers
data[2] = (accel_bias_reg[1] >> 8) & 0xFF;
data[3] = (accel_bias_reg[1]) & 0xFF;
data[3] = data[3] | mask_bit[1]; // preserve temperature compensation bit when writing back to accelerometer bias registers
data[4] = (accel_bias_reg[2] >> 8) & 0xFF;
data[5] = (accel_bias_reg[2]) & 0xFF;
data[5] = data[5] | mask_bit[2]; // 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[0]);
writeByte(mpu9150,MPU9150_ADDRESS, XA_OFFSET_L_TC, data[1]);
writeByte(mpu9150,MPU9150_ADDRESS, YA_OFFSET_H, data[2]);
writeByte(mpu9150,MPU9150_ADDRESS, YA_OFFSET_L_TC, data[3]);
writeByte(mpu9150,MPU9150_ADDRESS, ZA_OFFSET_H, data[4]);
writeByte(mpu9150,MPU9150_ADDRESS, ZA_OFFSET_L_TC, data[5]);
// Output scaled accelerometer biases for display in the main program
dest2[0] = (float)accel_bias[0]/(float)accelsensitivity;
dest2[1] = (float)accel_bias[1]/(float)accelsensitivity;
dest2[2] = (float)accel_bias[2]/(float)accelsensitivity;
}
// I2C functions to get easier all values
bool waitForI2CBus(tSensors link)
{
while (true)
{
switch (nI2CStatus[link])
{
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;
}
ubyte readByte( tSensors link, ubyte DevAdd,ubyte RegAddr)
{
ubyte buffer[1];
sReadmsg.nMsgSize = 2;
sReadmsg.nDeviceAddress = DevAdd;
sReadmsg.nLocationPtr = RegAddr;
sendI2CMsg(link,&sReadmsg.nMsgSize, 1 );
if (waitForI2CBus(link))
{
readI2CReply(link,&buffer[0],1 );
//nxtDisplayTextLine(6, "who:%d",buffer[0]);
}
return buffer[0];
}
void readBytes(tSensors link,ubyte DevAdd,ubyte RegAddr,ubyte count,ubyte *dest){
ubyte buffer[6];
sReadmsg.nMsgSize = 2;
sReadmsg.nDeviceAddress = DevAdd;
sReadmsg.nLocationPtr = RegAddr;
sendI2CMsg(link,&sReadmsg.nMsgSize,count);
if (waitForI2CBus(link))
{
readI2CReply(link, &buffer[0],count);
}
for (ubyte i=0;i<count;i++){
dest = buffer;}
} |
|
楼主
。单纯依靠6050的陀螺仪，四轴不旋转的飞几秒，会突然不受控的掉下来，我猜想是yaw角漂走了，导致PID拉不会来。我现在也正在弄HMC5883L也想搞椭圆拟合，或者加进IMU,做成AHRSupdate。我这里有椭圆拟合的资料，正准备研究，还有AHRS的算法，希望对你有所帮助。
