STM HAL L3GD20H
L3GD20H
秋月で入手可能な3軸ジャイロセンサーで遊んでみる。
https://www.st.com/ja/mems-and-sensors/l3gd20h.html#documentation
このIC、FIFO「First In First Out」とかできるが、ベタに読みだしていくことにする。
【ブレッドボード回路作製】
このIC自体は、I2CかSPIの両方で通信する事ができるが、今回はI2Cでの接続をする。
(I2CのOLEDを使っているが、ライブラリをもってきただけで、HALにおけるI2Cの理解している訳ではないので練習に）
I2CのSDA、SCLは、STM32F103のB7、B6に繋がるように接続（上ではOLEDとカスケードする形で接続）
SAOの４番ピンはI2Cの時のアドレスで最も下のビットがHIGHの時に、0x6BでLOWの時に、0x6Aに設定される。ここでは、LOWにした。５番ピンのCSはHIGHに接続する事で通信がI2Cになる。あとは１番ピンをＶｃｃに接続して、８番ピンをＧＮＤに接続する。
【コードと動作】
I2Cしか使わないので、ここでセーブして自動コードを生成する。
次に、表示器としてOLEDを使いたいので、必要なヘッダーファイルと、.cファイルをコピーしてくる。コピーはここを参照してコピー。または既にあるプロジェクトからコピーしてペーストする。
（USBでシリアルに出す場合は、ここを参照）
ヘッダーをインクルードするようにコードを記載する。
/* Private includes ----------------------------------------------------------*/
/* USER CODE BEGIN Includes */
#include "fonts.h"
#include "ssd1306.h"
/* USER CODE END Includes */
次に、I2Cのアドレス
/* Private define ------------------------------------------------------------*/
/* USER CODE BEGIN PD */
#define L3GD_address 0x6A<<1
/* USER CODE END PD */
を定義してやる。
このジャイロ４番ピンをLOWにしたのでアドレスは0x6A。
でここで、HALのI2Cのアドレスのお話。
I2Cのアドレスは、７ビットと10ビットのものがあるが、大抵は7ビット。
<<1
と後ろについていて左にビットシフトさせる。
で、このHALのI2C系の読み書きする命令が、あまり出来が良くない。
１ビット左にシフトしてからHALの関数に渡してあげないといけない。
まあこればかりはこういうものという事で。
（ネットとかからダウンロードしてくるサブルーチンなどは対処していて、ユーザがシフトさせる必要はないように作っているものもある）
次に、
/* USER CODE BEGIN PV */
uint8_t OLED_Buffer[64];
uint8_t I2C_TX_buffer[2];
uint8_t I2C_RX_buffer;
いくつかの変数を定義しておく。OLEDに文字を表示するときのバッファー。
I2Cの送信用のバッファー。ここでは２つの配列を定義。
最後に受け取るためのバッファーを用意する。受信は１バイトづつなので単なる変数でOK
さて、まず、はじめてのI2Cなので、簡単な所から。
whileループの外の直上の部分に以下のコードを記載する。
/* USER CODE BEGIN 2 */
SSD1306_Init(); // initializing SSD1306
I2C_TX_buffer[0]=0x0F;
HAL_I2C_Master_Transmit(&hi2c1, L3GD_address, &I2C_TX_buffer[0], 1, 1000);
HAL_I2C_Master_Receive(&hi2c1, L3GD_address, &I2C_RX_buffer, 1, 1000);
sprintf((char*)OLED_Buffer, "%x",I2C_RX_buffer);
SSD1306_GotoXY (0,0 );
SSD1306_Puts ((char*)OLED_Buffer, &Font_11x18, 1);
SSD1306_UpdateScreen();
使っている命令を解説しておくと、
HAL_I2C_Master_Transmit（HAL_I2C_Master_Receive）
第一引数：I2Cのハンドラ
第二引数：I2Cのアドレス
第三引数：送る（受け取る）バッファーのポインタ
第四引数：送る数（受け取る数）
最後の引数：エラーまでの待ち時間
最後の引数：エラーまでの待ち時間
ここまで記載したらコンパイルして、エラーがなければ書き込みしてやる。
無事に、書き込み、読み込みができたので次に。
このICの動作の流れ
初期設定：角速度を読み取りの各種の設定
値の読み取り
これらをレジスタに必要な値を書き込んだり、レジスタの値を読み角速度を手にいれる。
まず、設定。
最初の２ビットDR1とDR0は、データレートを決める（どれぐらいの角速度の変化をよみますか？）、で続くBW１とBW０はband widthのBWで、早い動きに追従すると、今度は非常に遅いものには対応しないので、その値を決める。
PDは、PowerDownで完全に停止するときに、ここのゼロにする。スリープする時は、ここを１にして以下３つをゼロにしろと書いてある。
Zen, Yen, Xenは、それぞれの軸をenableにするか？当然すべての軸を有効にするので通常は１にする。
問題は、DRとBW・・・。用途によって最適化するんだろうが、よくわからないので、とりあえず、00 00とここではする。Low_ODRの設定によっては、12.5Hzでカットオフなしか、100Hzでカットオフが12.5Hzになるが、Low_ODRはLow speed ODR. Default value: 0とあって遅い時は指定しないとゼロ。
上記から、コントロールバイトは、0B00001111となる。16進数で0x0F。
これをCTRL１のレジスタのアドレス0x20に書き込めばよいことになる。
値の読み取りは、レジスタのアドレスが0x28から0x2DまでにXの下位ビット、Xの上位ビッド、Yの下位、Y上位、Z下位、Z上位の順で読みより、Xの値を得るには、上位と下位をくっつけて値を出してあげる。
ここまでのコードを記載する。
まず、変数として、0x28-0x2Dまでの値を格納する変数XH,XL・・・を用意。
求めたい角速度を格納する変数として、X_rot・・・を用意。ここでint16_tにしておく事。
角度は正方向、負方向を検出するので、プラスとマイナスの値が入るので、unsignの定義ではなく単なる整数としておく。
/* USER CODE BEGIN PV */
uint8_t OLED_Buffer[64];
uint8_t I2C_TX_buffer[2];
uint8_t I2C_RX_buffer;
uint16_t XH,XL,YH,YL,ZH,ZL;
int16_t X_rot,Y_rot,Z_rot;//<--signed integer type
次に初期設定の部分
whileループの直前 /* USER CODE BEGIN 2 */の中の部分に以下を追記
/*L3GD20H setting*/
I2C_TX_buffer[0]=0x20; //CTRL1
I2C_TX_buffer[1]=0x0F;
HAL_I2C_Master_Transmit(&hi2c1, L3GD_address, I2C_TX_buffer, 2, 1000);
送り先のレジスタ、送り込む値を連続して投げつけるため、それぞれを配列に代入。
HAL_I2C_Master_Transmitで送り込むが、配列のポインターで開始位置を送って、その後２ビット分を指定、最後の1000はエラー待ち時間。
次にwhileの中で連続的に値を読み込む。少し長いが
/* USER CODE BEGIN 3 */
I2C_TX_buffer[0]=0x28;
HAL_I2C_Master_Transmit(&hi2c1, L3GD_address, &I2C_TX_buffer[0], 1, 1000);
HAL_I2C_Master_Receive (&hi2c1, L3GD_address, &I2C_RX_buffer, 1, 1000);
XL=I2C_RX_buffer;
I2C_TX_buffer[0]=0x29;
HAL_I2C_Master_Transmit(&hi2c1, L3GD_address, &I2C_TX_buffer[0], 1, 1000);
HAL_I2C_Master_Receive(&hi2c1, L3GD_address, &I2C_RX_buffer, 1, 1000);
XH=I2C_RX_buffer;
I2C_TX_buffer[0]=0x2A;
HAL_I2C_Master_Transmit(&hi2c1, L3GD_address, &I2C_TX_buffer[0], 1, 1000);
HAL_I2C_Master_Receive(&hi2c1, L3GD_address, &I2C_RX_buffer, 1, 1000);
YL=I2C_RX_buffer;
I2C_TX_buffer[0]=0x2B;
HAL_I2C_Master_Transmit(&hi2c1, L3GD_address, &I2C_TX_buffer[0], 1, 1000);
HAL_I2C_Master_Receive(&hi2c1, L3GD_address, &I2C_RX_buffer, 1, 1000);
YH=I2C_RX_buffer;
I2C_TX_buffer[0]=0x2C;
HAL_I2C_Master_Transmit(&hi2c1, L3GD_address, &I2C_TX_buffer[0], 1, 1000);
HAL_I2C_Master_Receive(&hi2c1, L3GD_address, &I2C_RX_buffer, 1, 1000);
ZL=I2C_RX_buffer;
I2C_TX_buffer[0]=0x2D;
HAL_I2C_Master_Transmit(&hi2c1, L3GD_address, &I2C_TX_buffer[0], 1, 1000);
HAL_I2C_Master_Receive(&hi2c1,L3GD_address, &I2C_RX_buffer, 1, 1000);
ZH=I2C_RX_buffer;
X_rot= (XH<<8)|XL;
Y_rot= (YH<<8)|YL;
Z_rot= (ZH<<8)|ZL;
sprintf((char*)OLED_Buffer, "x=%+6d",X_rot);
//while(CDC_Transmit_FS(OLED_Buffer, strlen((char*)OLED_Buffer)) == USBD_OK) {}
SSD1306_GotoXY (0,0);
SSD1306_Puts ((char*)OLED_Buffer, &Font_11x18, 1);
sprintf((char*)OLED_Buffer, "y=%+6d",Y_rot);
//while(CDC_Transmit_FS(OLED_Buffer, strlen((char*)OLED_Buffer)) == USBD_OK) {}
SSD1306_GotoXY (0, 20);
SSD1306_Puts ((char*)OLED_Buffer, &Font_11x18, 1);
sprintf((char*)OLED_Buffer, "z=6d",Z_rot);
// while(CDC_Transmit_FS(OLED_Buffer, strlen((char*)OLED_Buffer)) == USBD_OK) {}
SSD1306_GotoXY (0,40 );
SSD1306_Puts ((char*)OLED_Buffer, &Font_11x18, 1);
SSD1306_UpdateScreen();
既にIDを読み込んで表示させているので、ここでは解説は省略、ビット操作の所だけにする。
X_rot= (XH<<8)|XL;
の部分は、XHを８左にシフトして、XLと論理和（OR)をとって、8ビットの情報二つをつなげてあげている。
ビルドして書き込んでみる。
エラーがなければ、ｘ、ｙ、ｚに値が表示されていて、動かすと動かしている間だけ値が変化する事がわかる。
ちなみに角速度っていいいながちゃんとした角度に直すにはさらに係数をかけてあげる必要があるが、多くのアプリケーションで重要ではない。
【関数化】
I2Cの通信を勉強するが目的だったので、ベタに書きすぎているので関数化する。
プライベートコードの部分に、関数をいくつか定義
* Private user code ---------------------------------------------------------*/
/* USER CODE BEGIN 0 */
void L3GD20H_Write_Register(uint8_t Reg, uint8_t Write_Data)
{
uint8_t Write_buffer[2];
Write_buffer[0] = Reg;
Write_buffer[1] = Write_Data;
HAL_I2C_Master_Transmit(&hi2c1, L3GD_address, Write_buffer, 2, 500);
}
uint8_t L3GD20H_Read_Register(uint8_t Reg)
{
uint8_t txcmd = Reg;
uint8_t Read_Data;
HAL_I2C_Master_Transmit(&hi2c1,L3GD_address,&txcmd,1,500);
HAL_I2C_Master_Receive(&hi2c1, (L3GD_address)|0x1, &Read_Data, 1, 500);
return Read_Data;
}
void L3GD20H_Init()
{
L3GD20H_Write_Register(0x20,0x0f);
}
/* USER CODE END 0 */
各種のイニシャライズしている所に、作製した L3GD20H_Init();を置いてあげて、IDのチェックなんかをした部分はもう不要なので削除してしまう。
/* Initialize all configured peripherals */
MX_GPIO_Init();
MX_I2C1_Init();
MX_USB_DEVICE_Init();
/* USER CODE BEGIN 2 */
SSD1306_Init(); // initializing SSD1306
SSD1306_Clear();
//L3GD20H_Init();// initializing L3GD20H GYRO
L3GD20H_Init();
whileの中も作った関数で書き換えてあげればすっきり。
もっと関数化を進めて１行で済むようにしてあげるのが現実的。ここではここまでにする。
while (1)
{
/* USER CODE END WHILE */
/* USER CODE BEGIN 3 */
XL = L3GD20H_Read_Register(0x28);
XH = L3GD20H_Read_Register(0x29);
X_rot = XH <<8 | XL;
YL = L3GD20H_Read_Register(0x2A);
YH = L3GD20H_Read_Register(0x2B);
Y_rot = YH <<8 | YL;
ZL = L3GD20H_Read_Register(0x2C);
ZH = L3GD20H_Read_Register(0x2D);
Z_rot = ZH <<8 | ZL;
sprintf((char*)OLED_Buffer, "x=%+6d",X_rot);
SSD1306_GotoXY (0,0);
SSD1306_Puts ((char*)OLED_Buffer, &Font_11x18, 1);
sprintf((char*)OLED_Buffer, "y=%+6d",Y_rot);
SSD1306_GotoXY (0, 20);
SSD1306_Puts ((char*)OLED_Buffer, &Font_11x18, 1);
sprintf((char*)OLED_Buffer, "z=%+6d",Z_rot);
SSD1306_GotoXY (0,40 );
SSD1306_Puts ((char*)OLED_Buffer, &Font_11x18, 1);
SSD1306_UpdateScreen();
}
【可視化】
せっかくOLEDがあるので、数字をテキストで可視化するのではなく見た目で
わかりやすくしてみる。
さっきのテキストを吐き出していた部分をコメントアウトする。/* */で囲んでしまうか消してしまう。
/*
sprintf((char*)OLED_Buffer, "x=%+6d",X_rot);
SSD1306_GotoXY (0,0);
SSD1306_Puts ((char*)OLED_Buffer, &Font_11x18, 1);
sprintf((char*)OLED_Buffer, "y=%+6d",Y_rot);
SSD1306_GotoXY (0, 20);
SSD1306_Puts ((char*)OLED_Buffer, &Font_11x18, 1);
sprintf((char*)OLED_Buffer, "z=%+6d",Z_rot);
SSD1306_GotoXY (0,40 );
SSD1306_Puts ((char*)OLED_Buffer, &Font_11x18, 1);
*/
vex=3*X_rot/3277; //32768
vey=3*Y_rot/3277;
SSD1306_DrawFilledCircle(cx, cy, 4, 0) ;
if (cx>4 && cx<124){
cx=cx-vex;
}else
{
cx=cx;
}
if (cy>4 && cy<60){
cy=cy+vey;
}else
{
cy=cy;
}
SSD1306_DrawFilledCircle(cx, cy, 4, 1) ;
SSD1306_UpdateScreen();
やっている事は、出てくる値を画面中央に配置した●でＸ，Ｙ方向に傾けた時に角速度に応じて中央から●の位置が変わるってもの。傾けた状態で停止すると角速度がなくなるのでその場で停止。もとに戻すさっきとは逆の係数角速度と●が元の位置に戻ってくる。
姿勢角度を測定している訳ではないので、傾けるときにゆっくり、戻す時に速くとかすると中央に戻らない。
値は、最大で32768をとるけど、すごく高速に振り回した時になる値なので一桁小さい数字で割り算してあげて規格化。OLEDでそれなりの範囲動いてほしいので適当に３倍しているだけ。
センタの取り付けの方向によってはプラスマイナスの符号は調整してほしい。
画面の外まで行ってしまった時の処理が適当で対応できていない・・・力尽きた・・・
コメント
コメントを投稿