add Digital Motion Processor for MPU6050 - see issue #733

mpu6050/digital_motion_processor/Readme.md

@@ -0,0 +1,49 @@
+# MPU6050 Digital Motion Processor (DMP) device driver for Tinygo
+
+A mpu6050 gyro that just works. Thanks to these smart people for smoothing out the
+wrinkles. 
+
+Inspired by Maker's Wharf which is a C/C++ implemntation of Jeff Rowberg's MPU6050
+Arduino library. See it here: 
+[https://www.youtube.com/watch?v=k5i-vE5rZR0](https://www.youtube.com/watch?v=k5i-vE5rZR0)  
+
+Jeff Rowberg's Arduino library : 
+[https://github.com/ElectronicCats/mpu6050](https://github.com/ElectronicCats/mpu6050)  
+
+
+## Implementation
+
+Tested with the arduino-zero(SAMD21) Tinygo machine.   
+
+To run:  clone the repository, `go mod install` or `go get tinygo.org/x/drivers`   
+
+Then `tinygo flash --target=arduino-zero -monitor main.go`.  
+
+It will flash the arduino zero (or similar samd21 based) board with the connected MPU6050 on the default i2c ports.
+( checkout the video for hardware connection to the MPU6050).   
+
+Use the tinygo usb driver - it is really just a matter of identifying and then connecting See video.  
+
+Note: The demo does not use interrupts  
+
+## Usage
+
+When the Arduino/MPU6050 starts up it will run the calibration for all 6-axis'.
+It should be placed on a flat horizontal surface and kept STILL. Once calibration 
+has completed, the yaw, pitch and roll will be dumped at approx 100 millisecond intervals.   
+
+The output can be used together with VPython for simple animation.  
+
+### VPython animation
+
+The repo has simple bash scripts to get VPython up and running. 
+A python virtual environment is the easiest to get going.  
+Use the scripts as a guide:  
+  - create the Vitual env.  
+  - activate the venv `source ./bin/activate`  
+  - install the vpython and pyserial modules with pip3  
+  - edit the Animation.py to reference your usb serial connection. Use `python3 listUSBports` to find your usb connection.  
+  - run `python3 Animation.py` to view the DMP output.
+
+
+

mpu6050/digital_motion_processor/imu/imu.go

@@ -0,0 +1,167 @@
+package imu
+
+import (
+	"errors"
+	"main/mpu6050"
+
+	m "math"
+
+	"tinygo.org/x/drivers"
+)
+
+type IMU struct {
+	mpu mpu6050.Device
+}
+
+func New(bus drivers.I2C) (IMU, error) {
+	imu := IMU{}
+	imu.mpu = mpu6050.New(bus)
+	// TODO: Setup I2C to talk to MPU6050 at 400kHz
+
+	err := imu.mpu.Initialize()
+	if err != nil {
+		println("Bad MPU6050 device status.")
+		return imu, err
+	}
+
+	err = imu.mpu.TestConnection()
+	if err != nil {
+		println("Test for MPU6050 failed.")
+		return imu, err
+	}
+
+	err = imu.mpu.DMPinitialize()
+	if err != nil {
+		println("Initialize Digital Motion Processor (DMP) failed.")
+		return imu, err
+	}
+	imu.GuessOffsets()
+
+	println("MPU6050 connected!")
+	return imu, nil
+}
+
+// Init initializes DMP on the mpu6050
+// Note: New() must be called before Init()
+func (imu *IMU) Init() error {
+	if err := imu.mpu.SetDMPenabled(true); err != nil {
+		println("Failed to enable Digital Motion Processor.")
+		return err
+	}
+	return nil
+}
+
+// Note: New() must be called before this
+func (imu *IMU) load_offsets_from_storage_and_start_dmp() error {
+	if imu.IsCalibrated() {
+		err := imu.loadCalibration()
+		if err != nil {
+			println("MPU6050 not calibrated.")
+			return err
+		}
+		err = imu.mpu.SetDMPenabled(true)
+		if err != nil {
+			println("Failed to enable Digital Motion Processor.")
+			return err
+		}
+	} else {
+		println("MPU not calibrated.")
+		return errors.New("MPU is not calibrated")
+	}
+	return nil
+}
+func (imu *IMU) IsCalibrated() bool {
+	// TODO: read calibration flag from storage
+	return true
+}
+
+func (imu *IMU) Calibrate() error {
+	println("Calibration invoked ...")
+	imu.mpu.CalibrateAccel(6)
+	imu.mpu.CalibrateGyro(6)
+
+	// TODO: store calibation to EEPROM or FLASH for
+	// retieval on restarts.
+	return nil
+}
+
+func (imu *IMU) PrintIMUOffsets() {
+	ax, _ := imu.mpu.GetXAccelOffset()
+	ay, _ := imu.mpu.GetYAccelOffset()
+	az, _ := imu.mpu.GetZAccelOffset()
+	gx, _ := imu.mpu.GetXGyroOffset()
+	gy, _ := imu.mpu.GetYGyroOffset()
+	gz, _ := imu.mpu.GetZGyroOffset()
+
+	println("Accel X: ", ax,
+		"\tAccel Y: ", ay,
+		"\tAccel Z: ", az,
+		"\tGyro X: ", gx,
+		"\tGyro Y: ", gy,
+		"\tGyro Z: ", gz,
+	)
+
+}
+
+// use loadCalibration to get the calibration from
+// EEPROM or FLASH storage - see Maker's Wharf for inspirational work.
+func (imu *IMU) loadCalibration() error {
+	// TODO: get calibration from storage
+	return nil
+}
+
+func (imu *IMU) GetYawPitchRoll() (mpu6050.Angels, error) {
+	fifo_buffer := make([]byte, 64)
+	angle := mpu6050.Angels{}
+
+	err := imu.mpu.DMPgetCurrentFIFOPacket(fifo_buffer)
+	if err != nil {
+		println("DMP buffer unavailable: ", err.Error())
+		return angle, err
+	}
+	q, err := imu.mpu.DMPgetQuaternion(fifo_buffer)
+	if err != nil {
+		println("FIFO buffer to Quaternion conversion error")
+		return angle, err
+	}
+	//print(q.W, "\t", q.X, "\t", q.Y, "\t", q.Z, "\n")
+	q0 := float64(q.W)
+	q1 := float64(q.X)
+	q2 := float64(q.Y)
+	q3 := float64(q.Z)
+
+	// this conversion is based on Maker's Wharf corrected conversion
+	// that does not have a gimbal lock  - check the video
+	yr := -m.Atan2(-2.0*q1*q2+2.0*q0*q3, q2*q2-q3*q3-q1*q1+q0*q0)
+	pr := m.Asin(2.0*q2*q3 + 2.0*q0*q1)
+	rr := m.Atan2(-2.0*q1*q3+2.0*q0*q2, q3*q3-q2*q2-q1*q1+q0*q0)
+
+	// convert to radians
+	angle.Yaw = float32(yr * 180.0 / m.Pi)
+	angle.Pitch = float32(pr * 180.0 / m.Pi)
+	angle.Roll = float32(rr * 180.0 / m.Pi)
+
+	return angle, nil
+}
+
+func (imu *IMU) GuessOffsets() {
+	println("Apply Guess OFFSETS .....")
+	/* Supply your gyro offsets here, scaled for min sensitivity */
+
+	imu.mpu.SetXGyroOffset(294)
+	imu.mpu.SetYGyroOffset(-24)
+	imu.mpu.SetZGyroOffset(19)
+	imu.mpu.SetXAccelOffset(-524)
+	imu.mpu.SetYAccelOffset(-2261)
+	imu.mpu.SetZAccelOffset(620)
+}
+
+// Use basic (NOT DMP mode) MPU functions to see if the I2C comms works
+func (imu *IMU) Test_MPU6050() {
+	imu.mpu.Initialize()
+	x, y, z := imu.mpu.ReadRotation()
+	println("Rotation x:", x, "  y:", y, " z:", z)
+
+	x, y, z = imu.mpu.ReadAcceleration()
+	println("Acceleration x:", x, "  y:", y, " z:", z)
+}

mpu6050/digital_motion_processor/main.go

@@ -0,0 +1,87 @@
+package main
+
+import (
+	"device/sam"
+	"encoding/binary"
+	"machine"
+	"main/imu"
+	"strconv"
+	"time"
+)
+
+var (
+	// declare a I2C port - configure from generic sercomSERIAL
+	I2C_MPU6050 = &machine.I2C{Bus: sam.SERCOM3_I2CM, SERCOM: 3}
+)
+
+func main() {
+	// wait for terminal to connect
+	time.Sleep(time.Millisecond * 500)
+	println("Initialize Accelerometer on MPU6050 ...")
+
+	// setup a sercomSERIAL for I2C use
+	err := I2C_MPU6050.Configure(machine.I2CConfig{
+		//Frequency: uint32(400e3), // 400kHz
+		Frequency: uint32(100e3), // 100kHz
+	})
+
+	if err != nil {
+		println("Initiating I2C failed. Aborting")
+		for {
+			// failure
+		}
+	}
+
+	// Create and Initialise MPU6050 for DMP mode
+	imu, err := imu.New(I2C_MPU6050)
+	if err != nil {
+		println("Unable to create a MPU6050 instance. Aborting.")
+		for {
+			// failure
+		}
+	}
+
+	if true {
+		err = imu.Init()
+		if err != nil {
+			println("Unable to initialize IMU. Aborting.")
+			for {
+				// failure
+			}
+		}
+		imu.Calibrate()
+		println("MPU calibrated!!")
+		imu.PrintIMUOffsets()
+		for {
+			angles, err := imu.GetYawPitchRoll()
+			if err == nil {
+				println(
+					strconv.FormatFloat(float64(angles.Yaw), 'f', 2, 32), "\t",
+					strconv.FormatFloat(float64(angles.Pitch), 'f', 2, 32), "\t",
+					strconv.FormatFloat(float64(angles.Roll), 'f', 2, 32))
+			}
+			time.Sleep(time.Millisecond * 100) // sleep for 100 milliseconds - allow Animation.py to keep up
+		}
+	}
+}
+
+func test_tinygo_internal_integer_endianess() {
+	println("Testing endianess")
+	// test endainess
+	test := uint16(0xAABB)
+	b := make([]byte, 2)
+	// There are two ways to load the 16 bit integers
+	// 1. Th e manual method
+	b[0] = byte(test >> 8 & 0xFF)
+	b[1] = byte(test & 0xFF)
+	println("0x", strconv.FormatUint(uint64(b[0]), 16), strconv.FormatUint(uint64(b[1]), 16))
+	if b[0] == 0xAA && b[1] == 0xBB {
+		println("Tinygo integer is stored as BIG ENDIAN")
+	}
+	// 2. The GO library  way
+	binary.BigEndian.PutUint16(b, test)
+	println("0x", strconv.FormatUint(uint64(b[0]), 16), strconv.FormatUint(uint64(b[1]), 16))
+	if b[0] == 0xAA && b[1] == 0xBB {
+		println("Tinygo integer is stored as BIG ENDIAN")
+	}
+}