코드 일부만 써놓는건 무책임한거 같아서 전체코드 첨부
github.com/mokhwasomssi/stm32f4_hal_quadcopter
GitHub - mokhwasomssi/drone_with_gyroscopic_guard: Develop flight firmware and Design gyroscopic guard
Develop flight firmware and Design gyroscopic guard - GitHub - mokhwasomssi/drone_with_gyroscopic_guard: Develop flight firmware and Design gyroscopic guard
github.com
commit : Add calibration to icm20948 lib
센서에 offset을 저장하는 레지스터가 있다.
사용자가 계산한 offset을 이 레지스터에 넣어주면 자동으로 빼서 데이터를 출력해준다.
오프셋 레지스터에 값을 넣어주면 계속 데이터에 오프셋을 빼줄 필요가 없다는 거다.
// before
보정 값 = 센서 값 - 오프셋 값
// after
보정 값 = 센서 값
offset 구하는 건 어렵지 않다.
센서 데이터 받아서 평균 내주면 된다.
// average
for(int i = 0; i < samples; i++)
{
read_gyro_lsb(icm20948);
read_accel_lsb(icm20948);
gyro_bias[0] += icm20948->gyro_lsb_x;
gyro_bias[1] += icm20948->gyro_lsb_y;
gyro_bias[2] += icm20948->gyro_lsb_z;
accel_bias[0] += icm20948->accel_lsb_x;
accel_bias[1] += icm20948->accel_lsb_y;
accel_bias[2] += icm20948->accel_lsb_z;
}
gyro_bias[0] /= (int32_t) samples;
gyro_bias[1] /= (int32_t) samples;
gyro_bias[2] /= (int32_t) samples;
accel_bias[0] /= (int32_t) samples;
accel_bias[1] /= (int32_t) samples;
accel_bias[2] /= (int32_t) samples;
이 다음이 문젠데, 위에서 구한 오프셋을 그대로 offset 레지스터에 넣으면 안된다.
또 뭘 바꿔줘야 한다. 근데 왜 데이터 시트에서는 안알려줘?
자이로같은 경우는 offset 레지스터 reset value가 0이라서 그냥 바로 넣어주면 된다.
중요한 내용은 주석이 다 설명해준다.
// Construct the gyro biases for push to the hardware gyro bias registers,
// which are reset to zero upon device startup.
// Divide by 4 to get 32.9 LSB per deg/s to conform to expected bias input format.
// Biases are additive, so change sign on calculated average gyro biases
gyro_offset[0] = (-gyro_bias[0] / 4 >> 8) & 0xFF;
gyro_offset[1] = (-gyro_bias[0] / 4) & 0xFF;
gyro_offset[2] = (-gyro_bias[1] / 4 >> 8) & 0xFF;
gyro_offset[3] = (-gyro_bias[1] / 4) & 0xFF;
gyro_offset[4] = (-gyro_bias[2] / 4 >> 8) & 0xFF;
gyro_offset[5] = (-gyro_bias[2] / 4) & 0xFF;
가속도가 약간 까다로운데 참 짜증나는 친구다
가속도 offset register에는 이미 reset value에 값이 들어있어서 이 레지스터를 읽은 다음에
그 값에 계산한 offset 값을 빼줘야 한다.
// 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.
// read factory accel trim values
select_user_bank(userbank_1);
icm20948_read(B1_XA_OFFS_H, 2);
accel_bias_reg[0] = (int32_t)((int16_t)rx_buffer[0] << 8 | rx_buffer[1]);
icm20948_read(B1_YA_OFFS_H, 2);
accel_bias_reg[1] = (int32_t)((int16_t)rx_buffer[0] << 8 | rx_buffer[1]);
icm20948_read(B1_ZA_OFFS_H, 2);
accel_bias_reg[2] = (int32_t)((int16_t)rx_buffer[0] << 8 | rx_buffer[1]);
// accel offset register
accel_bias_reg[0] -= (accel_bias[0] / 8);
accel_bias_reg[1] -= (accel_bias[1] / 8);
accel_bias_reg[2] -= (accel_bias[2] / 8);
가속도 오프셋 레지스터의 모든 bit 0은 reserved이다.
참 까탈스러운게 계산한 오프셋 값이 홀수여서 bit 0이 1로 셋되면 offset값이 제대로 적용이 안되더라
mpu6050 라이브러리에서는 이게 temperature compensation bit라고 보존한다고 or연산을 하긴하는데
// https://github.com/kriswiner/MPU6050
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
icm20948에서는 뭐에 쓰는 앤지도 모르겠고 온도센서도 안쓰니까 걍 밀어버렸다. 작동은 잘 됨.
// ignore bit 0 (& 0xFE)
accel_offset[0] = (accel_bias_reg[0] >> 8) & 0xFF;
accel_offset[1] = (accel_bias_reg[0]) & 0xFE;
accel_offset[2] = (accel_bias_reg[1] >> 8) & 0xFF;
accel_offset[3] = (accel_bias_reg[1]) & 0xFE;
accel_offset[4] = (accel_bias_reg[2] >> 8) & 0xFF;
accel_offset[5] = (accel_bias_reg[2]) & 0xFE;
