さて、なぜこれをやったかというと、ロガー的なものを作ってみたかったからなんですね。
今回は、9軸モーションセンサであるInvenSense製MPU-9250をいじってみようと思います。
MPU-9250はジャイロセンサ、加速度センサ、地磁気センサの3つのセンサが搭載されているセンサで、I2CまたはSPIでその値を読みだすことができます。
ほかにもDMP（Digital Motion Processor）という計算チップが入っており、これら3つのセンサの値をから姿勢を算出し、オイラー角や四元数などの値を吐き出すこともできるようです。
今回は、とりあえずとっかかりとしてI2C接続でセンサの吐き出す加速度と角速度、そしておまけで付いてくる温度を読み出してUSBメモリーに記録していきたいと思います。
ちなみに、なぜ地磁気センサを読み出さないかというと、MPU-9250の中で別チップになっているからです。MPU-9250の中には加速度センサやジャイロセンサとは独立して、完全に別チップ（旭化成製AK8963）として地磁気センサが入っています。そのため、加速度センサの値等とは完全に別枠で制御してやる必要があります。なので、これはまた今度の課題とします。
さて、細かなスペックはデータシートとレジスターマップを見ればだいたいわかります。
ですので、どんどんコードを書いていきたいと思います。
typedef struct {
DRV_HANDLE hDriver;
DRV_I2C_BUFFER_HANDLE hBufferDriver;
uint8_t WriteBuffer[2];
uint8_t SlaveAddress;
} MPU9250;
まずは、MPU-9250にアクセスする用のデータをストックする構造体を作っておきます。
void I2CEventHandler(DRV_I2C_BUFFER_EVENT event, DRV_I2C_BUFFER_HANDLE bufferHandle, uintptr_t context)
{
/*
switch (event)
{
case DRV_I2C_BUFFER_EVENT_COMPLETE:
break;
case DRV_I2C_BUFFER_EVENT_ERROR:
break;
default:
break;
}
*/
}
_Bool MPU9250_Init(MPU9250 *pobj, SYS_MODULE_INDEX index, uint8_t SlaveAddr)
{
pobj->SlaveAddress = SlaveAddr;
pobj->hDriver = DRV_I2C_Open(index, DRV_IO_INTENT_READWRITE);
pobj->hBufferDriver = NULL;
DRV_I2C_BufferEventHandlerSet(pobj->hDriver, I2CEventHandler, (uintptr_t)pobj);
return pobj->hDriver != (DRV_HANDLE)NULL;
}
次は初期化の関数です。特に変なところは無いと思います。
I2Cの送受信が終わったときなどにそのイベントを受け取る関数を一応用意していますが、今回は使わないので中身は空にしています。
DRV_I2C_BUFFER_EVENT MPU9250_GetTransferStatus(MPU9250 *pobj)
{
return DRV_I2C_TransferStatusGet(pobj->hDriver, pobj->hBufferDriver);
}
_Bool MPU9250_IsI2CCommunicating(MPU9250 *pobj)
{
if(pobj->hBufferDriver == NULL)
return false;
else {
DRV_I2C_BUFFER_EVENT event = MPU9250_GetTransferStatus(pobj);
return (event != DRV_I2C_BUFFER_EVENT_COMPLETE) && (event != DRV_I2C_BUFFER_EVENT_ERROR);
}
}
I2Cの状態を確認するラッパー関数です。
MPU9250_IsI2CCommunicating関数は、これがTRUEを返すと現在送受信中で、FALSEを返すと現在アイドル中であることを表します。
_Bool MPU9250_StartWritingResistor(MPU9250 *pobj, uint8_t address, uint8_t data)
{
if(MPU9250_IsI2CCommunicating(pobj))
return false;
pobj->WriteBuffer[0] = address;
pobj->WriteBuffer[1] = data;
pobj->hBufferDriver = DRV_I2C_Transmit(pobj->hDriver, pobj->SlaveAddress << 1, pobj->WriteBuffer, 2, NULL);
return pobj->hBufferDriver != NULL;
}
_Bool MPU9250_StartReadingResistor(MPU9250 *pobj, uint8_t address, uint8_t *pdata)
{
return MPU9250_StartBurstReadingResistor(pobj, address, pdata, 1);
}
_Bool MPU9250_StartBurstReadingResistor(MPU9250 *pobj, uint8_t address, uint8_t *pdata, int length)
{
if(MPU9250_IsI2CCommunicating(pobj))
return false;
pobj->WriteBuffer[0] = address;
pobj->hBufferDriver = DRV_I2C_TransmitThenReceive(pobj->hDriver, pobj->SlaveAddress << 1, pobj->WriteBuffer, 1, pdata, length, NULL);
return pobj->hBufferDriver != NULL;
}
これが実際に送受信をする関数です。
注意しなければならないことは、スレーブアドレスはそのまま入れられないということです。
I2Cは最初の8bit中上位7bitでスレーブアドレスを送り、下位1bitでリードモードかライトモードかを指定します。
Harmonyのドライバ関数では、そのリード/ライトのビット付加は自動でやってくれますが、アドレスの1bitシフトはやってくれません。ですので、スレーブアドレスは左に1bitシフトしたうえで渡す必要があります。
また、これらの関数は読み書きをしたいメモリアドレスをパラメーターとして受け取りますが、DRV_I2C_TransmitThenReceive関数などでは内部でこの送信内容のデータのポインタを保持しています。ですので、この関数から制御が戻ると値が不定となるスタック領域ではだめで、MPU9250構造体の中に確保したメモリにアドレスをコピーしてそれを参照するようにしています。
uint16_t MPU9250_ParseUInt16(uint8_t *pData)
{
return (uint16_t)pData[0] << 8 | pData[1];
}
int16_t MPU9250_ParseInt16(uint8_t *pData)
{
return (int16_t)MPU9250_ParseUInt16(pData);
}
void MPU9250_ParseVector(uint8_t *pData, VECTOR_3D *pOut)
{
pOut->X = MPU9250_ParseInt16(pData);
pOut->Y = MPU9250_ParseInt16(pData + 2);
pOut->Z = MPU9250_ParseInt16(pData + 4);
}
あとはユーティリティ的なものですが、MPU-9250は16bit値は上位8bitのほうが若番のアドレスが振られています。多くの処理系とは異なる（とか言うと怒られそうですが）ので、手動でシフト演算するように作っています。
さて、このライブラリを使う側(app.c)は、だいたいこんな実装になります。
case APP_STATE_TMR_I2C_INIT:
//Initialize I2C and Send 'Who am I' command
if(MPU9250_Init(&appData.mpu9250, DRV_I2C_INDEX_0, MPU9250_I2C_ADDRESS_0)) {
MPU9250_StartReadingResistor(&appData.mpu9250, MPU9250_RA_WHO_AM_I, RXBuffer);
appData.state = APP_STATE_MPU9250_CHECK_WHO_AM_I;
} else
appData.state = APP_STATE_ERROR;
break;
case APP_STATE_MPU9250_CHECK_WHO_AM_I:
switch(MPU9250_GetTransferStatus(&appData.mpu9250)) {
case DRV_I2C_BUFFER_EVENT_COMPLETE:
if(RXBuffer[0] == MPU9250_RESPONSE_WHO_AM_I)
appData.state = APP_STATE_READ_RAW_DATA;
else
appData.state = APP_STATE_ERROR;
break;
case DRV_I2C_BUFFER_EVENT_ERROR:
appData.state = APP_STATE_ERROR;
break;
default:
break;
}
break;
case APP_STATE_READ_RAW_DATA:
if(MPU9250_StartBurstReadingResistor(&appData.mpu9250, MPU9250_RA_ACCEL_XOUT_H, RXBuffer, 14)) {
i2cWaitCounter = 0;
appData.state = APP_STATE_CHECK_RAW_DATA;
} else {
appData.state = APP_STATE_ERROR;
}
break;
case APP_STATE_CHECK_RAW_DATA:
{
DRV_I2C_BUFFER_EVENT event = MPU9250_GetTransferStatus(&appData.mpu9250);
switch(event) {
case DRV_I2C_BUFFER_EVENT_COMPLETE:
MPU9250_ParseVector(&RXBuffer[0], &appData.accel);
appData.temperature = MPU9250_ParseInt16(&RXBuffer[6]);
MPU9250_ParseVector(&RXBuffer[8], &appData.gyro);
appData.state = APP_STATE_READ_RAW_DATA;
break;
case DRV_I2C_BUFFER_EVENT_ERROR:
appData.state = APP_STATE_ERROR;
break;
default: //I2C communication in operating
if(i2cWaitCounter++ > 1000) {
appData.state = APP_STATE_ERROR;
}
break;
}
break;
}
まずはWhoAmIコマンドを送り、MPU-9250がちゃんと機能しているかを確認します。
その次は、加速度センサのx軸の上位8bitから14bytes連続で読みだします。ここに加速度センサの各軸の値、温度、ジャイロセンサの各軸の値が含まれます。
I2Cの読み込みが終了したら、値を分離して保存してあげます。
デフォルトで加速度は±2gFSですので、測定値の生の値を2^(16-2)で割ってから重力加速度の9.8を掛けてあげればm/s^2になります。
測定データの加速度の値です。ロギング中に基板をグリグリと回してあげたのでこのような測定結果が得られています。静置時にz軸方向におよそ9.8m/s^2が出ているので、値としてもおかしくないと思います。
…とまあここまですんなりと書きましたが、ハマりポイントはありました。
実際はこのI2C通信以外にもUSBメモリーの書き込み処理や、時間を計測するためのタイマー処理などをやっているわけで、コードは結構複雑になっています。
そんな中、データを取っていると0～1秒程度でハングアップしてしまう現象にかなり悩まされました。
I2Cがハングアップしているのか、USBがハングアップしているのか、LEDデバッグという超縛りプレイ環境のせいで切り分けに苦労しましたが 、結局はI2Cがハングアップしていました。
不具合の内容としては、上記プログラムの中でMPU9250_GetTransferStatus関数を呼び出し、I2Cの通信が完了するのを待つというところがありますが、いつまでたってもI2Cの通信が終わらないというものでした。
ですが、I2Cの不具合となれば、USBメモリーの読み書き機能が使えますので、実際にMPU9250_GetTransferStatus関数（DRV_I2C_TransferStatusGet関数）が返すDRV_I2C_BUFFER_EVENTの値と、DRV_I2C_BytesTransferred関数が返す送受信済みバイト数を記録してみました。
青線がDRV_I2C_BUFFER_EVENTの値で橙線が送受信済みバイト数です。
レジスタアドレス送信+14bytesリードで計15bytesの読み書きが行われています。
statusとしては正常に送信要求→リスタート→ACK送信の流れで読み書きができていますが、最後は5bytes分読み込んだところでブツンと受信が息絶えてしまっています。
そもそもI2Cはマスター側がクロックを発生させて、そのタイミングでスレーブがデータを送信しますので、スレーブがそれに反応できなかったとしてもプルアップ抵抗のために0xFFのデータが受かるだけです。そのため、受信中にハングアップするのは、明らかにスレーブではなくマスターのせいなわけです。
さてさて、こうなるとHarmonyのI2CドライバがDRV_I2C_TransmitThenReceive関数の呼び出しを受けて処理を開始し、各タイミングで発生する割り込みを処理し、送受信を完了させるまでの間にどこか問題があるということになって、一ユーザーがどうにかできるような領域じゃなくなってきてしまいます。困った困った。
ん…？各タイミングで発生する割り込み処理…？
設定を見ると、デフォルトでI2Cの割り込み優先度が1になっています。
タイマやUSBは4に設定されており、I2C割り込み中にもUSBの割り込みが発生することができます。
I2Cの割り込み処理をしている間に他の割り込みが入ってきて、受信データの処理が間に合わなくなったらどうなる…？ACKを送るタイミングを逃したらNACKだとスレーブが勘違いして送るのをやめるのでは…？
というわけで、優先度を最大の7にしてみました。
そうすると、無事、安定して動きましたとさ。
ふぅ、これで3日間くらい格闘してたぜ…。
これでやっと次のステップに移れる…。
0 件のコメント:
コメントを投稿