先週、秋月にて調達したL3GD20HジャイロモジュールをランサーストラトスのSPI予備端子に接続しました。ショートを防ぐためハヤコート（緑）で絶縁していたものを、ハヤコートリムーバで除去し、手持ちの両面ユニバーサル基盤やピンソケット等を使って、見てくれは悪いですがとりあえずハードは接続できました。
ジャイロモジュールの追加実装
モジュール調達前に準備したプログラムはやはりうまく作動しませんでした。都合３日ほどかけてやっとSPI接続ボーレート５MHｚで読み出しができるように修正しました。
参考までに私の関数は以下の通りです。
/*
* spiL3GD20H.c
*/
#include "stm32f4xx_hal.h"
#include <stdio.h>
SPI_HandleTypeDef hspi3;
/////////////////////////L3GD20H////////////////////////////
#define CTRL4 0x23 //フルスケールの設定レジスター
#define CTRL1 0x20 //ODR,カットオフ、パワーモード、XYZ軸の有効化選択レジスター
///////////////////////////////////////////////////////////
uint8_t GyroTempData[3];//センサからのデータ格納用配列
int16_t gZ = 0;//16bitの出力データ
//float gyroZ = 0;//ジャイロセンサから求めた角速度
//SPI通信用関数
void L3G_SPI_Write (uint8_t data, uint8_t WriteAddr)
{
HAL_GPIO_WritePin(GPIOB, GPIO_PIN_2, 0); //PB2 Low L3GD20H select
uint8_t cmd[2] = {WriteAddr | 0x00, data};
HAL_SPI_Transmit(&hspi3, cmd, 2, 1);
HAL_GPIO_WritePin(GPIOB, GPIO_PIN_2, 1); //PB2 high L3GD20H de-select
}
void L3G_SPI_Read(uint8_t *pBuffer, uint8_t ReadAddr, uint16_t NumByteToRead)
{
HAL_GPIO_WritePin(GPIOB, GPIO_PIN_2, 0); //PB2 Low L3GD20H select
uint8_t cmd[1+NumByteToRead];
cmd[0] = ReadAddr | 0x80 | 0x40;
for(int i = 1; i <= NumByteToRead; i++)
cmd[i] = 0x00;//dummy byte
HAL_SPI_TransmitReceive(&hspi3, cmd, pBuffer, NumByteToRead+1, 1);
HAL_GPIO_WritePin(GPIOB, GPIO_PIN_2, 1); //PB2 high L3GD20H de-select
}
void writeRegister(uint8_t subAddress, uint8_t data)
{
L3G_SPI_Write(data, subAddress);
HAL_Delay(10);
}
void readRegisters(uint8_t subAddress, uint8_t count, uint8_t* dest){
L3G_SPI_Read(dest, subAddress, count);
}
void L3GD20H_setup()
{
writeRegister(CTRL4, 0x30); //00110000 Full scale; 2000 dps
writeRegister(CTRL1, 0x8C); //10001100 ODR; 400Hz, Cut off; 20Hz, Power mode; Normal, Z; enable, Y and X; disable
}
int16_t L3GD20H_GetData()
{
readRegisters(0x2C, 2, GyroTempData);
gZ = (int16_t)( (uint16_t)(GyroTempData[2]<< 8) + GyroTempData[1] );
return gZ;
}
Z軸出力のみ取り出しています。しかも物理量には換算していません。ネットに転がっていたいろんなソースファイルを参考に作成しました。このプログラムはSTM32F405RGT6のSPI3を使っています。呼び出しにかかる時間は計測していません。
SDカードで取り出したログの例が以下の通りです。この時系列グラフは、10msec毎にサンプリングしたジャイロ出力値と、ステアリングサーボの現在値です。前のジャイロセンサMPU9250の出力よりはマシになりました。ヨーレートは前輪操舵角と車速度に関係した値ですが、グラフではほぼ同じ傾向に見えます。また、コース端での旋回中に一度当て舵的な操舵部分があります。これを藤原拓海君の様に何とか無くしてスムーズな旋回にしたいと考えています。
ジャイロ出力値とステアリング指令値
