初版

TinyGoとBNO055を使用したオイラー角表示テスト

セットアップや使用方法

(CC BY-NC-ND 4+) Creative Commons Attribution-NonCommercial-NoDerivs CC BY-NC-ND version 4.0 or later

# はじめに 　以前から、Tinygoでジャイロセンサをの実験をしてみたいと考えており、秋月電子でたBNO055を購入していました。BNO055は加速度、角速度、地磁気の9軸のデータが取得できるだけではなく、センサ自体にマイコンも内蔵して、センサの情報から四元数やオイラー角を出力することができます。今回はセンサから得られた四元数を使用して、オイラー角を計算して、表示させることにしました。 # BNO055について BNO055以下の特徴があります。 - センサーフュージョンをチップ内部で処理 3軸加速度・3軸ジャイロ・3軸地磁気の合計9軸の生データを、チップ内部のマイコンでリアルタイム計算します。そのため、メインマイコン側で複雑なカルマンフィルタやMadgwickフィルタを実装する必要がなく、プログラムの作成を簡単にしたり、マイコンへの負荷も減らすことができます。 - 多彩な姿勢データ出力に対応 加速度や角速度、地磁気だけではなくオイラー角、四元数を直接I2C/UARTで取得できます。 - マイコンの負荷軽減 姿勢計算が不要なため、マイコンにかかる負荷を最小限に抑えられます。 このように非常に便利なセンサですが、センサから得られるオイラー角を計算し、レジスタに出力するときに角度飛びや特定姿勢での回転誤差が生じるため、四元数からオイラー角を計算することで問題を解消しています。 四元数やオイラー角については説明が難しいので、GemminiなどのAIで聞いてください。（難しすぎて、私には理解できません・・・。） # 使用部品と回路図 使用した部品は表のとおりです。部品については秋月電子ですべて購入できます。マイコンもXiaoではなく、Raspberry Pi picoなどTinyGoが利用できるマイコンなら動作すると思います。今回はとりあえず、使っていないマイコンを使用しました。 | 使用部品 | 型番・値 |購入先| |:---:|:---:|:---:| | マイコン | seeed xiao |秋月電子| | 抵抗 | 2.2kΩ |秋月電子| | センサ | BNO055 |秋月電子| | 表示器 | SSD1306 |秋月電子| 回路図は図のとおりです。I2CでBNO055とSSD1306を接続しているだけです。  # プログラムについて TinyGoを使用して、作成しました。SSD1306についてはTinyGoのドライバを使用しています。また、表示用のフォントも外部の方が製作しているものを使用しています。BNO055の制御プログラムと動作プログラムは作成しました。 ```golang:BNO055制御プログラム package bno055 import ( "math" "time" "tinygo.org/x/drivers" ) // Accl 加速度センサデータ構造体 type Accl struct { XData uint16 // X方向 YData uint16 // Y方向 ZData uint16 // Z方向 } // Gyro ジャイロセンサデータ構造体 type Gyro struct { XData uint16 // X方向 YData uint16 // Y方向 ZData uint16 // Z方向 } // Mag 地磁気センサデータ構造体 type Mag struct { XData uint16 // X方向 YData uint16 // Y方向 ZData uint16 // Z方向 } // Euler オイラー角データ構造体 type Euler struct { HData uint16 RData uint16 PData uint16 } // Quaternion 四元数データ構造体 type Quaternion struct { WData uint16 XData uint16 YData uint16 ZData uint16 } // Sensor センサ情報格納用構造体 type Sensor struct { AcclData Accl GyroData Gyro MagData Mag EulerData Euler QutaData Quaternion } // Device デバイス情報格納用構造体 type Device struct { bus drivers.I2C deviceAddress uint8 chipId uint8 SensorData Sensor } type SensorCalibration struct { Systm uint16 Gyro uint16 Accel uint16 Mag uint16 } //--- 外部公開レシーバ --- // New レシーバ作成 func New(bus drivers.I2C, deviceAddress ...uint8) Device { d := Device{ bus: bus, deviceAddress: BNO055_Address_A, chipId: 0, SensorData: Sensor{}, } // アドレスが指定されていればそのアドレスに設定 if len(deviceAddress) > 0 { d.deviceAddress = deviceAddress[0] } return d } // Init センサ初期化関数 func (d *Device) Init(mode ...bno055_opmode) bool { var _mode bno055_opmode // モードチェック if len(mode) > 0 { _mode = mode[0] } else { _mode = OPERATION_MODE_IMUPLUS } // 起動チェック // 起動まで850ms time.Sleep(1000 * time.Millisecond) // 1秒待ち if !d.isConnected() { // 再度接続 time.Sleep(1000 * time.Millisecond) if !d.isConnected() { return false } } d.setmode(OPERATION_MODE_CONFIG) // リセット _ = d.writeRegister(d.deviceAddress, uint8(BNO055_SYS_TRIGGER_ADDR), []byte{0x20}) time.Sleep(100 * time.Millisecond) // 接続確認 for !d.isConnected() { time.Sleep(100 * time.Millisecond) } time.Sleep(50 * time.Millisecond) _ = d.writeRegister(d.deviceAddress, uint8(BNO055_PWR_MODE_ADDR), []byte{byte(POWER_MODE_NORMAL)}) time.Sleep(50 * time.Millisecond) _ = d.writeRegister(d.deviceAddress, uint8(BNO055_PAGE_ID_ADDR), []byte{0x00}) _ = d.writeRegister(d.deviceAddress, uint8(BNO055_SYS_TRIGGER_ADDR), []byte{0x00}) time.Sleep(50 * time.Millisecond) d.setmode(_mode) time.Sleep(50 * time.Millisecond) return true } // GetGyro ジャイロセンサデータ取得 func (d *Device) GetGyro() bool { data := make([]byte, 6) err := d.readRegister(d.deviceAddress, uint8(BNO055_GYRO_DATA_X_LSB_ADDR), data) if err != nil { return false } d.SensorData.GyroData.XData = uint16(data[1])<<8 | uint16(data[0]) d.SensorData.GyroData.YData = uint16(data[3])<<8 | uint16(data[2]) d.SensorData.GyroData.ZData = uint16(data[5])<<8 | uint16(data[4]) return true } // GetAccl 加速度センサデータ取得 func (d *Device) GetAccl() bool { data := make([]byte, 6) err := d.readRegister(d.deviceAddress, uint8(BNO055_ACCEL_DATA_X_LSB_ADDR), data) if err != nil { return false } d.SensorData.AcclData.XData = uint16(data[1])<<8 | uint16(data[0]) d.SensorData.AcclData.YData = uint16(data[3])<<8 | uint16(data[2]) d.SensorData.AcclData.ZData = uint16(data[5])<<8 | uint16(data[4]) return true } // GetAccl 地磁気センサデータ取得 func (d *Device) GetMag() bool { data := make([]byte, 6) err := d.readRegister(d.deviceAddress, uint8(BNO055_MAG_DATA_X_LSB_ADDR), data) if err != nil { return false } d.SensorData.MagData.XData = uint16(data[1])<<8 | uint16(data[0]) d.SensorData.MagData.YData = uint16(data[3])<<8 | uint16(data[2]) d.SensorData.MagData.ZData = uint16(data[5])<<8 | uint16(data[4]) return true } // GetEuler オイラー角データ取得 func (d *Device) GetEuler() bool { data := make([]byte, 6) err := d.readRegister(d.deviceAddress, uint8(BNO055_EULER_H_LSB_ADDR), data) if err != nil { return false } d.SensorData.EulerData.HData = uint16(data[1])<<8 | uint16(data[0]) d.SensorData.EulerData.RData = uint16(data[3])<<8 | uint16(data[2]) d.SensorData.EulerData.PData = uint16(data[5])<<8 | uint16(data[4]) return true } // GetQuaternion 四元数データ取得 func (d *Device) GetQuaternion() bool { data := make([]byte, 8) err := d.readRegister(d.deviceAddress, uint8(BNO055_QUATERNION_DATA_W_LSB_ADDR), data) if err != nil { return false } d.SensorData.QutaData.WData = uint16(data[1])<<8 | uint16(data[0]) d.SensorData.QutaData.XData = uint16(data[3])<<8 | uint16(data[2]) d.SensorData.QutaData.YData = uint16(data[5])<<8 | uint16(data[4]) d.SensorData.QutaData.ZData = uint16(data[7])<<8 | uint16(data[6]) return true } // QuaternionToEuler 四元数データからオイラー角を計算 func (d *Device) QuaternionToEuler() (chk bool, roll float64, pich float64, yaw float64) { var w, x, y, z float64 chk = d.GetQuaternion() if !chk { return chk, 0.0, 0.0, 0.0 } else { w = float64(int16(d.SensorData.QutaData.WData)) / float64(16384.0) x = float64(int16(d.SensorData.QutaData.XData)) / float64(16384.0) y = float64(int16(d.SensorData.QutaData.YData)) / float64(16384.0) z = float64(int16(d.SensorData.QutaData.ZData)) / float64(16384.0) // roll (x-axis rotation) ysqr := y * y t0 := +2.0 * (w*x + y*z) t1 := +1.0 - 2.0*(x*x+ysqr) roll = math.Atan2(t0, t1) * (180 / math.Pi) // pitch (y-axis rotation) t2 := +2.0 * (w*y - z*x) if t2 > 1.0 { t2 = 1.0 } else if t2 < -1.0 { t2 = -1.0 } pich = math.Asin(t2) * (180 / math.Pi) // yaw (z-axis rotation) t3 := +2.0 * (w*z + x*y) t4 := +1.0 - 2.0*(ysqr+z*z) yaw = math.Atan2(t3, t4) * (180 / math.Pi) } return chk, roll, pich, yaw } // PrintchipId チップID表示 func (d *Device) PrintchipId() { println(d.chipId) } // PrintchipId チップID表示 func (d *Device) GetTemp() (chk bool, temp int8) { data := make([]byte, 1) err := d.readRegister(d.deviceAddress, uint8(BNO055_TEMP_ADDR), data) if err != nil { return false, 0 } temp = int8(data[0]) return true, temp } // GetCalibration func (d *Device) GetCalibration() (chk bool, snr SensorCalibration) { data := make([]byte, 1) err := d.readRegister(d.deviceAddress, uint8(BNO055_CALIB_STAT_ADDR), data) if err != nil { snr.Accel = 0xff snr.Mag = 0xff snr.Gyro = 0xff snr.Systm = 0xff chk = false } else { snr.Systm = (uint16(data[0]) >> 6) & 0x03 snr.Gyro = (uint16(data[0]) >> 4) & 0x03 snr.Accel = (uint16(data[0]) >> 2) & 0x03 snr.Mag = (uint16(data[0])) & 0x03 chk = true } return chk, snr } // isConnected センサとの接続を確認 func (d *Device) isConnected() bool { data := make([]byte, 1) time.Sleep(100 * time.Millisecond) err := d.readRegister(d.deviceAddress, uint8(BNO055_CHIP_ID_ADDR), data) if err != nil { return false } d.chipId = data[0] return data[0] == BNO055_ID } // setmode func (d *Device) setmode(mode bno055_opmode) { var wData []byte wData = append(wData, byte(mode)) _ = d.writeRegister(d.deviceAddress, uint8(BNO055_OPR_MODE_ADDR), wData) time.Sleep(100 * time.Millisecond) } // readRegister I2C用レジスタ読み込み関数 func (d *Device) readRegister(addr uint8, reg uint8, data []byte) error { return d.bus.Tx(uint16(addr), []byte{reg}, data) } // writeRegister I2C用レジスタ書き込み関数 func (d *Device) writeRegister(addr uint8, reg uint8, data []byte) error { buf := make([]uint8, len(data)+1) buf[0] = reg copy(buf[1:], data) return d.bus.Tx(uint16(addr), buf, nil) } ``` ```golang:メインプログラム package main import ( "bno055/bno055" "fmt" font "github.com/Nondzu/ssd1306_font" "machine" "time" "tinygo.org/x/drivers/ssd1306" ) type sensor struct { roll float64 pich float64 yaw float64 } func getSensor(snrCh chan<- sensor) { var chk bool var snr sensor d := bno055.New(machine.I2C0) _ = d.Init() for { for { chk, snr.roll, snr.pich, snr.yaw = d.QuaternionToEuler() if chk { break } time.Sleep(time.Millisecond * 100) } fmt.Printf("Euler roll=%f, pich=%f, yaw=%f \n", snr.roll, snr.pich, snr.yaw) snrCh <- snr time.Sleep(time.Millisecond * 10) } } func ctrlLed(ledCh chan<- bool) { led := machine.LED led.Configure(machine.PinConfig{Mode: machine.PinOutput}) for { led.Low() time.Sleep(time.Millisecond * 500) led.High() time.Sleep(time.Millisecond * 500) ledCh <- true } } func procDisp(snrCh <-chan sensor, ch3 chan<- bool) { dev := ssd1306.NewI2C(machine.I2C0) dev.Configure(ssd1306.Config{ Address: 0x3C, Width: 128, Height: 64, }) dev.ClearBuffer() dev.ClearDisplay() //font library init display := font.NewDisplay(*dev) display.Configure(font.Config{FontType: font.FONT_11x18}) lcdprint := func(x int16, y int16, str string) { display.XPos = x // set position X display.YPos = y // set position Y display.PrintText(str) } for { select { case s := <-snrCh: lcdprint(0, 2, fmt.Sprintf("Roll=%5.1f", s.roll)) lcdprint(0, 22, fmt.Sprintf("Pich=%5.1f", s.pich)) lcdprint(0, 42, fmt.Sprintf("Yaw=%5.1f", s.yaw)) break } time.Sleep(time.Millisecond * 10) ch3 <- true } } func init() { err := machine.I2C0.Configure(machine.I2CConfig{ Frequency: 400 * machine.KHz, }) if err != nil { return } } func main() { // 起動待ち time.Sleep(time.Millisecond * 100) // チャネル作成 snrCh := make(chan sensor, 1) ledCh := make(chan bool, 1) devCh := make(chan bool, 1) go getSensor(snrCh) go ctrlLed(ledCh) go procDisp(snrCh, devCh) for { // Receive channel data from Goroutine select { case <-ledCh: break case <-devCh: break } } } ``` # 動作している様子 実際に動作している動画です。 @[youtube](https://youtu.be/Kq7ASdrKIeU) # 最後に 今回、BNO055を使用したオイラー角表示をしてみました。このセンサをうまく使えば、姿勢制御を簡単にできるように感じました。次の機会に、姿勢制御にチャレンジしてみたいと思います。