kriswiner
commented
8 years ago Yes, I see what you are doing. I can also get it to work some of the time. I'll try your method and see if it improves the odds.
Open matherp opened 8 years ago
kriswiner
commented
8 years ago Yes, I see what you are doing. I can also get it to work some of the time. I'll try your method and see if it improves the odds.
gjwo
commented
8 years ago Same fix in Java - mask the bottom bits with 0xFE to clear bit zero, then the or operation will work correctly
// 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.
// XA_OFFSET is a 15 bit quantity with bits 14:7 in the high byte and 6:0 in the low byte with temperature compensation in bit0
// so having got it in a 16 bit short, and having preserved the bottom bit, the number must be shifted right by 1 or divide by 2
// to give the correct value for calculations. After calculations it must be shifted left by 1 or multiplied by 2 to get
// the bytes correct, then the preserved bit0 can be put back before the bytes are written to registers
short[] accelBiasReg = roMPU.read16BitRegisters( Registers.XA_OFFSET_H, 3);
System.out.print("accelBiasReg (16bit): "+Arrays.toString(accelBiasReg));
System.out.format(" [0x%X, 0x%X, 0x%X] %n",accelBiasReg[0],accelBiasReg[1],accelBiasReg[2]);
int mask = 0x01; // Define mask for temperature compensation bit 0 of lower byte of accelerometer bias registers
byte[] mask_bit = new byte[]{0, 0, 0}; // Define array to hold mask bit for each accelerometer bias axis
for(int s = 0; s < 3; s++) {
if((accelBiasReg[s] & mask)==1) mask_bit[s] = 0x01; // If temperature compensation bit is set, record that fact in mask_bit
//divide accelBiasReg by 2 to remove the bottom bit and preserve any sign (java has no unsigned 16 bit numbers)
accelBiasReg[s] /=2;
}
// Construct total accelerometer bias, including calculated average accelerometer bias from above
for (int i = 0; i<3; i++)
{
accelBiasReg[i] -= (accelBiasAvg[i]/8); // Subtract calculated averaged accelerometer bias scaled to 2048 LSB/g (16 g full scale)
accelBiasReg[i] *=2; //multiply by two to leave the bottom bit clear and but all the bits in the correct bytes
}
System.out.print("(accelBiasReg - biasAvg/8)*2 (16bit): "+Arrays.toString(accelBiasReg));
System.out.format(" [0x%X, 0x%X, 0x%X] %n",accelBiasReg[0],accelBiasReg[1],accelBiasReg[2]);
buffer = new byte[6];
// XA_OFFSET is a 15 bit quantity with bits 14:7 in the high byte and 6:0 in the low byte with temperature compensation in bit0
buffer[0] = (byte)((accelBiasReg[0] >> 8) & 0xFF); //Shift down and mask top 8 bits
buffer[1] = (byte)((accelBiasReg[0]) & 0xFE); //copy bits 7-1 clear bit 0
buffer[1] = (byte)(buffer[1] | mask_bit[0]); // preserve temperature compensation bit when writing back to accelerometer bias registers
buffer[2] = (byte)((accelBiasReg[1] >> 8) & 0xFF); //Shift down and mask top 8 bits
buffer[3] = (byte)((accelBiasReg[1]) & 0xFE); //copy bits 7-1 clear bit 0
buffer[3] = (byte)(buffer[3] | mask_bit[1]); // preserve temperature compensation bit when writing back to accelerometer bias registers
buffer[4] = (byte)((accelBiasReg[2] >> 8) & 0xFF); //Shift down and mask top 8 bits
buffer[5] = (byte)((accelBiasReg[2]) & 0xFE); //copy bits 7-1 clear bit 0
buffer[5] = (byte)(buffer[5] | mask_bit[2]); // preserve temperature compensation bit when writing back to accelerometer bias registers
System.out.print("accelBiasReg bytes: "+Arrays.toString(buffer));
System.out.format(" [0x%X, 0x%X, 0x%X, 0x%X, 0x%X, 0x%X]%n",buffer[0],buffer[1],buffer[2],buffer[3],buffer[4],buffer[5]);
// Apparently this is not working for the acceleration biases in the MPU-9250
// Are we handling the temperature correction bit properly? - see comments above
// Push accelerometer biases to hardware registers
roMPU.writeByteRegister(Registers.XA_OFFSET_H, buffer[0]);
roMPU.writeByteRegister(Registers.XA_OFFSET_L, buffer[1]);
roMPU.writeByteRegister(Registers.YA_OFFSET_H, buffer[2]);
roMPU.writeByteRegister(Registers.YA_OFFSET_L, buffer[3]);
roMPU.writeByteRegister(Registers.ZA_OFFSET_H, buffer[4]);
roMPU.writeByteRegister(Registers.ZA_OFFSET_L, buffer[5]);
bafonso
commented
7 years ago Here is how I have updated the code to reflect these changes. Can anyone check if this matches the java code?
void MPU9250::calibrateMPU9250(float * gyroBias, float * accelBias)
{
uint8_t data[12]; // data array to hold accelerometer and gyro x, y, z, data
uint16_t ii, packet_count, fifo_count;
int32_t gyro_bias[3] = {0, 0, 0}, accel_bias[3] = {0, 0, 0};
// reset device
// Write a one to bit 7 reset bit; toggle reset device
writeByte(MPU9250_ADDRESS, PWR_MGMT_1, 0x80);
delay(100);
// get stable time source; Auto select clock source to be PLL gyroscope
// reference if ready else use the internal oscillator, bits 2:0 = 001
writeByte(MPU9250_ADDRESS, PWR_MGMT_1, 0x01);
writeByte(MPU9250_ADDRESS, PWR_MGMT_2, 0x00);
delay(200);
// Configure device for bias calculation
writeByte(MPU9250_ADDRESS, INT_ENABLE, 0x00); // Disable all interrupts
writeByte(MPU9250_ADDRESS, FIFO_EN, 0x00); // Disable FIFO
writeByte(MPU9250_ADDRESS, PWR_MGMT_1, 0x00); // Turn on internal clock source
writeByte(MPU9250_ADDRESS, I2C_MST_CTRL, 0x00); // Disable I2C master
writeByte(MPU9250_ADDRESS, USER_CTRL, 0x00); // Disable FIFO and I2C master modes
writeByte(MPU9250_ADDRESS, USER_CTRL, 0x0C); // Reset FIFO and DMP
delay(15);
// Configure MPU6050 gyro and accelerometer for bias calculation
writeByte(MPU9250_ADDRESS, CONFIG, 0x01); // Set low-pass filter to 188 Hz
writeByte(MPU9250_ADDRESS, SMPLRT_DIV, 0x00); // Set sample rate to 1 kHz
writeByte(MPU9250_ADDRESS, GYRO_CONFIG, 0x00); // Set gyro full-scale to 250 degrees per second, maximum sensitivity
writeByte(MPU9250_ADDRESS, ACCEL_CONFIG, 0x00); // Set accelerometer full-scale to 2 g, maximum sensitivity
uint16_t gyrosensitivity = 131; // = 131 LSB/degrees/sec
uint16_t accelsensitivity = 16384; // = 16384 LSB/g
// Configure FIFO to capture accelerometer and gyro data for bias calculation
writeByte(MPU9250_ADDRESS, USER_CTRL, 0x40); // Enable FIFO
writeByte(MPU9250_ADDRESS, FIFO_EN, 0x78); // Enable gyro and accelerometer sensors for FIFO (max size 512 bytes in MPU-9150)
delay(40); // accumulate 40 samples in 40 milliseconds = 480 bytes
// At end of sample accumulation, turn off FIFO sensor read
writeByte(MPU9250_ADDRESS, FIFO_EN, 0x00); // Disable gyro and accelerometer sensors for FIFO
readBytes(MPU9250_ADDRESS, FIFO_COUNTH, 2, &data[0]); // read FIFO sample count
fifo_count = ((uint16_t)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++)
{
int16_t accel_temp[3] = {0, 0, 0}, gyro_temp[3] = {0, 0, 0};
readBytes(MPU9250_ADDRESS, FIFO_R_W, 12, &data[0]); // read data for averaging
accel_temp[0] = (int16_t) (((int16_t)data[0] << 8) | data[1] ); // Form signed 16-bit integer for each sample in FIFO
accel_temp[1] = (int16_t) (((int16_t)data[2] << 8) | data[3] );
accel_temp[2] = (int16_t) (((int16_t)data[4] << 8) | data[5] );
gyro_temp[0] = (int16_t) (((int16_t)data[6] << 8) | data[7] );
gyro_temp[1] = (int16_t) (((int16_t)data[8] << 8) | data[9] );
gyro_temp[2] = (int16_t) (((int16_t)data[10] << 8) | data[11]);
accel_bias[0] += (int32_t) accel_temp[0]; // Sum individual signed 16-bit biases to get accumulated signed 32-bit biases
accel_bias[1] += (int32_t) accel_temp[1];
accel_bias[2] += (int32_t) accel_temp[2];
gyro_bias[0] += (int32_t) gyro_temp[0];
gyro_bias[1] += (int32_t) gyro_temp[1];
gyro_bias[2] += (int32_t) gyro_temp[2];
}
accel_bias[0] /= (int32_t) packet_count; // Normalize sums to get average count biases
accel_bias[1] /= (int32_t) packet_count;
accel_bias[2] /= (int32_t) packet_count;
gyro_bias[0] /= (int32_t) packet_count;
gyro_bias[1] /= (int32_t) packet_count;
gyro_bias[2] /= (int32_t) packet_count;
if(accel_bias[2] > 0L) {accel_bias[2] -= (int32_t) accelsensitivity;} // Remove gravity from the z-axis accelerometer bias calculation
else {accel_bias[2] += (int32_t) 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(MPU9250_ADDRESS, XG_OFFSET_H, data[0]);
writeByte(MPU9250_ADDRESS, XG_OFFSET_L, data[1]);
writeByte(MPU9250_ADDRESS, YG_OFFSET_H, data[2]);
writeByte(MPU9250_ADDRESS, YG_OFFSET_L, data[3]);
writeByte(MPU9250_ADDRESS, ZG_OFFSET_H, data[4]);
writeByte(MPU9250_ADDRESS, ZG_OFFSET_L, data[5]);
// Output scaled gyro biases for display in the main program
gyroBias[0] = (float) gyro_bias[0]/(float) gyrosensitivity;
gyroBias[1] = (float) gyro_bias[1]/(float) gyrosensitivity;
gyroBias[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.
// XA_OFFSET is a 15 bit quantity with bits 14:7 in the high byte and 6:0 in the low byte with temperature compensation in bit0
// so having got it in a 16 bit short, and having preserved the bottom bit, the number must be shifted right by 1 or divide by 2
// to give the correct value for calculations. After calculations it must be shifted left by 1 or multiplied by 2 to get
// the bytes correct, then the preserved bit0 can be put back before the bytes are written to registers
int16_t accel_bias_reg[3] = {0, 0, 0}; // A place to hold the factory accelerometer trim biases
readBytes(MPU9250_ADDRESS, XA_OFFSET_H, 2, &data[0]); // Read factory accelerometer trim values
accel_bias_reg[0] = (((int16_t)data[0] << 8) | data[1]);
readBytes(MPU9250_ADDRESS, YA_OFFSET_H, 2, &data[0]);
accel_bias_reg[1] = (((int16_t)data[0] << 8) | data[1]);
readBytes(MPU9250_ADDRESS, ZA_OFFSET_H, 2, &data[0]);
accel_bias_reg[2] = (((int16_t)data[0] << 8) | data[1]);
// Until here we're still dealing w/ it as if it was 16bit. Now we need to figure out the temp info.
// bit 0 is the rightmost bit, where our temperature is. this is why our mask is 0x01, ie, 0000 0001 (for uint8_t)
uint16_t mask = 0x01; //=> (0000)0001 // Define mask for temperature compensation bit 0 of lower byte of accelerometer bias registers
uint8_t 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 ) {
mask_bit[ii] = 0x01; // If temperature compensation bit is set, record that fact in mask_bit
//divide accelBiasReg by 2 to remove the bottom bit and preserve any sign (java has no unsigned 16 bit numbers)
accel_bias_reg[ii] /=2;
}
}
// See https://github.com/kriswiner/MPU-9250/issues/77
// and https://github.com/kriswiner/MPU-9250/issues/49
for (int i=0; i< 3; i++) {
accel_bias_reg[i] += (int16_t)((accel_bias[i]/8) << 1); // bit shifting by 1 (<<1) is the same as multiplying by two
}
data[0] = (accel_bias_reg[0] >> 8) & 0xFF; //Shift down and mask top 8 bits
data[1] = (accel_bias_reg[0]) & 0xFE; //copy bits 7-1 clear bit 0
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]) & 0xFE;
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]) & 0xFE;
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(MPU9250_ADDRESS, XA_OFFSET_H, data[0]);
writeByte(MPU9250_ADDRESS, XA_OFFSET_L, data[1]);
writeByte(MPU9250_ADDRESS, YA_OFFSET_H, data[2]);
writeByte(MPU9250_ADDRESS, YA_OFFSET_L, data[3]);
writeByte(MPU9250_ADDRESS, ZA_OFFSET_H, data[4]);
writeByte(MPU9250_ADDRESS, ZA_OFFSET_L, data[5]);
// Output scaled accelerometer biases for display in the main program
accelBias[0] = (float)accel_bias[0]/(float)accelsensitivity;
accelBias[1] = (float)accel_bias[1]/(float)accelsensitivity;
accelBias[2] = (float)accel_bias[2]/(float)accelsensitivity;
}
Hi
I've been porting your code to run in MMBasic on the Micromite.
I think I've solved this issue unless it is already solved and the comment out of date.
The problem comes because the offsets are 15-bit signed numbers stored in the 15 most significant bits with bit 0 used for whatever. My clunky Basic code which seems to work properly is
` readBytes(MPU9250_ADDRESS, XA_OFFSET_H, 2, rData()) ' Read factory accelerometer trim values accel_bias_reg(0) = iint15((rData(0) << 7) OR ((rData(1) and &HFE)>>1)) if rData(1) and &H1 then mask_bit(0)=&H1 readBytes(MPU9250_ADDRESS, YA_OFFSET_H, 2, rData()) accel_bias_reg(1) = iint15((rData(0) << 7) OR ((rData(1) and &HFE)>>1)) if rData(1) and &H1 then mask_bit(1)=&H1 readBytes(MPU9250_ADDRESS, ZA_OFFSET_H, 2, rData()) accel_bias_reg(2) = iint15((rData(0) << 7) OR ((rData(1) and &HFE)>>1)) if rData(1) and &H1 then mask_bit(2)=&H1
' Construct total accelerometer bias, including calculated average accelerometer bias from above accel_bias_reg(0) = 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_reg(1) - (accel_bias(1)/8) accel_bias_reg(2) = accel_bias_reg(2) - (accel_bias(2)/8) rData(0) = (accel_bias_reg(0) \ 128) AND &HFF rData(1) = ((accel_bias_reg(0))<<1 ) AND &HFE rData(1) = rData(1) OR mask_bit(0) ' preserve temperature compensation bit when writing back to accelerometer bias registers rData(2) = (accel_bias_reg(1) \ 128) AND &HFF rData(3) = ((accel_bias_reg(1)) <<1) AND &HFE rData(3) = rData(3) OR mask_bit(1) ' preserve temperature compensation bit when writing back to accelerometer bias registers rData(4) = (accel_bias_reg(2) \ 128) AND &HFF rData(5) = ((accel_bias_reg(2))<<1) AND &HFE rData(5) = rData(5) OR mask_bit(2) ' preserve temperature compensation bit when writing back to accelerometer bias registers `
iint15 just dose a sign extend and I use integer division () because >> doesn't bring in the sign bit in MMBasic. Hopefully though you can see what I am doing