シリーズ：実験シリーズ（フェーズ3）
対応ロードマップ：フェーズ3 / E3-05
この記事で扱う範囲：E3-04 で加速度3軸とジャイロ3軸の生値を周期的に読めるようになったので、今回は ボードを静止させた状態で何回か値を読み、「動いていないのに出ている値」をズレとして求めます。その後、毎回の読み取り値からそのズレ分を差し引く ところまで進める。
1. 目的
今回は、IMUの 簡易オフセット補正 を行います。
E3-04では、IMUから次の6つの生値を読み、UARTログに出しました。
- 加速度X/Y/Z
- ジャイロX/Y/Z
実際にログを見ると、ボードを静かに置いていても、値が完全に0にならないことがあります。
特にジャイロは、静止しているなら本来は0付近になってほしい値です。
しかし実際には、
GYRO X= 12 Y= -20 Z= 5のように、少しズレた値が出ることがあります。
このような 静止しているのに残っているズレ を、今回は簡単に補正します。
今回のゴールは、正確なセンサ校正ではありません。
まずは、
- 静止中に何回か読む
- 平均値を求める
- その平均値をオフセットとして保存する
- 以後の値からオフセットを引く
という流れを理解することです。
2. 前提・環境
2-1. 前提
この記事は、次の内容が終わっている前提で進めます。
- フェーズ0：ビルド / 書き込み / main到達 / UARTログ出力
- E2-02：1msごとの時刻を作る
- E2-03：周期処理
- E2-05：状態で整理（IDLE / RUN / ERROR）
- E3-00：I2C配線の“詰まりどころ”を先に潰す
- E3-01：0x68 に対して応答があるか確認する
- E3-02：WHO_AM_I を1バイト読める
- E3-03：加速度X軸を2バイト連続で読める
- E3-04：加速度3軸とジャイロ3軸の生値を周期的に読める
2-2. 使用するもの
- 評価ボード：RTK7EKA8M2S00001BE
- e² studio + FSP
- Tera Term
- IMUモジュール：QCIOT-ICM42688P（PMOD BOARD ICM-42688-P）
- USB-UART変換ケーブル（またはUSB-UART変換モジュール）
- ジャンパ線（UART接続用）
2-3. 今回の前提条件
接続条件は、E3-04 と同じ下記内容です。
- 通信方式：I2C
- 接続方法：QCIOT-ICM42688P の J1 を評価ボードの Pmod1（J26）へ接続
- 評価ボード側設定：SW4-1 = OFF、SW4-2 = ON
- AD0：Low（J4 にジャンパキャップあり）
- I2Cアドレス：0x68
2-4. 今回使うレジスタ
今回も、E3-04 と同じく 0x1F から 0x2A までの12バイトを読みます。
| データ | 上位バイト | 下位バイト |
|---|---|---|
| 加速度X | ACCEL_DATA_X1 = 0x1F | ACCEL_DATA_X0 = 0x20 |
| 加速度Y | ACCEL_DATA_Y1 = 0x21 | ACCEL_DATA_Y0 = 0x22 |
| 加速度Z | ACCEL_DATA_Z1 = 0x23 | ACCEL_DATA_Z0 = 0x24 |
| ジャイロX | GYRO_DATA_X1 = 0x25 | GYRO_DATA_X0 = 0x26 |
| ジャイロY | GYRO_DATA_Y1 = 0x27 | GYRO_DATA_Y0 = 0x28 |
| ジャイロZ | GYRO_DATA_Z1 = 0x29 | GYRO_DATA_Z0 = 0x2A |
今回の読み方は E3-04 と同じです。
違うのは、読み取った値をそのまま表示するのではなく、静止時の平均値を引いた値も表示することです。
3. 今回の変更点
3-1. 配線変更
今回は配線変更はありません。
E3-04 と同じ下記内容になります。
- QCIOT-ICM42688P の J1 を評価ボードの Pmod1（J26）へ接続
- 評価ボード側は SW4-1 = OFF、SW4-2 = ON
- J4 にジャンパキャップあり
3-2. 設定変更
FSP の I2C Master 設定は、E3-04 と同じ下記内容です。
- 通信方式：I2C Master
- アドレス幅：7bit
- 通信速度：100kHz
- スレーブアドレス：0x68
- Callback：
i2c_master_callback
3-3. コード変更
今回は、E3-04 のコードに次の処理を追加します。
- 起動直後にIMUを静止させた状態で、複数回データを読む
- 読んだ値を合計する
- 合計値を読み取り回数で割って、平均値を求める
- 平均値をオフセットとして保存する
- 以後のログでは、生値と補正後の値を表示する
今回の中心は、次の考え方です。
補正後の値 = 生値 - オフセット値たとえば、静止中のジャイロX軸の平均が 12 だった場合、
以後はジャイロX軸の生値から 12 を引きます。
gyro_x_corrected = gyro_x_raw - gyro_x_offsetこれにより、静止中のジャイロ値が0に近づきます。
4. 手順
4-1. E3-04 と同じ条件で接続する
まず、E3-04 と同じ条件で、QCIOT-ICM42688P と評価ボードを接続します。
- QCIOT-ICM42688P の J1 を評価ボードの Pmod1（J26）へ接続
- SW4-1 = OFF
- SW4-2 = ON
- J4 にジャンパキャップあり
4-2. ボードを静かに置く
今回の補正では、起動直後にIMUの値を何回か読みます。
そのため、実行する前に、評価ボードを机の上などに静かに置いてください。
このとき大事なのは、次の点です。
- 起動直後はボードを動かさない
- 手で持ったまま実行しない
- 机の上で安定させる
- 補正中に傾けたり揺らしたりしない
補正中に動かしてしまうと、動いている値まで平均に入ってしまいます。
その結果、正しいオフセットになりません。
4-3. 起動時にオフセットを取得する
今回は、起動後に次のようなログを出します。
IMU offset calibration start
Keep board still...
IMU offset calibration OKこのログが出るまでは、ボードを動かさないようにします。
4-4. 補正前と補正後の値を比べる
補正が終わったら、500msごとにログを出します。
今回は、生値と補正後の値が分かるように、ジャイロを中心に表示します。
RAW GYRO X= 12 Y= -20 Z= 5 | CORR GYRO X= 0 Y= 0 Z= 0
RAW GYRO X= 15 Y= -18 Z= 6 | CORR GYRO X= 3 Y= 2 Z= 1完全に0にはなりません。
ただし、静止中の値が以前より0に近づけばOKです。
5. コード
※下記は考え方を分かりやすくするためのシンプルな例です。
※ I2Cインスタンス名、UARTインスタンス名、コールバック名は、自分のプロジェクトに合わせて置き換えてください。
※ FSPの設定や使用しているI2Cモジュールにより、R_IIC_MASTER_... の部分は環境に合わせて調整してください。
#include "hal_data.h"
#include <stdio.h>
#include <string.h>
#include <stdbool.h>
#include <stdint.h>
#define IMU_I2C_ADDR (0x68U)
/* ICM-42688-P Register */
#define IMU_REG_ACCEL_DATA_X1 (0x1FU)
#define IMU_REG_PWR_MGMT0 (0x4EU)
#define IMU_REG_GYRO_CONFIG0 (0x4FU)
#define IMU_REG_ACCEL_CONFIG0 (0x50U)
/* 0x1F から 0x2A まで読むので 12バイト */
#define IMU_RAW_DATA_LENGTH (12U)
/* 設定値 */
#define IMU_PWR_MGMT0_ACCEL_GYRO_LN (0x0FU)
#define IMU_GYRO_CONFIG0_100HZ (0x08U)
#define IMU_ACCEL_CONFIG0_2G_100HZ (0x68U)
#define I2C_TIMEOUT_COUNT (100000U)
#define IMU_LOG_INTERVAL_MS (500U)
/* オフセット計算用 */
#define IMU_OFFSET_SAMPLE_COUNT (100U)
#define IMU_OFFSET_SAMPLE_WAIT_MS (10U)
typedef struct st_imu_raw_data
{
int16_t accel_x;
int16_t accel_y;
int16_t accel_z;
int16_t gyro_x;
int16_t gyro_y;
int16_t gyro_z;
} imu_raw_data_t;
typedef struct st_imu_offset
{
int16_t accel_x;
int16_t accel_y;
int16_t accel_z;
int16_t gyro_x;
int16_t gyro_y;
int16_t gyro_z;
} imu_offset_t;
typedef struct st_imu_corrected_data
{
int16_t accel_x;
int16_t accel_y;
int16_t accel_z;
int16_t gyro_x;
int16_t gyro_y;
int16_t gyro_z;
} imu_corrected_data_t;
static volatile bool g_i2c_done = false;
static volatile bool g_i2c_error = false;
static volatile uint32_t g_ms_count = 0U;
static void uart_print(const char * p_text);
static bool imu_write_register_1byte(uint8_t reg_addr, uint8_t value);
static bool imu_read_register_bytes(uint8_t reg_addr, uint8_t * p_buffer, uint32_t length);
static bool imu_init(void);
static bool imu_read_raw_data(imu_raw_data_t * p_data);
static bool imu_calibrate_offset(imu_offset_t * p_offset);
static void imu_apply_offset(const imu_raw_data_t * p_raw,
const imu_offset_t * p_offset,
imu_corrected_data_t * p_corrected);
static int16_t make_int16(uint8_t upper, uint8_t lower);
static void print_imu_data(const imu_raw_data_t * p_raw,
const imu_corrected_data_t * p_corrected);
void i2c_master_callback(i2c_master_callback_args_t * p_args)
{
if (NULL == p_args)
{
return;
}
switch (p_args->event)
{
case I2C_MASTER_EVENT_ABORTED:
{
g_i2c_error = true;
g_i2c_done = true;
break;
}
case I2C_MASTER_EVENT_RX_COMPLETE:
case I2C_MASTER_EVENT_TX_COMPLETE:
{
g_i2c_error = false;
g_i2c_done = true;
break;
}
default:
{
break;
}
}
}
/* E2-02 / E2-03 で作った 1msカウンタ用のコールバック例 */
void timer0_callback(timer_callback_args_t * p_args)
{
if (NULL == p_args)
{
return;
}
if (TIMER_EVENT_CYCLE_END == p_args->event)
{
g_ms_count++;
}
}
void hal_entry(void)
{
fsp_err_t err;
imu_raw_data_t imu_raw;
imu_corrected_data_t imu_corrected;
imu_offset_t imu_offset;
uint32_t last_log_ms = 0U;
err = R_SCI_B_UART_Open(&g_uart0_ctrl, &g_uart0_cfg);
if (FSP_SUCCESS != err)
{
while (1)
{
;
}
}
uart_print("IMU offset correction start\r\n");
err = R_IIC_MASTER_Open(&g_i2c_master0_ctrl, &g_i2c_master0_cfg);
if (FSP_SUCCESS != err)
{
uart_print("I2C open error\r\n");
while (1)
{
;
}
}
err = R_IIC_MASTER_SlaveAddressSet(&g_i2c_master0_ctrl,
IMU_I2C_ADDR,
I2C_MASTER_ADDR_MODE_7BIT);
if (FSP_SUCCESS != err)
{
uart_print("Slave address set error\r\n");
while (1)
{
;
}
}
if (!imu_init())
{
uart_print("IMU init error\r\n");
while (1)
{
;
}
}
uart_print("IMU init OK\r\n");
uart_print("IMU offset calibration start\r\n");
uart_print("Keep board still...\r\n");
if (!imu_calibrate_offset(&imu_offset))
{
uart_print("IMU offset calibration error\r\n");
while (1)
{
;
}
}
uart_print("IMU offset calibration OK\r\n");
while (1)
{
if ((g_ms_count - last_log_ms) >= IMU_LOG_INTERVAL_MS)
{
last_log_ms += IMU_LOG_INTERVAL_MS;
if (imu_read_raw_data(&imu_raw))
{
imu_apply_offset(&imu_raw, &imu_offset, &imu_corrected);
print_imu_data(&imu_raw, &imu_corrected);
}
else
{
uart_print("IMU raw read error\r\n");
}
}
}
}
static bool imu_init(void)
{
if (!imu_write_register_1byte(IMU_REG_PWR_MGMT0, IMU_PWR_MGMT0_ACCEL_GYRO_LN))
{
return false;
}
/* 加速度・ジャイロをOFFからONにした直後は少し待つ */
for (volatile uint32_t i = 0; i < 50000U; i++)
{
__asm volatile ("nop");
}
if (!imu_write_register_1byte(IMU_REG_GYRO_CONFIG0, IMU_GYRO_CONFIG0_100HZ))
{
return false;
}
if (!imu_write_register_1byte(IMU_REG_ACCEL_CONFIG0, IMU_ACCEL_CONFIG0_2G_100HZ))
{
return false;
}
return true;
}
static bool imu_read_raw_data(imu_raw_data_t * p_data)
{
uint8_t raw[IMU_RAW_DATA_LENGTH];
if (NULL == p_data)
{
return false;
}
if (!imu_read_register_bytes(IMU_REG_ACCEL_DATA_X1, raw, IMU_RAW_DATA_LENGTH))
{
return false;
}
p_data->accel_x = make_int16(raw[0], raw[1]);
p_data->accel_y = make_int16(raw[2], raw[3]);
p_data->accel_z = make_int16(raw[4], raw[5]);
p_data->gyro_x = make_int16(raw[6], raw[7]);
p_data->gyro_y = make_int16(raw[8], raw[9]);
p_data->gyro_z = make_int16(raw[10], raw[11]);
return true;
}
static bool imu_calibrate_offset(imu_offset_t * p_offset)
{
imu_raw_data_t data;
int32_t sum_accel_x = 0;
int32_t sum_accel_y = 0;
int32_t sum_accel_z = 0;
int32_t sum_gyro_x = 0;
int32_t sum_gyro_y = 0;
int32_t sum_gyro_z = 0;
if (NULL == p_offset)
{
return false;
}
for (uint32_t i = 0; i < IMU_OFFSET_SAMPLE_COUNT; i++)
{
if (!imu_read_raw_data(&data))
{
return false;
}
sum_accel_x += data.accel_x;
sum_accel_y += data.accel_y;
sum_accel_z += data.accel_z;
sum_gyro_x += data.gyro_x;
sum_gyro_y += data.gyro_y;
sum_gyro_z += data.gyro_z;
R_BSP_SoftwareDelay(IMU_OFFSET_SAMPLE_WAIT_MS, BSP_DELAY_UNITS_MILLISECONDS);
}
p_offset->accel_x = (int16_t)(sum_accel_x / (int32_t)IMU_OFFSET_SAMPLE_COUNT);
p_offset->accel_y = (int16_t)(sum_accel_y / (int32_t)IMU_OFFSET_SAMPLE_COUNT);
p_offset->accel_z = (int16_t)(sum_accel_z / (int32_t)IMU_OFFSET_SAMPLE_COUNT);
p_offset->gyro_x = (int16_t)(sum_gyro_x / (int32_t)IMU_OFFSET_SAMPLE_COUNT);
p_offset->gyro_y = (int16_t)(sum_gyro_y / (int32_t)IMU_OFFSET_SAMPLE_COUNT);
p_offset->gyro_z = (int16_t)(sum_gyro_z / (int32_t)IMU_OFFSET_SAMPLE_COUNT);
return true;
}
static void imu_apply_offset(const imu_raw_data_t * p_raw,
const imu_offset_t * p_offset,
imu_corrected_data_t * p_corrected)
{
if ((NULL == p_raw) || (NULL == p_offset) || (NULL == p_corrected))
{
return;
}
p_corrected->accel_x = (int16_t)(p_raw->accel_x - p_offset->accel_x);
p_corrected->accel_y = (int16_t)(p_raw->accel_y - p_offset->accel_y);
p_corrected->accel_z = (int16_t)(p_raw->accel_z - p_offset->accel_z);
p_corrected->gyro_x = (int16_t)(p_raw->gyro_x - p_offset->gyro_x);
p_corrected->gyro_y = (int16_t)(p_raw->gyro_y - p_offset->gyro_y);
p_corrected->gyro_z = (int16_t)(p_raw->gyro_z - p_offset->gyro_z);
}
static int16_t make_int16(uint8_t upper, uint8_t lower)
{
return (int16_t)(((uint16_t)upper << 8) | lower);
}
static bool imu_write_register_1byte(uint8_t reg_addr, uint8_t value)
{
fsp_err_t err;
uint8_t write_buf[2];
uint32_t timeout;
write_buf[0] = reg_addr;
write_buf[1] = value;
g_i2c_done = false;
g_i2c_error = false;
err = R_IIC_MASTER_Write(&g_i2c_master0_ctrl, write_buf, 2U, false);
if (FSP_SUCCESS != err)
{
return false;
}
timeout = I2C_TIMEOUT_COUNT;
while ((false == g_i2c_done) && (timeout > 0U))
{
timeout--;
}
if ((0U == timeout) || (true == g_i2c_error))
{
return false;
}
return true;
}
static bool imu_read_register_bytes(uint8_t reg_addr, uint8_t * p_buffer, uint32_t length)
{
fsp_err_t err;
uint32_t timeout;
if ((NULL == p_buffer) || (0U == length))
{
return false;
}
/* 1) 読みたい先頭レジスタ番号を書く */
g_i2c_done = false;
g_i2c_error = false;
err = R_IIC_MASTER_Write(&g_i2c_master0_ctrl, ®_addr, 1U, true);
if (FSP_SUCCESS != err)
{
return false;
}
timeout = I2C_TIMEOUT_COUNT;
while ((false == g_i2c_done) && (timeout > 0U))
{
timeout--;
}
if ((0U == timeout) || (true == g_i2c_error))
{
return false;
}
/* 2) 指定バイト数だけ連続で読む */
g_i2c_done = false;
g_i2c_error = false;
err = R_IIC_MASTER_Read(&g_i2c_master0_ctrl, p_buffer, length, false);
if (FSP_SUCCESS != err)
{
return false;
}
timeout = I2C_TIMEOUT_COUNT;
while ((false == g_i2c_done) && (timeout > 0U))
{
timeout--;
}
if ((0U == timeout) || (true == g_i2c_error))
{
return false;
}
return true;
}
static void print_imu_data(const imu_raw_data_t * p_raw,
const imu_corrected_data_t * p_corrected)
{
char msg[200];
if ((NULL == p_raw) || (NULL == p_corrected))
{
return;
}
snprintf(msg, sizeof(msg),
"RAW GYRO X=%6d Y=%6d Z=%6d | CORR GYRO X=%6d Y=%6d Z=%6d\r\n",
p_raw->gyro_x,
p_raw->gyro_y,
p_raw->gyro_z,
p_corrected->gyro_x,
p_corrected->gyro_y,
p_corrected->gyro_z);
uart_print(msg);
}
static void uart_print(const char * p_text)
{
fsp_err_t err;
err = R_SCI_B_UART_Write(&g_uart0_ctrl, (uint8_t *) p_text, strlen(p_text));
if (FSP_SUCCESS != err)
{
while (1)
{
;
}
}
/* UART送信完了待ちの簡易版。
* 本格的にはUARTコールバックで送信完了を待つ形にする。
*/
for (volatile uint32_t i = 0; i < 1000000U; i++)
{
__asm volatile ("nop");
}
}6. コードの見方（今回大事なところだけ）
6-1. オフセットとは何か
オフセットとは、簡単に言うと ズレ です。
今回の例では、ボードを静かに置いているのに、ジャイロが次のように出ていたとします。
GYRO X=12静止しているなら、本当は0に近い値になってほしいです。
この 12 が、静止時に残っているズレです。
そこで、起動時に静止状態で何回か値を読み、その平均を求めます。
静止時の平均 = オフセット以後は、読み取った生値からオフセットを引きます。
補正後の値 = 生値 - オフセットたとえば、
生値 = 15
オフセット = 12なら、
補正後の値 = 15 - 12 = 3になります。
6-2. 複数回読んで平均する理由
1回だけ読んだ値をオフセットにすると、その瞬間のノイズの影響を受けやすくなります。
たとえば、静止中でも次のように少し値が揺れることがあります。
10, 12, 11, 13, 12このような場合、1回だけ読んだ 10 をオフセットにするより、
何回か読んだ平均を使った方が、落ち着いた値になります。
今回のコードでは、次の回数だけ読みます。
#define IMU_OFFSET_SAMPLE_COUNT (100U)つまり、100回読んで平均します。
6-3. 合計用の変数は int32_t にする
オフセット計算では、次のように合計用の変数を用意しています。
int32_t sum_gyro_x = 0;
int32_t sum_gyro_y = 0;
int32_t sum_gyro_z = 0;IMUの1回分の値は int16_t です。
しかし、100回分を足すと、値が大きくなる可能性があります。
そのため、合計用の変数は int16_t ではなく、少し余裕のある int32_t にしています。
sum_gyro_x += data.gyro_x;最後に、合計値をサンプル数で割ります。
p_offset->gyro_x = (int16_t)(sum_gyro_x / (int32_t)IMU_OFFSET_SAMPLE_COUNT);これで、静止時の平均値が求まります。
6-4. 補正後の値を作る
補正は、次の関数で行っています。
static void imu_apply_offset(const imu_raw_data_t * p_raw,
const imu_offset_t * p_offset,
imu_corrected_data_t * p_corrected)中身はシンプルです。
p_corrected->gyro_x = (int16_t)(p_raw->gyro_x - p_offset->gyro_x);
p_corrected->gyro_y = (int16_t)(p_raw->gyro_y - p_offset->gyro_y);
p_corrected->gyro_z = (int16_t)(p_raw->gyro_z - p_offset->gyro_z);これは、
補正後 = 生値 - オフセットをそのままC言語で書いているだけです。
6-5. 加速度の補正には注意する
今回のコードでは、加速度も同じようにオフセットを求めています。
p_offset->accel_x = ...
p_offset->accel_y = ...
p_offset->accel_z = ...ただし、加速度はジャイロより少し注意が必要です。
加速度センサには、ボードを静止させていても 重力 が入ります。
つまり、静止中でも加速度Z軸などが大きな値になることがあります。
これは異常ではありません。
たとえば、E3-04では次のようなログ例がありました。
ACC X= -120 Y= 340 Z= 16320この Z=16320 のような値は、置き方によっては重力の影響です。
そのため、加速度の平均値をそのまま全部引いてしまうと、重力まで消したような値になります。
今回の加速度補正は、あくまで
- 補正という考え方を見る
- 起動時の姿勢からどれくらい変わったかを見る
ための簡易的なものです。
次回以降、ピッチ角やロール角を出すときは、加速度の重力成分を使います。
そのため、姿勢推定に使う加速度値は、今回の補正値をそのまま使うのではなく、目的に合わせて扱う必要があります。
ここは少し難しいので、今回の段階では次の理解で十分です。
- ジャイロは、静止時の平均を引くと0に近づきやすい
- 加速度は、静止時にも重力が入る
- 加速度の補正は、ジャイロより注意が必要
7. 実行結果
7-1. 確認したいログ
実行すると、Tera Termに次のようなログが出ればOKです。
IMU offset correction start
IMU init OK
IMU offset calibration start
Keep board still...
IMU offset calibration OK
RAW GYRO X= 12 Y= -20 Z= 5 | CORR GYRO X= 0 Y= 0 Z= 0
RAW GYRO X= 15 Y= -18 Z= 6 | CORR GYRO X= 3 Y= 2 Z= 1
RAW GYRO X= 10 Y= -23 Z= 4 | CORR GYRO X= -2 Y= -3 Z= -1数値は、使用しているIMUや置き方によって変わります。
上の値と一致する必要はありません。
7-2. 補正前後で見るポイント
今回見たいのは、絶対に0になることではありません。
見たいのは、次の点です。
RAW GYROは静止中でも少しズレているCORR GYROはRAW GYROより0に近い- ボードを動かすと
CORR GYROも大きく変わる - ボードを静止させると
CORR GYROがまた0付近に戻る
7-3. 完全に0にならなくてもよい
補正後の値は、完全に0で止まるとは限りません。
たとえば、次のように少し揺れていても問題ありません。
CORR GYRO X= -2 Y= 3 Z= 1
CORR GYRO X= 1 Y= -1 Z= 2
CORR GYRO X= 0 Y= 2 Z= -2センサにはノイズがあります。
そのため、補正後も少し値が揺れるのは自然です。
今回は、静止時のズレが小さくなったと確認できればOKです。
8. ハマりポイント／原因と対策
8-1. IMU offset calibration error になる
原因
- E3-04 の生値読み取りが安定していない
- オフセット取得中にI2C読み取りが失敗している
- I2Cの送受信完了を待てていない
- タイムアウト時間が短すぎる
- IMU初期化が終わる前に読み取りを始めている
対策
- まず E3-04 に戻り、加速度/ジャイロの生値が安定して読めるか確認する
imu_read_raw_data()が単体で成功する状態に戻すimu_init()のあとに少し待つ処理が入っているか確認する- I2Cコールバックで
TX_COMPLETE/RX_COMPLETEを受けているか確認する - 最初は
IMU_OFFSET_SAMPLE_COUNTを10Uなどに減らして確認する
8-2. 補正してもジャイロが0付近にならない
原因
- オフセット取得中にボードを動かしている
- 手で持ったまま実行している
- 机が揺れている
- サンプル数が少なすぎる
- ジャイロの設定や読み取り値が安定していない
対策
- 起動前にボードを机の上に置く
IMU offset calibration OKが出るまで触らない- サンプル数を
100Uなど、ある程度多めにする - まずは静止状態でログを見て、補正後の値が0付近に近づくか確認する
- それでも大きくずれる場合は、E3-04 の生値ログに戻って値の動きを見る
8-3. 補正後の値が逆におかしくなる
原因
- オフセット計算中に動かしてしまった
- 合計値や平均値の計算が間違っている
int16_tのまま合計してオーバーフローしている- 生値とオフセットを引く軸を間違えている
gyro_xからgyro_yのオフセットを引くなど、対応がずれている
対策
- 合計用の変数を
int32_tにする gyro_xにはoffset.gyro_xを引くgyro_yにはoffset.gyro_yを引くgyro_zにはoffset.gyro_zを引く- まずジャイロだけで補正を確認し、加速度は後で見る
8-4. 起動するたびに補正値が少し違う
原因
- センサ値にはノイズがある
- 起動時の置き方が少し違う
- 周囲の振動や手の揺れが入っている
- サンプル数が少ない
対策
- 完全一致しなくても問題ないと考える
- ボードを同じ向きで机に置く
- 補正中は触らない
- 必要ならサンプル数を増やす
- ただし、サンプル数を増やすと起動時の待ち時間も長くなる
8-5. 加速度の補正後が0付近になってしまう
原因
- 静止時の加速度平均をそのまま引いている
- 重力成分までオフセットとして扱っている
対策
- 今回の加速度補正は「起動時の姿勢からの変化を見るもの」と考える
- 姿勢推定では、加速度の重力成分を使うことを覚えておく
- 次回のピッチ/ロール推定では、加速度の扱いを改めて整理する
- まずはジャイロ補正を中心に理解する
8-6. 補正中なのか通常ログなのか分かりにくい
原因
- 補正開始/終了のログを出していない
- 起動直後にすぐ通常ログが出ている
- Tera Termのログが流れて見落としている
対策
- 補正開始時に
IMU offset calibration startを出す - 補正中に
Keep board still...を出す - 補正完了時に
IMU offset calibration OKを出す - 通常ログは補正完了後に出す
9. 今回わかったこと
今回の実験で大事なのは、
センサの値は、そのまま使う前に「ズレ」を考える必要がある ということです。
E3-04では、加速度3軸とジャイロ3軸の生値を周期的に読みました。
今回は、その生値に対して、
補正後の値 = 生値 - 静止時の平均値という簡単な補正を行いました。
今回できるようになったことは、次の通りです。
- 静止中にIMU値を複数回読む
- 読んだ値の平均を求める
- 平均値をオフセットとして保存する
- 生値からオフセットを引く
- 補正前と補正後の値をログで比較する
- ジャイロ値が静止中に0付近へ近づくことを確認する
今回の補正は、あくまで簡易版です。
温度変化や長時間のドリフトまで補正するものではありません。
ただし、姿勢推定や安全停止に進む前に、
「センサ値にはズレがある」
「そのズレを測って引くことができる」
という考え方を体験しておくことは重要です。
10. 次回やること
次回は、E3-06 ピッチ/ロール推定（最小） に進みます。
11. 関連リンク
- E3-00：I2C配線の“詰まりどころ”を先に潰す
- E3-01：I2C接続確認：0x68 に対して応答があるか確認
- E3-02：1レジスタ読み：WHO_AM_I を読む
- E3-03：連続読み：加速度X軸を2バイト読む
- E3-04：IMU生値取得：加速度とジャイロの生値を周期的に読んでログに出す
- E3-06：ピッチ/ロール推定（最小）
- 基礎シリーズ：GNDとは？なぜ基準点が必要なのか？
- 基礎シリーズ：UARTログ入門
- 基礎シリーズ：タイマで周期処理
- 基礎シリーズ：C言語の型と数値（予定）
- 基礎シリーズ：センサの補正とは（予定）