マイクロマウス研修(のり) マウス自作研修

マイクロマウス研修(のり)[39]STM32F446 STEP7-2 SPI(IMU)

マイクロマウス研修(のり)

記事一覧 のり マイクロマウス研修

前回TDK InvenSense社ICM-20602と通信でき、回路の正常動作が確認ができました。次はセンサデータの取得です。

参考サイトです。

マイクロマウス研修(kora編)[24] STM32マイコンでSPI通信
今回は、新型マウスのSPI通信を設定します。今回製作したマイクロマウスは、STM32F732マイコンに搭載されている3つのSPIのうち2つを使用します。左右のエンコーダがSPI1に、ジャイロセンサがSPI2に接続してあります。
STM32 + HALのSPIでMPU-6500と通信する  | Sora's Activity Record
STM32マイコンのペリフェラル関連記事を一覧にまとめました。 こんにちは。 STM32F405RGT(STMicroelectronics)のHALドライバを使用して、MPU65...
Bitbucket

ICM-20602データシート

温度

前回出てきたConfigurationの資料を再掲します。

画像引用:ICM-20602データシート

ここには、ジャイロのローパスフィルタ(3-dB BW)周波数やノイズ帯域幅(Noise BW)、センサ値取得周期(Rate)、温度のローパスフィルタ(3-dB BW)周波数が設定できます。マイクロマウスだとジャイロセンサは1kHzの制御周期で使うので、デフォルトでは250Hzなので遅いです。こちらも後ほど適切に設定する必要がありますが、初期値の0x80のままにしておきます。

温度Configurationの資料です。データシートに書いてあるアドレスは0x41と0x42。


画像引用:ICM-20602データシート

計算式が書いてあります。

TEMP_degC = (TEMP_OUT/Temp_Sensivity) + RoomTemp_Offset = (TEMP_OUT/326.8) + 25[℃]

というのをプログラムに組み込めば良さそうです。

ジャイロ

設定を見てみます。アドレスは0x1B、初期値は0x00。


画像引用:ICM-20602データシート

デフォルトのままだと、計測範囲は±250[dps]のようです。マイクロマウスだと±2000[dps]が必要なので、のちほど変更すると思いますが一旦このままで行きます。

次、計測データの資料を見ます。


画像引用:ICM-20602データシート

  • ジャイロX軸:Hバイト側0x43、Lバイト側0x44
  • ジャイロY軸:Hバイト側0x45、Lバイト側0x46
  • ジャイロZ軸:Hバイト側0x47、Lバイト側0x48

計算式は、

GYRO_OUT= Gyro_Sensivity * angular_rate = 131 * angular_rate

というのをプログラムに組み込めば良さそうです。


画像引用:ICM-20602データシート

Gyro_Sensivityは、計測範囲±250[dps]だと131と書かれています。計測範囲±2000[dps]にする場合は、16.4に書き換えることになりそうです。

加速度

設定を見てみます。2つありますね。アドレスは0x1Cとx1D。


画像引用:ICM-20602データシート

1つ目は計測範囲の設定のようです。デフォルトの±2Gのままで行きます。2つ目は内部フィルタをいろいろ設定できるようです。こちらものちのち設定すると思いますが、そのまま初期値でいきます。

次、計測データの資料を見ます。XYZ軸それぞれで、Hバイト側とLバイト側で別れています。


画像引用:ICM-20602データシート

  • 加速度X軸:Hバイト側0x3B、Lバイト側0x3C
  • 加速度Y軸:Hバイト側0x3D、Lバイト側0x3E
  • 加速度Z軸:Hバイト側0x3F、Lバイト側0x40

計算式が書かれていませんが、おそらくジャイロと同じようにSencivityをかけてあげれば良いのだと思います。それに当たる資料を見てみます。

Sensivityは、計測範囲±2[G]では16384と書かれています。

プログラム

前回のプログラムだと、CSでHighとLowをいちいち切り替える記述や1Byteデータの送受信がありました。そこは使いませるよう関数化してしまいます。

/**
  * @brief  Write 1byte to ICM-20602
  * @param  address
  * @param  data
  * @retval None
  */
static void IMU_Write1byte(uint8_t addr, uint8_t data)
{
	uint8_t address = addr & 0x7f;

	HAL_GPIO_WritePin(SPI2_SSEL_GPIO_Port, SPI2_SSEL_Pin, GPIO_PIN_RESET);
	HAL_SPI_Transmit(&hspi2, &address, 1, 1);
	HAL_SPI_Transmit(&hspi2, &data, 1, 1);
	HAL_GPIO_WritePin(SPI2_SSEL_GPIO_Port, SPI2_SSEL_Pin, GPIO_PIN_SET);
}

/**
  * @brief  Read 1byte to ICM-20602
  * @param  address
  * @retval Receive data
  */
static uint8_t IMU_Read1byte(uint8_t addr)
{
	uint8_t address = addr | 0x80;
	uint8_t data;

	HAL_GPIO_WritePin(SPI2_SSEL_GPIO_Port, SPI2_SSEL_Pin, GPIO_PIN_RESET);
	HAL_SPI_Transmit(&hspi2, &address, 1, 1);
	HAL_SPI_Receive(&hspi2, &data, 1, 1);
	HAL_GPIO_WritePin(SPI2_SSEL_GPIO_Port, SPI2_SSEL_Pin, GPIO_PIN_SET);

	return data;
}

 

設定の部分はイニシャライズ関数にまとめてしまいます。なぜかダミーデータを一度送らないと、データ取得できず不安定になりました。参考サイトにも注意事項として記述があり、お作法があるようです。

/**
  * @brief  Initialize ICM-20602
  * @param  None
  * @retval None
  */
static void IMU_Initialize(void)
{
    // Dummy WHO AM I send
	uint8_t who_am_i = IMU_Read1byte(0x75);

    // POWER MANAGEMENT 1
	IMU_Write1byte(0x6B, 0b00000000);		// Wake up & Internal 20MHz oscillator
	HAL_Delay(50);

    // WHO AM I
	who_am_i = IMU_Read1byte(0x75);
	printf("who am i = %x\n\r", who_am_i);

    // CONFIGURATION
	IMU_Write1byte(0x1A, 0x80);
    HAL_Delay(50);

    // ACCELEROMETER CONFIGURATION
    IMU_Write1byte(0x1C, 0b00000000);		// Accel Full Scale Select ±2g
    HAL_Delay(20);

    // GYROSCOPE CONFIGURATION
    IMU_Write1byte(0x1B, 0b00000000);		// Gyro Full Scale Select ±250 dps
    HAL_Delay(50);
}

 

main関数で用意する変数とイニシャライズ関数の実行です。

  /* USER CODE BEGIN 2 */
  setbuf(stdout, NULL);
  setbuf(stdin, NULL );
  printf("Started!\n\r");

  int16_t accel_x_value;
  int16_t accel_y_value;
  int16_t accel_z_value;
  float accel_x;
  float accel_y;
  float accel_z;

  int16_t gyro_x_value;
  int16_t gyro_y_value;
  int16_t gyro_z_value;
  float gyro_x_ang_vel;
  float gyro_y_ang_vel;
  float gyro_z_ang_vel;

  int16_t temp_value;
  float temp;

  IMU_Initialize();

  /* USER CODE END 2 */

計測と表示部分です。

  /* Infinite loop */
  /* USER CODE BEGIN WHILE */
  while (1)
  {
	  // ACCELEROMETER MEASUREMENT ACCEL_XOUT_H & ACCEL_XOUT_L
	  accel_x_value = ( ((int16_t)IMU_Read1byte(0x3B)<<8) | ((int16_t)IMU_Read1byte(0x3C)&0x00ff) );
	  accel_x = (float)accel_x_value / 16384;
	  printf("ACCEL_XOUT = %6d[Raw] = %6.3lf[g]\n\r", accel_x_value, accel_x);

	  // ACCELEROMETER MEASUREMENT ACCEL_YOUT_H & ACCEL_YOUT_L
	  accel_y_value = ( ((int16_t)IMU_Read1byte(0x3D)<<8) | ((int16_t)IMU_Read1byte(0x3E)&0x00ff) );
	  accel_y = (float)accel_y_value / 16384;
	  printf("ACCEL_YOUT = %6d[Raw] = %6.3lf[g]\n\r", accel_y_value, accel_y);

	  // ACCELEROMETER MEASUREMENT ACCEL_ZOUT_H & ACCEL_ZOUT_L
	  accel_z_value = ( ((int16_t)IMU_Read1byte(0x3F)<<8) | ((int16_t)IMU_Read1byte(0x40)&0x00ff) );
	  accel_z = (float)accel_z_value / 16384;
	  printf("ACCEL_ZOUT = %6d[Raw] = %6.3lf[g]\n\r", accel_z_value, accel_z);

	  // GYROSCOPE MEASUREMENT GYRO_XOUT_H & GYRO_XOUT_L
	  gyro_x_value = ( ((int16_t)IMU_Read1byte(0x43)<<8) | ((int16_t)IMU_Read1byte(0x44)&0x00ff) );
	  gyro_x_ang_vel = (float)gyro_x_value / 131;
	  printf(" GYRO_XOUT = %5d[Raw] = %8.3lf[deg/s]\n\r", gyro_x_value, gyro_x_ang_vel);

	  // GYROSCOPE MEASUREMENT GYRO_YOUT_H & GYRO_YOUT_L
	  gyro_y_value = ( ((int16_t)IMU_Read1byte(0x45)<<8) | ((int16_t)IMU_Read1byte(0x46)&0x00ff) );
	  gyro_y_ang_vel = (float)gyro_y_value / 131;
	  printf(" GYRO_YOUT = %5d[Raw] = %8.3lf[deg/s]\n\r", gyro_y_value, gyro_y_ang_vel);

	  // GYROSCOPE MEASUREMENT GYRO_ZOUT_H & GYRO_ZOUT_L
	  gyro_z_value = ( ((int16_t)IMU_Read1byte(0x47)<<8) | ((int16_t)IMU_Read1byte(0x48)&0x00ff) );
	  gyro_z_ang_vel = (float)gyro_z_value / 131;
	  printf(" GYRO_ZOUT = %5d[Raw] = %8.3lf[deg/s]\n\r", gyro_z_value, gyro_z_ang_vel);

	  // TEMPERATURE MEASUREMENT GYRO_ZOUT_H & GYRO_ZOUT_L
	  temp_value = ( ((int16_t)IMU_Read1byte(0x41)<<8) | ((int16_t)IMU_Read1byte(0x42)&0x00ff) );
	  temp = (float)temp_value / 326.8 + 25.0;
	  printf("  TEMP_OUT = %5d[Raw] = %3.1lf[℃]\n\r", temp_value, temp);

	  HAL_Delay(10);

	  // 画面クリア&カーソル初期化
	  printf("\033[2J");
	  printf("\033[%d;%dH" ,0,0);

    /* USER CODE END WHILE */

Hバイト側とLバイト側の1バイトずつ取得したデータを2バイトに統合して用意した変数に格納します。
その生データを、それぞれ計算式(Sensivityの値とかをかける)で分かりやすい単位に変換します。

実行

出力してみます。静止時のセンサ生データと単位変換したデータです。

動画です。

マイクロマウス研修 鉄鼠動作確認 IMUセンサデータ

静止時のデータなので、Z軸加速は正常に1Gを表示しています。なぜか、Y軸加速度だけ-2Gに張り付いてしまいました。アドレスだけしか変えてないので、原因がわかりません。機体を変えても同様なので、故障ではなさそうです。

Who Am I ダミーデータの送信以外で、何らかのお作法があるのでしょうか?後ほどオシロスコープで調べることになりそうです。
次回は、定期割り込み処理を行います。

タイトルとURLをコピーしました