elchika elchika
ログイン/登録
keiske-hongyoのアイコン画像
keiske-hongyo 2026年08月13日作成 © CC BY-NC-ND 4+
セットアップや使用方法 セットアップや使用方法 閲覧数 32
keiske-hongyo 2026年08月13日作成 © CC BY-NC-ND 4+ セットアップや使用方法 セットアップや使用方法 閲覧数 32

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

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

はじめに

以前から、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の制御プログラムと動作プログラムは作成しました。

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)
}
メインプログラム
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
		}
	}
}

動作している様子

実際に動作している動画です。

ここに動画が表示されます

最後に

今回、BNO055を使用したオイラー角表示をしてみました。このセンサをうまく使えば、姿勢制御を簡単にできるように感じました。次の機会に、姿勢制御にチャレンジしてみたいと思います。

keiske-hongyoのアイコン画像
keiske-hongyo
マイコンを活用した電子工作に取り組んでいます。 もしよかったら、色々とアドバイスをいただけたらと思います。
ログインしてコメントを投稿する

投稿者の人気記事

ライフエンジ