diff --git a/mpu6050/digital_motion_processor/.DS_Store b/mpu6050/digital_motion_processor/.DS_Store new file mode 100644 index 000000000..644f493c3 Binary files /dev/null and b/mpu6050/digital_motion_processor/.DS_Store differ diff --git a/mpu6050/digital_motion_processor/Readme.md b/mpu6050/digital_motion_processor/Readme.md new file mode 100644 index 000000000..7c4084ec7 --- /dev/null +++ b/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. + + + diff --git a/mpu6050/digital_motion_processor/imu/imu.go b/mpu6050/digital_motion_processor/imu/imu.go new file mode 100644 index 000000000..31f41015e --- /dev/null +++ b/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) +} diff --git a/mpu6050/digital_motion_processor/main.go b/mpu6050/digital_motion_processor/main.go new file mode 100644 index 000000000..86fe516a4 --- /dev/null +++ b/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") + } +} diff --git a/mpu6050/digital_motion_processor/mpu6050/DMP.go b/mpu6050/digital_motion_processor/mpu6050/DMP.go new file mode 100644 index 000000000..faada387c --- /dev/null +++ b/mpu6050/digital_motion_processor/mpu6050/DMP.go @@ -0,0 +1,555 @@ +// This module owes its design to Jeff Rowberg +// https://github.com/ElectronicCats/mpu6050 + +// It is a stripped down and minimilistic version for DMP purposes +// as used by Maker's Wharf + +// The DMP module and functions was exported and translated from the C/C++ +// implementation by Jeff Rowberg ( Electronic Cats) and +// converted for Tinygo use as an extened mpu6050 device (see Tinygo devices). + +// The work was inspired by this video from Maker's Wharf +// https://www.youtube.com/watch?v=k5i-vE5rZR0 + +// Tested with the Arduino-Zero Tinygo machine (SAMD21) + +package mpu6050 + +import ( + "errors" + "strconv" + "time" +) + +var ( + print_debug_message = false +) + +/** Power on and prepare for general usage. + * This will activate the device and take it out of sleep mode (which must be done + * after start-up). This function also sets both the accelerometer and the gyroscope + * to their most sensitive settings, namely +/- 2g and +/- 250 degrees/sec, and sets + * the clock source to use the X Gyro for reference, which is slightly better than + * the default internal clock source. + */ +func (d Device) Initialize() error { + err := d.SetClockSource(CLOCK_PLL_XGYRO) + if err != nil { + return err + } + + err = d.SetFullScaleGyroRange(GYRO_FS_250) + if err != nil { + return err + } + + err = d.SetFullScaleAccelRange(AFS_RANGE_2G) + if err != nil { + return err + } + + err = d.SetSleepEnabled(false) + if err != nil { + return err + } + return nil +} + +// SetSleepEnabled - toggle the mpu6050 sleep mode (0x00 == OFF, 0x01 == SLEEP MODE ON) +func (d Device) SetSleepEnabled(s bool) error { + if s == true { + return d.WriteBit(PWR_MGMT_1, PWR_MGMT_1_SLEEP_BIT, 0x01) + } else { + return d.WriteBit(PWR_MGMT_1, PWR_MGMT_1_SLEEP_BIT, 0x00) + } +} + +func (d Device) SetXGyroOffset(offset int16) error { + return d.WriteWords(XG_OFFS_USRH, []int16{offset}) +} + +func (d Device) GetXGyroOffset() (int16, error) { + buf := make([]int16, 1) + if err := d.ReadWords(XG_OFFS_USRH, buf); err != nil { + return buf[0], err + } + return buf[0], nil +} + +func (d Device) SetYGyroOffset(offset int16) error { + return d.WriteWords(YG_OFFS_USRH, []int16{offset}) +} + +func (d Device) GetYGyroOffset() (int16, error) { + buf := make([]int16, 1) + if err := d.ReadWords(YG_OFFS_USRH, buf); err != nil { + return buf[0], err + } + return buf[0], nil +} + +func (d Device) SetZGyroOffset(offset int16) error { + return d.WriteWords(ZG_OFFS_USRH, []int16{offset}) +} + +func (d Device) GetZGyroOffset() (int16, error) { + buf := make([]int16, 1) + if err := d.ReadWords(ZG_OFFS_USRH, buf); err != nil { + return buf[0], err + } + return buf[0], nil +} + +func (d Device) SetXAccelOffset(offset int16) error { + var saveAddress uint8 + id, _ := d.getDeviceID() + if id < 0x38 { + saveAddress = XA_OFFS_H + } else { + saveAddress = 0x77 + } + return d.WriteWords(saveAddress, []int16{offset}) +} + +func (d Device) GetXAccelOffset() (int16, error) { + var saveAddress uint8 + buf := make([]int16, 1) + id, _ := d.getDeviceID() + if id < 0x38 { + saveAddress = XA_OFFS_H + } else { + saveAddress = 0x77 + } + if err := d.ReadWords(saveAddress, buf); err != nil { + return buf[0], err + } + return buf[0], nil +} + +func (d Device) SetYAccelOffset(offset int16) error { + var saveAddress uint8 + id, _ := d.getDeviceID() + if id < 0x38 { + saveAddress = YA_OFFS_H + } else { + saveAddress = 0x7A + } + return d.WriteWords(saveAddress, []int16{offset}) +} + +func (d Device) GetYAccelOffset() (int16, error) { + var saveAddress uint8 + buf := make([]int16, 1) + id, _ := d.getDeviceID() + if id < 0x38 { + saveAddress = YA_OFFS_H + } else { + saveAddress = 0x7A + } + if err := d.ReadWords(saveAddress, buf); err != nil { + return buf[0], err + } + return buf[0], nil +} + +func (d Device) SetZAccelOffset(offset int16) error { + var saveAddress uint8 + id, _ := d.getDeviceID() + if id < 0x38 { + saveAddress = ZA_OFFS_H + } else { + saveAddress = 0x7D + } + return d.WriteWords(saveAddress, []int16{offset}) +} + +func (d Device) GetZAccelOffset() (int16, error) { + var saveAddress uint8 + buf := make([]int16, 1) + id, _ := d.getDeviceID() + if id < 0x38 { + saveAddress = ZA_OFFS_H + } else { + saveAddress = 0x7D + } + if err := d.ReadWords(saveAddress, buf); err != nil { + return buf[0], err + } + return buf[0], nil +} + +func (d Device) TestConnection() error { + if !d.Connected() { + return errors.New("Connection test for MPU6050 failed.") + } + return nil +} + +func debug_print(str string) { + if !print_debug_message { + return + } + println(str) +} + +func (d Device) DMPinitialize() error { + // reset device + debug_print("\n\nResetting MPU6050...\n") + d.reset() + time.Sleep(time.Millisecond * 30) + + // disable sleep mode + d.SetSleepEnabled(false) + + // get MPU hardware revision + d.setMemoryBank(0x10, true, true) + d.setMemoryStartAddress(0x06) + debug_print("Checking hardware revision...") + debug_print("Revision @ user[16][6] = " + strconv.Itoa(int(d.readMemoryByte()))) + debug_print("Resetting memory bank selection to 0...") + d.setMemoryBank(0, false, false) + + // check OTP bank valid + debug_print("Reading OTP bank valid flag...") + if d.getOTPBankValid() { + debug_print("OTP bank is valid!") + } else { + debug_print("OTP bank is invalid!") + } + + // setup weird slave stuff (?) + debug_print("Setting slave 0 address to 0x7F...") + d.setSlaveAddress(0, 0x7F) + debug_print("Disabling I2C Master mode...") + d.setI2CMasterModeEnabled(false) + debug_print("Setting slave 0 address to 0x68 (self)...") + d.setSlaveAddress(0, 0x68) + debug_print("Resetting I2C Master control...") + d.resetI2CMaster() + time.Sleep(time.Millisecond * 20) + + debug_print("Setting clock source to Z Gyro...") + d.setClockSource(CLOCK_PLL_ZGYRO) + + debug_print("Setting DMP and FIFO_OFLOW interrupts enabled...") + d.setIntEnabled(1< 0 +} + +func (d Device) setSlaveAddress(num uint8, address uint8) { + if num > 3 { + return + } + d.WriteBytes(I2C_SLV0_ADDR+num*3, []byte{address}) +} + +func (d Device) setI2CMasterModeEnabled(s bool) { + if s == true { + d.WriteBit(USER_CTRL, USER_CTRL_I2C_MST_EN_BIT, 0x01) + } else { + d.WriteBit(USER_CTRL, USER_CTRL_I2C_MST_EN_BIT, 0x00) + } +} + +func (d Device) resetI2CMaster() { + d.WriteBit(USER_CTRL, USER_CTRL_I2C_MST_RESET_BIT, 0x01) +} + +func (d Device) setClockSource(source uint8) error { + return d.WriteBits(PWR_MGMT_1, PWR_MGMT_1_CLKSEL_BIT, PWR_MGMT_1_CLKSEL_LENGTH, source) +} + +func (d Device) setIntEnabled(interrupt uint8) error { + return d.WriteBytes(INT_ENABLE, []byte{interrupt}) +} + +func (d Device) setRate(rate uint8) error { + return d.WriteBytes(SMPLRT_DIV, []byte{rate}) +} + +func (d Device) setExternalFrameSync(sync uint8) error { + return d.WriteBits(CONFIG, CONFIG_EXT_SYNC_SET_BIT, CONFIG_EXT_SYNC_SET_LENGTH, sync) +} + +func (d Device) setDLPFMode(mode uint8) error { + return d.WriteBits(CONFIG, CONFIG_DLPF_CFG_BIT, CONFIG_DLPF_CFG_LENGTH, mode) +} + +func (d Device) setFullScaleGyroRange(rnge uint8) error { + return d.WriteBits(GYRO_CONFIG, GYRO_CONFIG_FS_SEL_BIT, GYRO_CONFIG_FS_SEL_LENGTH, rnge) +} + +func (d Device) setDMPConfig1(config uint8) error { + return d.WriteBytes(DMP_CFG_1, []byte{config}) +} + +func (d Device) setDMPConfig2(config uint8) error { + return d.WriteBytes(DMP_CFG_2, []byte{config}) +} + +func (d Device) setOTPBankValid(s bool) error { + if s == true { + return d.WriteBit(XG_OFFS_TC, TC_OTP_BNK_VLD_BIT, 0x01) // 0x01 == set enabled + } else { + return d.WriteBit(XG_OFFS_TC, TC_OTP_BNK_VLD_BIT, 0x00) // 0x00 == set disabled + } +} + +func (d Device) setMotionDetectionThreshold(threshold uint8) error { + return d.WriteBytes(MOT_THR, []byte{threshold}) +} + +func (d Device) setZeroMotionDetectionThreshold(threshold uint8) error { + return d.WriteBytes(ZRMOT_THR, []byte{threshold}) +} + +func (d Device) setMotionDetectionDuration(duration uint8) error { + return d.WriteBytes(MOT_DUR, []byte{duration}) +} + +func (d Device) setZeroMotionDetectionDuration(duration uint8) error { + return d.WriteBytes(ZRMOT_DUR, []byte{duration}) +} + +func (d Device) setFIFOEnabled(s bool) error { + if s == true { + return d.WriteBit(USER_CTRL, USER_CTRL_FIFO_EN_BIT, 0x01) // 0x01 enabled + } else { + return d.WriteBit(USER_CTRL, USER_CTRL_FIFO_EN_BIT, 0x00) // 0x00 disabled + } +} + +func (d Device) resetDMP() error { + return d.WriteBit(USER_CTRL, USER_CTRL_DMP_RESET_BIT, 0x01) +} + +func (d Device) setDMPEnabled(s bool) error { + if s == true { + return d.WriteBit(USER_CTRL, USER_CTRL_DMP_EN_BIT, 0x01) // 0x01 set enablrd + } else { + return d.WriteBit(USER_CTRL, USER_CTRL_DMP_EN_BIT, 0x00) // 0x00 set disablrd + } +} + +func (d Device) resetFIFO() error { + return d.WriteBit(USER_CTRL, USER_CTRL_FIFO_RESET_BIT, 0x01) +} + +func (d Device) getIntStatus() uint8 { + buf := []byte{0} + d.ReadBytes(INT_STATUS, buf) + return buf[0] +} + +func (d Device) SetDMPenabled(s bool) error { + if s == true { + if err := d.WriteBit(USER_CTRL, USER_CTRL_DMP_EN_BIT, 0x01); err != nil { + return err + } + } else { + if err := d.WriteBit(USER_CTRL, USER_CTRL_DMP_EN_BIT, 0x00); err != nil { + return err + } + } + return nil +} + +// the DMP is dumping Quaternions as fast as it can. +// Maybe faster than we would like. So try and get a relative +// fresh complete packet from the FIFO buffer. +func (d Device) getGurrentFIFOPacket(buf []byte, length int16) { + var fifoC int16 + packetReceived := false + + iGetFIFOCount := func() int16 { + fifoC = int16(d.getFIFOCount()) + return fifoC + } + + for !packetReceived { + if iGetFIFOCount() > length { + if fifoC > 5*length { + d.resetFIFO() + time.Sleep(time.Millisecond) + // try to strip fragmented head + if iGetFIFOCount()%length != 0 { + trash := make([]byte, fifoC%length) + d.ReadBytes(FIFO_R_W, trash) + } + continue + } + if fifoC%length == 0 { + err := d.getFIFOBytes(buf, uint8(length)) + if err == nil { + packetReceived = true + } + } + } + } +} + +func (d Device) getFIFOTimeout() int64 { + return int64(FIFO_DEFAULT_TIMEOUT) +} + +func (d Device) getFIFOCount() uint16 { + buf := make([]byte, 2) + d.ReadBytes(FIFO_COUNTH, buf) + return (uint16(buf[0]) << 8) | uint16(buf[1]) +} + +func (d Device) getFIFOBytes(buf []byte, length uint8) error { + tbuf := make([]byte, length) + err := d.ReadBytes(FIFO_R_W, tbuf) + if err != nil { + return err + } + copy(buf, tbuf) + return nil +} + +func (d Device) DMPgetCurrentFIFOPacket(buf []byte) error { + dmpPacketSize := int16(d.dmpPacketSize) + if len(buf) < int(dmpPacketSize) { + return errors.New("Buffer size is too small") + } + d.getGurrentFIFOPacket(buf, dmpPacketSize) + + // dump the buffer + //for i := 0; i < int(dmpPacketSize); i++ { + // print(strconv.FormatInt(int64(buf[i]), 16)) + //} + //println() + return nil +} + +type Quaternion struct { + W float32 + X float32 + Y float32 + Z float32 +} + +type Angels struct { + Yaw float32 + Pitch float32 + Roll float32 +} + +func (d Device) DMPgetQuaternion(buf []byte) (Quaternion, error) { + data := make([]int16, 4) + q := Quaternion{} + data[0] = int16(uint16(buf[0])<<8 | uint16(buf[1])) + data[1] = int16(uint16(buf[4])<<8 | uint16(buf[5])) + data[2] = int16(uint16(buf[8])<<8 | uint16(buf[9])) + data[3] = int16(uint16(buf[12])<<8 | uint16(buf[13])) + + q.W = float32(data[0]) / 16384.0 + q.X = float32(data[1]) / 16384.0 + q.Y = float32(data[2]) / 16384.0 + q.Z = float32(data[3]) / 16384.0 + + // Not sure why this does not work - I guess we have to convert from uint to + // int before we cast to float32 as in the case above + //q.W = float32(binary.BigEndian.Uint32(buf[0:])) / 16384.0 + //q.X = float32(binary.BigEndian.Uint32(buf[4:])) / 16384.0 + //q.Y = float32(binary.BigEndian.Uint32(buf[8:])) / 16384.0 + //q.Z = float32(binary.BigEndian.Uint32(buf[12:])) / 16384.0 + return q, nil +} diff --git a/mpu6050/digital_motion_processor/mpu6050/DMP_firmware.go b/mpu6050/digital_motion_processor/mpu6050/DMP_firmware.go new file mode 100644 index 000000000..cf1a82d1f --- /dev/null +++ b/mpu6050/digital_motion_processor/mpu6050/DMP_firmware.go @@ -0,0 +1,151 @@ +package mpu6050 + +var ( + /* ================================================================================================ * + | Default MotionApps v2.0 42-byte FIFO packet structure: | + | | + | [QUAT W][ ][QUAT X][ ][QUAT Y][ ][QUAT Z][ ][GYRO X][ ][GYRO Y][ ] | + | 0 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 | + | | + | [GYRO Z][ ][ACC X ][ ][ACC Y ][ ][ACC Z ][ ][ ] | + | 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 | + * ================================================================================================ */ + + // this block of memory gets written to the MPU on start-up, and it seems + // to be volatile memory, so it has to be done each time (it only takes ~1 + // second though) + + // I Only Changed this by applying all the configuration data and capturing it before startup: + // *** this is a capture of the DMP Firmware after all the messy changes were made so we can just load it + PROGMEM = []byte{ + /* bank # 0 */ + 0xFB, 0x00, 0x00, 0x3E, 0x00, 0x0B, 0x00, 0x36, 0x00, 0x01, 0x00, 0x02, 0x00, 0x03, 0x00, 0x00, + 0x00, 0x65, 0x00, 0x54, 0xFF, 0xEF, 0x00, 0x00, 0xFA, 0x80, 0x00, 0x0B, 0x12, 0x82, 0x00, 0x01, + 0x00, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x28, 0x00, 0x00, 0xFF, 0xFF, 0x45, 0x81, 0xFF, 0xFF, 0xFA, 0x72, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x03, 0xE8, 0x00, 0x00, 0x00, 0x01, 0x00, 0x01, 0x7F, 0xFF, 0xFF, 0xFE, 0x80, 0x01, + 0x00, 0x1B, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x40, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 0x02, 0xCB, 0x47, 0xA2, 0x20, 0x00, 0x00, 0x00, + 0x20, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 0x60, 0x00, 0x00, 0x00, + 0x41, 0xFF, 0x00, 0x00, 0x00, 0x00, 0x0B, 0x2A, 0x00, 0x00, 0x16, 0x55, 0x00, 0x00, 0x21, 0x82, + 0xFD, 0x87, 0x26, 0x50, 0xFD, 0x80, 0x00, 0x00, 0x00, 0x1F, 0x00, 0x00, 0x00, 0x05, 0x80, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x02, 0x00, 0x00, 0x00, 0x03, 0x00, 0x00, + 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x04, 0x6F, 0x00, 0x02, 0x65, 0x32, 0x00, 0x00, 0x5E, 0xC0, + 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0xFB, 0x8C, 0x6F, 0x5D, 0xFD, 0x5D, 0x08, 0xD9, 0x00, 0x7C, 0x73, 0x3B, 0x00, 0x6C, 0x12, 0xCC, + 0x32, 0x00, 0x13, 0x9D, 0x32, 0x00, 0xD0, 0xD6, 0x32, 0x00, 0x08, 0x00, 0x40, 0x00, 0x01, 0xF4, + 0xFF, 0xE6, 0x80, 0x79, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0xD0, 0xD6, 0x00, 0x00, 0x27, 0x10, + /* bank # 1 */ + 0xFB, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x00, 0x01, 0x00, 0x00, 0x00, + 0x00, 0x00, 0xFA, 0x36, 0xFF, 0xBC, 0x30, 0x8E, 0x00, 0x05, 0xFB, 0xF0, 0xFF, 0xD9, 0x5B, 0xC8, + 0xFF, 0xD0, 0x9A, 0xBE, 0x00, 0x00, 0x10, 0xA9, 0xFF, 0xF4, 0x1E, 0xB2, 0x00, 0xCE, 0xBB, 0xF7, + 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x04, 0x00, 0x02, 0x00, 0x02, 0x02, 0x00, 0x00, 0x0C, + 0xFF, 0xC2, 0x80, 0x00, 0x00, 0x01, 0x80, 0x00, 0x00, 0xCF, 0x80, 0x00, 0x40, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x06, 0x00, 0x00, 0x00, 0x00, 0x14, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x09, 0x23, 0xA1, 0x35, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x03, 0x3F, 0x68, 0xB6, 0x79, 0x35, 0x28, 0xBC, 0xC6, 0x7E, 0xD1, 0x6C, + 0x80, 0x00, 0xFF, 0xFF, 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0xB2, 0x6A, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x3F, 0xF0, 0x00, 0x00, 0x00, 0x30, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x40, 0x00, + 0x00, 0x00, 0x25, 0x4D, 0x00, 0x2F, 0x70, 0x6D, 0x00, 0x00, 0x05, 0xAE, 0x00, 0x0C, 0x02, 0xD0, + /* bank # 2 */ + 0x00, 0x00, 0x00, 0x00, 0x00, 0x65, 0x00, 0x54, 0xFF, 0xEF, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x01, 0x00, 0x00, 0x44, 0x00, 0x01, 0x00, 0x05, 0x8B, 0xC1, 0x00, 0x00, 0x01, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x65, 0x00, 0x00, 0x00, 0x54, 0x00, 0x00, 0xFF, 0xEF, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x1B, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x1B, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + /* bank # 3 */ + 0xD8, 0xDC, 0xBA, 0xA2, 0xF1, 0xDE, 0xB2, 0xB8, 0xB4, 0xA8, 0x81, 0x91, 0xF7, 0x4A, 0x90, 0x7F, + 0x91, 0x6A, 0xF3, 0xF9, 0xDB, 0xA8, 0xF9, 0xB0, 0xBA, 0xA0, 0x80, 0xF2, 0xCE, 0x81, 0xF3, 0xC2, + 0xF1, 0xC1, 0xF2, 0xC3, 0xF3, 0xCC, 0xA2, 0xB2, 0x80, 0xF1, 0xC6, 0xD8, 0x80, 0xBA, 0xA7, 0xDF, + 0xDF, 0xDF, 0xF2, 0xA7, 0xC3, 0xCB, 0xC5, 0xB6, 0xF0, 0x87, 0xA2, 0x94, 0x24, 0x48, 0x70, 0x3C, + 0x95, 0x40, 0x68, 0x34, 0x58, 0x9B, 0x78, 0xA2, 0xF1, 0x83, 0x92, 0x2D, 0x55, 0x7D, 0xD8, 0xB1, + 0xB4, 0xB8, 0xA1, 0xD0, 0x91, 0x80, 0xF2, 0x70, 0xF3, 0x70, 0xF2, 0x7C, 0x80, 0xA8, 0xF1, 0x01, + 0xB0, 0x98, 0x87, 0xD9, 0x43, 0xD8, 0x86, 0xC9, 0x88, 0xBA, 0xA1, 0xF2, 0x0E, 0xB8, 0x97, 0x80, + 0xF1, 0xA9, 0xDF, 0xDF, 0xDF, 0xAA, 0xDF, 0xDF, 0xDF, 0xF2, 0xAA, 0x4C, 0xCD, 0x6C, 0xA9, 0x0C, + 0xC9, 0x2C, 0x97, 0x97, 0x97, 0x97, 0xF1, 0xA9, 0x89, 0x26, 0x46, 0x66, 0xB0, 0xB4, 0xBA, 0x80, + 0xAC, 0xDE, 0xF2, 0xCA, 0xF1, 0xB2, 0x8C, 0x02, 0xA9, 0xB6, 0x98, 0x00, 0x89, 0x0E, 0x16, 0x1E, + 0xB8, 0xA9, 0xB4, 0x99, 0x2C, 0x54, 0x7C, 0xB0, 0x8A, 0xA8, 0x96, 0x36, 0x56, 0x76, 0xF1, 0xB9, + 0xAF, 0xB4, 0xB0, 0x83, 0xC0, 0xB8, 0xA8, 0x97, 0x11, 0xB1, 0x8F, 0x98, 0xB9, 0xAF, 0xF0, 0x24, + 0x08, 0x44, 0x10, 0x64, 0x18, 0xF1, 0xA3, 0x29, 0x55, 0x7D, 0xAF, 0x83, 0xB5, 0x93, 0xAF, 0xF0, + 0x00, 0x28, 0x50, 0xF1, 0xA3, 0x86, 0x9F, 0x61, 0xA6, 0xDA, 0xDE, 0xDF, 0xD9, 0xFA, 0xA3, 0x86, + 0x96, 0xDB, 0x31, 0xA6, 0xD9, 0xF8, 0xDF, 0xBA, 0xA6, 0x8F, 0xC2, 0xC5, 0xC7, 0xB2, 0x8C, 0xC1, + 0xB8, 0xA2, 0xDF, 0xDF, 0xDF, 0xA3, 0xDF, 0xDF, 0xDF, 0xD8, 0xD8, 0xF1, 0xB8, 0xA8, 0xB2, 0x86, + /* bank # 4 */ + 0xB4, 0x98, 0x0D, 0x35, 0x5D, 0xB8, 0xAA, 0x98, 0xB0, 0x87, 0x2D, 0x35, 0x3D, 0xB2, 0xB6, 0xBA, + 0xAF, 0x8C, 0x96, 0x19, 0x8F, 0x9F, 0xA7, 0x0E, 0x16, 0x1E, 0xB4, 0x9A, 0xB8, 0xAA, 0x87, 0x2C, + 0x54, 0x7C, 0xB9, 0xA3, 0xDE, 0xDF, 0xDF, 0xA3, 0xB1, 0x80, 0xF2, 0xC4, 0xCD, 0xC9, 0xF1, 0xB8, + 0xA9, 0xB4, 0x99, 0x83, 0x0D, 0x35, 0x5D, 0x89, 0xB9, 0xA3, 0x2D, 0x55, 0x7D, 0xB5, 0x93, 0xA3, + 0x0E, 0x16, 0x1E, 0xA9, 0x2C, 0x54, 0x7C, 0xB8, 0xB4, 0xB0, 0xF1, 0x97, 0x83, 0xA8, 0x11, 0x84, + 0xA5, 0x09, 0x98, 0xA3, 0x83, 0xF0, 0xDA, 0x24, 0x08, 0x44, 0x10, 0x64, 0x18, 0xD8, 0xF1, 0xA5, + 0x29, 0x55, 0x7D, 0xA5, 0x85, 0x95, 0x02, 0x1A, 0x2E, 0x3A, 0x56, 0x5A, 0x40, 0x48, 0xF9, 0xF3, + 0xA3, 0xD9, 0xF8, 0xF0, 0x98, 0x83, 0x24, 0x08, 0x44, 0x10, 0x64, 0x18, 0x97, 0x82, 0xA8, 0xF1, + 0x11, 0xF0, 0x98, 0xA2, 0x24, 0x08, 0x44, 0x10, 0x64, 0x18, 0xDA, 0xF3, 0xDE, 0xD8, 0x83, 0xA5, + 0x94, 0x01, 0xD9, 0xA3, 0x02, 0xF1, 0xA2, 0xC3, 0xC5, 0xC7, 0xD8, 0xF1, 0x84, 0x92, 0xA2, 0x4D, + 0xDA, 0x2A, 0xD8, 0x48, 0x69, 0xD9, 0x2A, 0xD8, 0x68, 0x55, 0xDA, 0x32, 0xD8, 0x50, 0x71, 0xD9, + 0x32, 0xD8, 0x70, 0x5D, 0xDA, 0x3A, 0xD8, 0x58, 0x79, 0xD9, 0x3A, 0xD8, 0x78, 0x93, 0xA3, 0x4D, + 0xDA, 0x2A, 0xD8, 0x48, 0x69, 0xD9, 0x2A, 0xD8, 0x68, 0x55, 0xDA, 0x32, 0xD8, 0x50, 0x71, 0xD9, + 0x32, 0xD8, 0x70, 0x5D, 0xDA, 0x3A, 0xD8, 0x58, 0x79, 0xD9, 0x3A, 0xD8, 0x78, 0xA8, 0x8A, 0x9A, + 0xF0, 0x28, 0x50, 0x78, 0x9E, 0xF3, 0x88, 0x18, 0xF1, 0x9F, 0x1D, 0x98, 0xA8, 0xD9, 0x08, 0xD8, + 0xC8, 0x9F, 0x12, 0x9E, 0xF3, 0x15, 0xA8, 0xDA, 0x12, 0x10, 0xD8, 0xF1, 0xAF, 0xC8, 0x97, 0x87, + /* bank # 5 */ + 0x34, 0xB5, 0xB9, 0x94, 0xA4, 0x21, 0xF3, 0xD9, 0x22, 0xD8, 0xF2, 0x2D, 0xF3, 0xD9, 0x2A, 0xD8, + 0xF2, 0x35, 0xF3, 0xD9, 0x32, 0xD8, 0x81, 0xA4, 0x60, 0x60, 0x61, 0xD9, 0x61, 0xD8, 0x6C, 0x68, + 0x69, 0xD9, 0x69, 0xD8, 0x74, 0x70, 0x71, 0xD9, 0x71, 0xD8, 0xB1, 0xA3, 0x84, 0x19, 0x3D, 0x5D, + 0xA3, 0x83, 0x1A, 0x3E, 0x5E, 0x93, 0x10, 0x30, 0x81, 0x10, 0x11, 0xB8, 0xB0, 0xAF, 0x8F, 0x94, + 0xF2, 0xDA, 0x3E, 0xD8, 0xB4, 0x9A, 0xA8, 0x87, 0x29, 0xDA, 0xF8, 0xD8, 0x87, 0x9A, 0x35, 0xDA, + 0xF8, 0xD8, 0x87, 0x9A, 0x3D, 0xDA, 0xF8, 0xD8, 0xB1, 0xB9, 0xA4, 0x98, 0x85, 0x02, 0x2E, 0x56, + 0xA5, 0x81, 0x00, 0x0C, 0x14, 0xA3, 0x97, 0xB0, 0x8A, 0xF1, 0x2D, 0xD9, 0x28, 0xD8, 0x4D, 0xD9, + 0x48, 0xD8, 0x6D, 0xD9, 0x68, 0xD8, 0xB1, 0x84, 0x0D, 0xDA, 0x0E, 0xD8, 0xA3, 0x29, 0x83, 0xDA, + 0x2C, 0x0E, 0xD8, 0xA3, 0x84, 0x49, 0x83, 0xDA, 0x2C, 0x4C, 0x0E, 0xD8, 0xB8, 0xB0, 0xA8, 0x8A, + 0x9A, 0xF5, 0x20, 0xAA, 0xDA, 0xDF, 0xD8, 0xA8, 0x40, 0xAA, 0xD0, 0xDA, 0xDE, 0xD8, 0xA8, 0x60, + 0xAA, 0xDA, 0xD0, 0xDF, 0xD8, 0xF1, 0x97, 0x86, 0xA8, 0x31, 0x9B, 0x06, 0x99, 0x07, 0xAB, 0x97, + 0x28, 0x88, 0x9B, 0xF0, 0x0C, 0x20, 0x14, 0x40, 0xB8, 0xB0, 0xB4, 0xA8, 0x8C, 0x9C, 0xF0, 0x04, + 0x28, 0x51, 0x79, 0x1D, 0x30, 0x14, 0x38, 0xB2, 0x82, 0xAB, 0xD0, 0x98, 0x2C, 0x50, 0x50, 0x78, + 0x78, 0x9B, 0xF1, 0x1A, 0xB0, 0xF0, 0x8A, 0x9C, 0xA8, 0x29, 0x51, 0x79, 0x8B, 0x29, 0x51, 0x79, + 0x8A, 0x24, 0x70, 0x59, 0x8B, 0x20, 0x58, 0x71, 0x8A, 0x44, 0x69, 0x38, 0x8B, 0x39, 0x40, 0x68, + 0x8A, 0x64, 0x48, 0x31, 0x8B, 0x30, 0x49, 0x60, 0xA5, 0x88, 0x20, 0x09, 0x71, 0x58, 0x44, 0x68, + /* bank # 6 */ + 0x11, 0x39, 0x64, 0x49, 0x30, 0x19, 0xF1, 0xAC, 0x00, 0x2C, 0x54, 0x7C, 0xF0, 0x8C, 0xA8, 0x04, + 0x28, 0x50, 0x78, 0xF1, 0x88, 0x97, 0x26, 0xA8, 0x59, 0x98, 0xAC, 0x8C, 0x02, 0x26, 0x46, 0x66, + 0xF0, 0x89, 0x9C, 0xA8, 0x29, 0x51, 0x79, 0x24, 0x70, 0x59, 0x44, 0x69, 0x38, 0x64, 0x48, 0x31, + 0xA9, 0x88, 0x09, 0x20, 0x59, 0x70, 0xAB, 0x11, 0x38, 0x40, 0x69, 0xA8, 0x19, 0x31, 0x48, 0x60, + 0x8C, 0xA8, 0x3C, 0x41, 0x5C, 0x20, 0x7C, 0x00, 0xF1, 0x87, 0x98, 0x19, 0x86, 0xA8, 0x6E, 0x76, + 0x7E, 0xA9, 0x99, 0x88, 0x2D, 0x55, 0x7D, 0x9E, 0xB9, 0xA3, 0x8A, 0x22, 0x8A, 0x6E, 0x8A, 0x56, + 0x8A, 0x5E, 0x9F, 0xB1, 0x83, 0x06, 0x26, 0x46, 0x66, 0x0E, 0x2E, 0x4E, 0x6E, 0x9D, 0xB8, 0xAD, + 0x00, 0x2C, 0x54, 0x7C, 0xF2, 0xB1, 0x8C, 0xB4, 0x99, 0xB9, 0xA3, 0x2D, 0x55, 0x7D, 0x81, 0x91, + 0xAC, 0x38, 0xAD, 0x3A, 0xB5, 0x83, 0x91, 0xAC, 0x2D, 0xD9, 0x28, 0xD8, 0x4D, 0xD9, 0x48, 0xD8, + 0x6D, 0xD9, 0x68, 0xD8, 0x8C, 0x9D, 0xAE, 0x29, 0xD9, 0x04, 0xAE, 0xD8, 0x51, 0xD9, 0x04, 0xAE, + 0xD8, 0x79, 0xD9, 0x04, 0xD8, 0x81, 0xF3, 0x9D, 0xAD, 0x00, 0x8D, 0xAE, 0x19, 0x81, 0xAD, 0xD9, + 0x01, 0xD8, 0xF2, 0xAE, 0xDA, 0x26, 0xD8, 0x8E, 0x91, 0x29, 0x83, 0xA7, 0xD9, 0xAD, 0xAD, 0xAD, + 0xAD, 0xF3, 0x2A, 0xD8, 0xD8, 0xF1, 0xB0, 0xAC, 0x89, 0x91, 0x3E, 0x5E, 0x76, 0xF3, 0xAC, 0x2E, + 0x2E, 0xF1, 0xB1, 0x8C, 0x5A, 0x9C, 0xAC, 0x2C, 0x28, 0x28, 0x28, 0x9C, 0xAC, 0x30, 0x18, 0xA8, + 0x98, 0x81, 0x28, 0x34, 0x3C, 0x97, 0x24, 0xA7, 0x28, 0x34, 0x3C, 0x9C, 0x24, 0xF2, 0xB0, 0x89, + 0xAC, 0x91, 0x2C, 0x4C, 0x6C, 0x8A, 0x9B, 0x2D, 0xD9, 0xD8, 0xD8, 0x51, 0xD9, 0xD8, 0xD8, 0x79, + /* bank # 7 */ + 0xD9, 0xD8, 0xD8, 0xF1, 0x9E, 0x88, 0xA3, 0x31, 0xDA, 0xD8, 0xD8, 0x91, 0x2D, 0xD9, 0x28, 0xD8, + 0x4D, 0xD9, 0x48, 0xD8, 0x6D, 0xD9, 0x68, 0xD8, 0xB1, 0x83, 0x93, 0x35, 0x3D, 0x80, 0x25, 0xDA, + 0xD8, 0xD8, 0x85, 0x69, 0xDA, 0xD8, 0xD8, 0xB4, 0x93, 0x81, 0xA3, 0x28, 0x34, 0x3C, 0xF3, 0xAB, + 0x8B, 0xF8, 0xA3, 0x91, 0xB6, 0x09, 0xB4, 0xD9, 0xAB, 0xDE, 0xFA, 0xB0, 0x87, 0x9C, 0xB9, 0xA3, + 0xDD, 0xF1, 0x20, 0x28, 0x30, 0x38, 0x9A, 0xF1, 0x28, 0x30, 0x38, 0x9D, 0xF1, 0xA3, 0xA3, 0xA3, + 0xA3, 0xF2, 0xA3, 0xB4, 0x90, 0x80, 0xF2, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, + 0xA3, 0xB2, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xB0, 0x87, 0xB5, 0x99, 0xF1, 0x28, 0x30, 0x38, + 0x98, 0xF1, 0xA3, 0xA3, 0xA3, 0xA3, 0x97, 0xA3, 0xA3, 0xA3, 0xA3, 0xF3, 0x9B, 0xA3, 0x30, 0xDC, + 0xB9, 0xA7, 0xF1, 0x26, 0x26, 0x26, 0xFE, 0xD8, 0xFF, + } +) diff --git a/mpu6050/digital_motion_processor/mpu6050/base.go b/mpu6050/digital_motion_processor/mpu6050/base.go new file mode 100644 index 000000000..1eb2e187f --- /dev/null +++ b/mpu6050/digital_motion_processor/mpu6050/base.go @@ -0,0 +1,266 @@ +// Read and write to registers/address based on Jeff's utility functions +// These are very similar except for the fact that the length(size) of +// is implied by the length of the []byte. This works very well with +// the I2C read and write design in Tinygo device modules + +// Note: not all of Jeff Rowberg's utilities are implemented - Only those +// required for the DMP configuration + +package mpu6050 + +import ( + "encoding/binary" + "errors" + "strconv" +) + +// WriteBit writes 0 or 1 to bit position 0-7 +func (d Device) WriteBit(regAddr uint8, bitNum uint8, data uint8) error { + b := make([]byte, 1) + d.ReadBytes(regAddr, b) + if data != 0 { + b[0] |= (0x01 << bitNum) + } else { + b[0] &= 0xFF & ^(0x01 << bitNum) + } + return d.WriteBytes(regAddr, b) +} + +// WriteBits writes a set of bits of length at starting position bitStart +// +// 010 value to write +// +// 76543210 bit numbers +// +// xxx args: bitStart=4, length=3 +// +// 00011100 mask byte +// 10101111 original value (sample) +// 10100011 original & ~mask +// 10101011 masked | value +func (d Device) WriteBits(regAddr, bitStart, length, data uint8) error { + b := []byte{0} + err := d.ReadBytes(regAddr, b) + if err != nil { + return err + } + var mask uint8 + mask = ((1 << length) - 1) << (bitStart - length + 1) + data = data << (bitStart - length + 1) // shift data into correct position + data &= mask // zero all non-important bits in data + b[0] &= ^(mask) // zero all important bits in existing byte + b[0] |= data // combine data with existing byte + return d.WriteBytes(regAddr, b) +} + +// ReadBits reads a set of bits of length at starting position bitStart +// +// 01101001 read byte +// 76543210 bit numbers +// +// xxx args: bitStart=4, length=3 +// 010 masked +// -> 010 shifted +func (d Device) ReadBits(regAddr, bitStart, length uint8) (uint8, error) { + buf := []byte{0} + err := d.ReadBytes(regAddr, buf) + if err != nil { + return buf[0], err + } + var mask uint8 + mask = ((1 << length) - 1) << (bitStart - length + 1) + buf[0] &= mask + buf[0] = buf[0] >> (bitStart - length + 1) + + return buf[0], nil +} + +// ReadBytes reads bytes from the I2C device for the length of data +func (d Device) ReadBytes(reg uint8, data []byte) error { + return d.bus.Tx(uint16(d.Address), []byte{reg}, data) +} + +// WriteBytes writes bytes to the I2C device for the length of data +func (d Device) WriteBytes(reg uint8, data []byte) error { + buf := make([]uint8, len(data)+1) + buf[0] = reg + copy(buf[1:], data) + return d.bus.Tx(uint16(d.Address), buf, nil) +} + +// ReadWords reads int16 Arrays from the registers +func (d Device) ReadWords(reg uint8, data []int16) error { + b := make([]byte, len(data)*2) + err := d.ReadBytes(reg, b) + if err != nil { + return err + } + for i := 0; i < len(data); i++ { + data[i] = int16(binary.BigEndian.Uint16(b[i*2:])) + //data[i] = int16(b[2*i])<<8 | int16(b[2*i+1]) + } + return nil +} + +// WriteWords writes int16 Arrays to the registers +func (d *Device) WriteWords(regAddr uint8, data []int16) error { + b := make([]byte, len(data)*2) + for i := 0; i < len(data); i++ { + binary.BigEndian.PutUint16(b[i*2:], uint16(data[i])) + //b[i*2] = byte((data[i] >> 8) & 0xFF) + //b[i*2+1] = byte(data[i] & 0xFF) + } + return d.WriteBytes(regAddr, b) +} + +// Alla Jeff Rowberg style - better than mine! +func (d Device) writeMemoryBlock(data []byte, dataSize int, bank uint8, address uint8, verify bool) error { + // safety check - force user to specify length of data[] + if int(dataSize) != len(data) { + err_msg := "Size of datablock is invalid" + return errors.New(err_msg) + } + d.setMemoryBank(bank, false, false) + d.setMemoryStartAddress(address) + for i := 0; i < dataSize; { + + chunkSize := DMP_MEMORY_CHUNK_SIZE + + if i+chunkSize > dataSize { + chunkSize = dataSize - i + } + + if chunkSize > 256-int(address) { + chunkSize = 256 - int(address) + } + + if err := d.WriteBytes(MEM_R_W, data[i:i+chunkSize]); err != nil { + return err + } + + if verify { + d.setMemoryBank(bank, false, false) + d.setMemoryStartAddress(address) + buf := make([]byte, chunkSize) + if err := d.ReadBytes(MEM_R_W, buf); err != nil { + return err + } + for ii, b := range data[i : i+chunkSize] { + if b != buf[ii] { + // dump the buffer + println("length: ", len(buf)) + for j := 0; j < len(buf); j++ { + print(strconv.FormatInt(int64(buf[j]), 16)) + } + println() + // dump the data + for j := 0; j < chunkSize; j++ { + print(strconv.FormatInt(int64(data[i+j]), 16)) + } + println() + return errors.New("Verify writeMemoryBlock failed") + } + } + } + i += chunkSize + address += uint8(chunkSize) // address will wrap automatically at 256 - swith to a new bank and restart a 0 offset + if i < dataSize { + if address == 0 { + bank++ + } + d.setMemoryBank(bank, false, false) + d.setMemoryStartAddress(address) + } + } + return nil +} + +/* ------ MY version of the above ----- +// writeMemoryBlock -dumps []byte to the MPU6050 Read and Write address - the MPU6050 will take the data and move it to +// the bank and address as indicated +func (d Device) writeMemoryBlock(data []byte, dataSize uint16, bank uint8, address uint8, verify bool) error { + d.setMemoryBank(bank, false, false) + d.setMemoryStartAddress(address) + if int(dataSize) != len(data) { + err_msg := "Size of datablock is invalid" + return errors.New(err_msg) + } + n := dataSize + for n > 0 { + index := dataSize - n + if n >= DMP_MEMORY_CHUNK_SIZE { + if err := d.WriteBytes(MEM_R_W, data[index:index+DMP_MEMORY_CHUNK_SIZE]); err != nil { + return err + } + time.Sleep(time.Millisecond * 30) + if verify { + d.setMemoryBank(bank, false, false) + d.setMemoryStartAddress(address) + buf := make([]byte, DMP_MEMORY_CHUNK_SIZE) + if err := d.ReadBytes(MEM_R_W, buf); err != nil { + return err + } + for i, b := range data[index : index+DMP_MEMORY_CHUNK_SIZE] { + if b != buf[i] { + // dump the buffer + println("length: ", len(buf)) + for i := 0; i < len(buf); i++ { + print(strconv.FormatInt(int64(buf[i]), 16)) + } + println() + // dump the data + for i := 0; i < DMP_MEMORY_CHUNK_SIZE; i++ { + print(strconv.FormatInt(int64(data[int(index)+i]), 16)) + } + println() + return errors.New("Verify writeMemoryBlock failed") + } + } + } + n -= DMP_MEMORY_CHUNK_SIZE + address += DMP_MEMORY_CHUNK_SIZE // uint8 automatically wraps to 0 at 256 + + if address == 0 { // Increase bank ie next 256 chunk + bank++ + } + if err := d.setMemoryBank(bank, false, false); err != nil { + return err + } + if err := d.setMemoryStartAddress(address); err != nil { + return err + } + print(".") + } else { + if err := d.WriteBytes(MEM_R_W, data[index:index+n]); err != nil { + return err + } + if verify { + d.setMemoryBank(bank, false, false) + d.setMemoryStartAddress(address) + buf := make([]byte, n) + if err := d.ReadBytes(MEM_R_W, buf); err != nil { + return err + } + for i, b := range data[index : index+n] { + if b != buf[i] { + // dump the buffer + println("length: ", len(buf)) + for i := 0; i < len(buf); i++ { + print(strconv.FormatInt(int64(buf[i]), 16)) + } + println() + // dump the data + for i := 0; i < int(n); i++ { + print(strconv.FormatInt(int64(data[int(index)+i]), 16)) + } + println() + return errors.New("Verify writeMemoryBlock failed") + } + } + } + n = 0 + } + } + return nil +} +*/ diff --git a/mpu6050/digital_motion_processor/mpu6050/calibrate.go b/mpu6050/digital_motion_processor/mpu6050/calibrate.go new file mode 100644 index 000000000..8b94dc664 --- /dev/null +++ b/mpu6050/digital_motion_processor/mpu6050/calibrate.go @@ -0,0 +1,185 @@ +package mpu6050 + +import ( + "math" + "strconv" + "time" +) + +/* Arduino version +func mapValue(x, in_min, in_max, out_min, out_max int64) int64 { + val := (x-in_min)*(out_max-out_min)/(in_max-in_min) + out_min + println("mapValue: ", val) + return val +} +*/ + +// My verison +func mapValue(value, fromLow, fromHi, toLow, toHi int64) int64 { + /* + if value < fromLow { + value = fromLow + } + if value > toHi { + value = toHi + } + */ + inRatio := (value - fromLow) / (fromHi - fromLow) + outValue := toLow + inRatio*(toHi-toLow) + return outValue +} + +func (d Device) CalibrateGyro(Loops uint8) { + kP := float64(0.3) + kI := float64(90) + var x float64 + x = (100 - float64(mapValue(int64(Loops), 1, 5, 20, 0))) * .01 + kP *= x + kI *= x + + d.PID(0x43, kP, kI, Loops) +} + +func (d Device) CalibrateAccel(Loops uint8) { + + kP := float64(0.3) + kI := float64(20) + var x float64 + x = (100 - float64(mapValue(int64(Loops), 1, 5, 20, 0))) * .01 + kP *= x + kI *= x + d.PID(0x3B, kP, kI, Loops) +} + +func (d Device) PID(readAddress uint8, kP, kI float64, loops uint8) { + var saveAddress uint8 + gravity := uint16(8192) + shift := uint8(2) + id, _ := d.getDeviceID() + if readAddress == 0x3B { + ar, _ := d.getFullScaleAccelRange() + gravity = 16384 >> ar + if id < 0x38 { + saveAddress = 0x06 + } else { + saveAddress = 0x77 + shift = uint8(3) + } + } else { + saveAddress = 0x13 + } + + debug_print("id: 0x" + strconv.FormatUint(uint64(id), 16)) + debug_print("readAddress: 0x" + strconv.FormatUint(uint64(readAddress), 16)) + debug_print("saveAddress: 0x" + strconv.FormatUint(uint64(saveAddress), 16)) + debug_print("shift: 0x" + strconv.FormatUint(uint64(shift), 16)) + + data := make([]int16, 1) + var reading float64 + bitZero := make([]int16, 3) + var pid_error, pterm float64 + iterm := make([]float64, 3) + var esample int16 + var esum uint32 + + print(">") + for i := uint8(0); i < 3; i++ { + err := d.ReadWords(saveAddress+i*shift, data) + if err != nil { + println("Unable to read words. Aborting") + for { + //failure + } + } + reading = float64(data[0]) + if saveAddress != 0x13 { + bitZero[i] = data[0] & int16(1) + iterm[i] = reading * 8 + } else { + iterm[i] = reading * 4 + } + } + + for l := 0; l < int(loops); l++ { + esample = 0 + for c := 0; c < 100; c++ { + esum = 0 + for i := uint8(0); i < 3; i++ { + err := d.ReadWords(readAddress+i*2, data) + if err != nil { + println("Unable to read words. Aborting") + for { + //failure + } + } + reading = float64(data[0]) + if readAddress == 0x3B && i == 2 { + reading -= float64(gravity) + } + pid_error = -reading + esum += uint32(math.Abs(reading)) + pterm = kP * pid_error + iterm[i] += pid_error * 0.001 * kI // Integral term 1000 calculations + if saveAddress != 0x13 { + data[0] = int16(math.Round((pterm + iterm[i]) / 8)) // compute pid output + data[0] = int16(uint16(data[0])&0xFFFE) | bitZero[i] // insert Bit0 saved at beginning + } else { + data[0] = int16(math.Round((pterm + iterm[i]) / 4)) // compute PID output + } + err = d.WriteWords(saveAddress+i*shift, data) + if err != nil { + println("Unable to write words. Aborting") + for { + //failure + } + } + } + if (c == 99) && (esum > 1000) { + c = 0 + print("*") + } + var factor float64 + if readAddress == 0x3B { + factor = 0.05 + } else { + factor = 1.0 + } + if (float64(esum) * factor) < 5 { + esample++ + } + if (esum < 100) && (c > 10) && (esample >= 10) { + break + } + time.Sleep(time.Millisecond) + } + print(".") + kP *= 0.75 + kI *= 0.75 + for i := uint8(0); i < 3; i++ { + if saveAddress != 0x13 { + data[0] = int16(math.Round(iterm[i] / 8)) + data[0] = int16(uint16(data[0])&0xFFFE) | bitZero[i] + } else { + data[0] = int16(math.Round(iterm[i] / 4)) + } + err := d.WriteWords(saveAddress+i*shift, data) + if err != nil { + println("Error writing words to MPU6050. Aborting ...") + for { + //failure + } + } + } + + } + d.resetFIFO() + d.resetDMP() +} + +func (d Device) getFullScaleAccelRange() (uint8, error) { + return d.ReadBits(ACCEL_CONFIG, ACCEL_CONFIG_AFS_SEL_BIT, ACCEL_CONFIG_AFS_SEL_LENGTH) +} + +func (d Device) getDeviceID() (uint8, error) { + return d.ReadBits(WHO_AM_I, WHO_AM_I_BIT, WHO_AM_I_LENGTH) +} diff --git a/mpu6050/digital_motion_processor/mpu6050/mpu.go b/mpu6050/digital_motion_processor/mpu6050/mpu.go new file mode 100644 index 000000000..ea50e32df --- /dev/null +++ b/mpu6050/digital_motion_processor/mpu6050/mpu.go @@ -0,0 +1,98 @@ +// Package mpu6050 provides a driver for the MPU6050 accelerometer and gyroscope +// made by InvenSense. +// +// Datasheets: +// https://store.invensense.com/datasheets/invensense/MPU-6050_DataSheet_V3%204.pdf +// https://www.invensense.com/wp-content/uploads/2015/02/MPU-6000-Register-Map1.pdf +package mpu6050 // import "tinygo.org/x/drivers/mpu6050" + +import ( + "tinygo.org/x/drivers" +) + +// Device wraps an I2C connection to a MPU6050 device. +type Device struct { + bus drivers.I2C + Address uint16 + dmpPacketSize uint16 // DMP defualt packet size +} + +// New creates a new MPU6050 connection. The I2C bus must already be +// configured. +// +// This function only creates the Device object, it does not touch the device. +func New(bus drivers.I2C) Device { + return Device{bus: bus, Address: Address, dmpPacketSize: 42} +} + +// Connected returns whether a MPU6050 has been found. +// It does a "who am I" request and checks the response. +func (d Device) Connected() bool { + data := []byte{0} + d.ReadBytes(WHO_AM_I, data) + return data[0] == 0x68 +} + +// Configure sets up the device for communication. +func (d Device) Configure() error { + return d.SetClockSource(CLOCK_INTERNAL) +} + +// ReadAcceleration reads the current acceleration from the device and returns +// it in µg (micro-gravity). When one of the axes is pointing straight to Earth +// and the sensor is not moving the returned value will be around 1000000 or +// -1000000. +func (d Device) ReadAcceleration() (x int32, y int32, z int32) { + data := make([]byte, 6) + d.ReadBytes(ACCEL_XOUT_H, data) + // Now do two things: + // 1. merge the two values to a 16-bit number (and cast to a 32-bit integer) + // 2. scale the value to bring it in the -1000000..1000000 range. + // This is done with a trick. What we do here is essentially multiply by + // 1000000 and divide by 16384 to get the original scale, but to avoid + // overflow we do it at 1/64 of the value: + // 1000000 / 64 = 15625 + // 16384 / 64 = 256 + x = int32(int16((uint16(data[0])<<8)|uint16(data[1]))) * 15625 / 256 + y = int32(int16((uint16(data[2])<<8)|uint16(data[3]))) * 15625 / 256 + z = int32(int16((uint16(data[4])<<8)|uint16(data[5]))) * 15625 / 256 + return +} + +// ReadRotation reads the current rotation from the device and returns it in +// µ°/s (micro-degrees/sec). This means that if you were to do a complete +// rotation along one axis and while doing so integrate all values over time, +// you would get a value close to 360000000. +func (d Device) ReadRotation() (x int32, y int32, z int32) { + data := make([]byte, 6) + d.ReadBytes(GYRO_XOUT_H, data) + // First the value is converted from a pair of bytes to a signed 16-bit + // value and then to a signed 32-bit value to avoid integer overflow. + // Then the value is scaled to µ°/s (micro-degrees per second). + // This is done in the following steps: + // 1. Multiply by 250 * 1000_000 + // 2. Divide by 32768 + // The following calculation (x * 15625 / 2048 * 1000) is essentially the + // same but avoids overflow. First both operations are divided by 16 leading + // to multiply by 15625000 and divide by 2048, and then part of the multiply + // is done after the divide instead of before. + x = int32(int16((uint16(data[0])<<8)|uint16(data[1]))) * 15625 / 2048 * 1000 + y = int32(int16((uint16(data[2])<<8)|uint16(data[3]))) * 15625 / 2048 * 1000 + z = int32(int16((uint16(data[4])<<8)|uint16(data[5]))) * 15625 / 2048 * 1000 + return +} + +// SetClockSource allows the user to configure the clock source. +func (d Device) SetClockSource(source uint8) error { + return d.WriteBytes(PWR_MGMT_1, []uint8{source}) +} + +// SetFullScaleGyroRange allows the user to configure the scale range for the gyroscope. +func (d Device) SetFullScaleGyroRange(rng uint8) error { + return d.WriteBytes(GYRO_CONFIG, []uint8{rng}) +} + +// SetFullScaleAccelRange allows the user to configure the scale range for the accelerometer. +func (d Device) SetFullScaleAccelRange(rng uint8) error { + return d.WriteBytes(ACCEL_CONFIG, []uint8{rng}) +} diff --git a/mpu6050/digital_motion_processor/mpu6050/registers.go b/mpu6050/digital_motion_processor/mpu6050/registers.go new file mode 100644 index 000000000..18e02c6da --- /dev/null +++ b/mpu6050/digital_motion_processor/mpu6050/registers.go @@ -0,0 +1,233 @@ +package mpu6050 + +// Constants/addresses used for I2C. + +// The I2C address which this device listens to. +const Address = 0x68 + +// Registers. Names, addresses and comments copied from the datasheet. +const ( + // Self test registers + SELF_TEST_X = 0x0D + SELF_TEST_Y = 0x0E + SELF_TEST_Z = 0x0F + SELF_TEST_A = 0x10 + + SMPLRT_DIV = 0x19 // Sample rate divider + CONFIG = 0x1A // Configuration + GYRO_CONFIG = 0x1B // Gyroscope configuration + ACCEL_CONFIG = 0x1C // Accelerometer configuration + FIFO_EN = 0x23 // FIFO enable + + // I2C pass-through configuration + I2C_MST_CTRL = 0x24 + I2C_SLV0_ADDR = 0x25 + I2C_SLV0_REG = 0x26 + I2C_SLV0_CTRL = 0x27 + I2C_SLV1_ADDR = 0x28 + I2C_SLV1_REG = 0x29 + I2C_SLV1_CTRL = 0x2A + I2C_SLV2_ADDR = 0x2B + I2C_SLV2_REG = 0x2C + I2C_SLV2_CTRL = 0x2D + I2C_SLV3_ADDR = 0x2E + I2C_SLV3_REG = 0x2F + I2C_SLV3_CTRL = 0x30 + I2C_SLV4_ADDR = 0x31 + I2C_SLV4_REG = 0x32 + I2C_SLV4_DO = 0x33 + I2C_SLV4_CTRL = 0x34 + I2C_SLV4_DI = 0x35 + I2C_MST_STATUS = 0x36 + + // Interrupt configuration + INT_PIN_CFG = 0x37 // Interrupt pin/bypass enable configuration + INT_ENABLE = 0x38 // Interrupt enable + INT_STATUS = 0x3A // Interrupt status + + // Accelerometer measurements + ACCEL_XOUT_H = 0x3B + ACCEL_XOUT_L = 0x3C + ACCEL_YOUT_H = 0x3D + ACCEL_YOUT_L = 0x3E + ACCEL_ZOUT_H = 0x3F + ACCEL_ZOUT_L = 0x40 + + // Temperature measurement + TEMP_OUT_H = 0x41 + TEMP_OUT_L = 0x42 + + // Gyroscope measurements + GYRO_XOUT_H = 0x43 + GYRO_XOUT_L = 0x44 + GYRO_YOUT_H = 0x45 + GYRO_YOUT_L = 0x46 + GYRO_ZOUT_H = 0x47 + GYRO_ZOUT_L = 0x48 + + // External sensor data + EXT_SENS_DATA_00 = 0x49 + EXT_SENS_DATA_01 = 0x4A + EXT_SENS_DATA_02 = 0x4B + EXT_SENS_DATA_03 = 0x4C + EXT_SENS_DATA_04 = 0x4D + EXT_SENS_DATA_05 = 0x4E + EXT_SENS_DATA_06 = 0x4F + EXT_SENS_DATA_07 = 0x50 + EXT_SENS_DATA_08 = 0x51 + EXT_SENS_DATA_09 = 0x52 + EXT_SENS_DATA_10 = 0x53 + EXT_SENS_DATA_11 = 0x54 + EXT_SENS_DATA_12 = 0x55 + EXT_SENS_DATA_13 = 0x56 + EXT_SENS_DATA_14 = 0x57 + EXT_SENS_DATA_15 = 0x58 + EXT_SENS_DATA_16 = 0x59 + EXT_SENS_DATA_17 = 0x5A + EXT_SENS_DATA_18 = 0x5B + EXT_SENS_DATA_19 = 0x5C + EXT_SENS_DATA_20 = 0x5D + EXT_SENS_DATA_21 = 0x5E + EXT_SENS_DATA_22 = 0x5F + EXT_SENS_DATA_23 = 0x60 + + // I2C peripheral data out + I2C_PER0_DO = 0x63 + I2C_PER1_DO = 0x64 + I2C_PER2_DO = 0x65 + I2C_PER3_DO = 0x66 + I2C_MST_DELAY_CT = 0x67 + + // Clock settings + CLOCK_INTERNAL = 0x00 + CLOCK_PLL_XGYRO = 0x01 + CLOCK_PLL_YGYRO = 0x02 + CLOCK_PLL_ZGYRO = 0x03 + CLOCK_PLL_EXTERNAL_32_768_KZ = 0x04 + CLOCK_PLL_EXTERNAL_19_2_MHZ = 0x05 + CLOCK_RESERVED = 0x06 + CLOCK_STOP = 0x07 + + // Accelerometer settings + AFS_RANGE_2G = 0x00 + AFS_RANGE_4G = 0x01 + AFS_RANGE_8G = 0x02 + AFS_RANGE_16G = 0x03 + + // Gyroscope settings + FS_RANGE_250 = 0x00 + FS_RANGE_500 = 0x01 + FS_RANGE_1000 = 0x02 + FS_RANGE_2000 = 0x03 + + // other registers + SIGNAL_PATH_RES = 0x68 // Signal path reset + USER_CTRL = 0x6A // User control + PWR_MGMT_1 = 0x6B // Power Management 1 + PWR_MGMT_2 = 0x6C // Power Management 2 + FIFO_COUNTH = 0x72 // FIFO count registers (high bits) + FIFO_COUNTL = 0x73 // FIFO count registers (low bits) + FIFO_R_W = 0x74 // FIFO read/write + WHO_AM_I = 0x75 // Who am I + + // other data values (bits) + BANK_SEL = 0x6D // DMP bank selection + MEM_START_ADDR = 0x6E // Register - + MEM_R_W = 0x6F // Register - Memory + TC_OTP_BNK_VLD_BIT = 0x00 // bit 0 + XG_OFFS_TC = 0x00 // Register - [7] PWR_MODE, [6:1] XG_OFFS_TC, [0] OTP_BNK_VLD + DMP_CODE_SIZE = 1929 + DMP_FIFO_RATE_DIVISOR = 0x01 + DMP_MEMORY_CHUNK_SIZE = 16 + WIRE_BUFFER_LENGTH = 32 + + FIFO_DEFAULT_TIMEOUT = 11000 + + PWR_MGMT_1_DEVICE_RESET_BIT = 0x07 + PWR_MGMT_1_SLEEP_BIT = 0x06 + PWR_MGMT_1_CYCLE_BIT = 0x05 + PWR_MGMT_1_TEMP_DIS_BIT = 0x03 + PWR_MGMT_1_CLKSEL_BIT = 0x02 + PWR_MGMT_1_CLKSEL_LENGTH = 0x03 + + USER_CTRL_DMP_EN_BIT = 0x07 + USER_CTRL_FIFO_EN_BIT = 0x06 + USER_CTRL_I2C_MST_EN_BIT = 0x05 + USER_CTRL_I2C_IF_DIS_BIT = 0x04 + USER_CTRL_DMP_RESET_BIT = 0x03 + USER_CTRL_FIFO_RESET_BIT = 0x02 + USER_CTRL_I2C_MST_RESET_BIT = 0x01 + USER_CTRL_SIG_COND_RESET_BIT = 0x00 + + CONFIG_EXT_SYNC_SET_BIT = 0x05 + CONFIG_EXT_SYNC_SET_LENGTH = 0x03 + CONFIG_DLPF_CFG_BIT = 0x02 + CONFIG_DLPF_CFG_LENGTH = 0x03 + + GYRO_CONFIG_FS_SEL_BIT = 0x04 + GYRO_CONFIG_FS_SEL_LENGTH = 0x02 + + SYNC_DISABLED = 0x0 + SYNC_TEMP_OUT_L = 0x1 + SYNC_GYRO_XOUT_L = 0x2 + SYNC_GYRO_YOUT_L = 0x3 + SYNC_GYRO_ZOUT_L = 0x4 + SYNC_ACCEL_XOUT_L = 0x5 + SYNC_ACCEL_YOUT_L = 0x6 + SYNC_ACCEL_ZOUT_L = 0x7 + + DLPF_BW_256 = 0x00 + DLPF_BW_188 = 0x01 + DLPF_BW_98 = 0x02 + DLPF_BW_42 = 0x03 + DLPF_BW_20 = 0x04 + DLPF_BW_10 = 0x05 + DLPF_BW_5 = 0x06 + + GYRO_FS_250 = 0x00 + GYRO_FS_500 = 0x01 + GYRO_FS_1000 = 0x02 + GYRO_FS_2000 = 0x03 + + DMP_CFG_1 = 0x70 + DMP_CFG_2 = 0x71 + + MOT_THR = 0x1F + MOT_DUR = 0x20 + ZRMOT_THR = 0x21 + ZRMOT_DUR = 0x22 + + INTERRUPT_FF_BIT = 0x07 + INTERRUPT_MOT_BIT = 0x06 + INTERRUPT_ZMOT_BIT = 0x05 + INTERRUPT_FIFO_OFLOW_BIT = 0x04 + INTERRUPT_I2C_MST_INT_BIT = 0x03 + INTERRUPT_PLL_RDY_INT_BIT = 0x02 + INTERRUPT_DMP_INT_BIT = 0x01 + INTERRUPT_DATA_RDY_BIT = 0x00 + + ACCEL_CONFIG_XA_ST_BIT = 0x07 + ACCEL_CONFIG_YA_ST_BIT = 0x06 + ACCEL_CONFIG_ZA_ST_BIT = 0x05 + ACCEL_CONFIG_AFS_SEL_BIT = 0x04 + ACCEL_CONFIG_AFS_SEL_LENGTH = 0x02 + ACCEL_CONFIG_ACCEL_HPF_BIT = 0x02 + ACCEL_CONFIG_ACCEL_HPF_LENGTH = 0x03 + + WHO_AM_I_BIT = 0x06 + WHO_AM_I_LENGTH = 0x06 + + XG_OFFS_USRH = 0x13 //[15:0] XG_OFFS_USR + XG_OFFS_USRL = 0x14 + YG_OFFS_USRH = 0x15 //[15:0] YG_OFFS_USR + YG_OFFS_USRL = 0x16 + ZG_OFFS_USRH = 0x17 //[15:0] ZG_OFFS_USR + ZG_OFFS_USRL = 0x18 + + XA_OFFS_H = 0x06 //[15:0] XA_OFFS + XA_OFFS_L_TC = 0x07 + YA_OFFS_H = 0x08 //[15:0] YA_OFFS + YA_OFFS_L_TC = 0x09 + ZA_OFFS_H = 0x0A //[15:0] ZA_OFFS + ZA_OFFS_L_TC = 0x0B +) diff --git a/mpu6050/digital_motion_processor/python-animation/.DS_Store b/mpu6050/digital_motion_processor/python-animation/.DS_Store new file mode 100644 index 000000000..4fe009c99 Binary files /dev/null and b/mpu6050/digital_motion_processor/python-animation/.DS_Store differ diff --git a/mpu6050/digital_motion_processor/python-animation/Animation.py b/mpu6050/digital_motion_processor/python-animation/Animation.py new file mode 100644 index 000000000..4ae7393b5 --- /dev/null +++ b/mpu6050/digital_motion_processor/python-animation/Animation.py @@ -0,0 +1,124 @@ +# Check this youtube video to get +# details of this work +# https://www.youtube.com/watch?v=k5i-vE5rZR0 +from vpython import * +import math +import serial + +def rotationInfo( ringPos, arrowOffset, angleName): + """ + Returns ring with arrow indicating direction of rotation along a given axis + + ringPos: ring position + arrowOffset: arrow offset from ring center + angleName: this will be one of 'yaw, pitch or roll' + """ + + # Ring + rotRing = ring(pos = ringPos, axis = ringPos, radius = arrowOffset.mag, + thickness = 0.04) + # Arrow + ringArrow = arrow(pos = ringPos + arrowOffset, axis = cross(ringPos, arrowOffset), + radius = 0.2, thickness = 0.6, length = 0.3, shaftwidth = 0.05) + # TextLabel + ringText = text(pos = ringPos * (1 + (0.4 * ringPos.mag)), text = angleName, + color = color.white, height = 0.2, depth = 0.05, align = 'center', + up = vector(0, 0, -1)) + return compound([rotRing, ringArrow, ringText]) + +def setScene(): + """ + Set scene properties and creates objects that are fixed + """ + + # Scene + scene.range = 5 + # scene.forward = vector( -0.8, -1.2, -0.8) # uncomment to get a more 3D view + scene.background = color.cyan + scene.width = 1200 + scene.height = 1000 + + title = text(pos=vec(0, 3, 0), text='MPU6050', align='center', color=color.blue, height = 0.4, depth = 0.2) + + return title + +def createRotatingObjects(): + # MPU6050 module + # Original orientation as per video + mpu = box(length = 4, height = 2, width = .2, opacity = 0.3, pos = vector(0, 0, 0), color = color.blue ) + # orientation as per my board + # mpu = box(length = 2, height = .02, width = 4, opacity = 0.3, pos = vector(0, 0, 0), color = color.blue ) + + # markings on MPU6050 module + Yaxis = arrow(length = 2, shaftwidth = 0.1, axis = mpu.axis, color = color.white) + Xaxis = arrow(length = 0.7, shaftwidth = 0.1, axis = vector(0, 0, 1), color = color.white) + XaxisLabel = text(pos = vector(-0.6, 0.1, 0.7), text = 'X-axis', color = color.white, + height = 0.2, depth = 0.05, align = 'center', up = vector(0, 0, -1)) + YaxisLabel = text(pos = vector(1, 0.1, -0.1), text = 'Y-axis', color = color.white, + height = 0.2, depth = 0.05, align = 'center', up = vector(0, 0, -1)) + topSideLabel = text(pos = vector(-1.2, 0.1, -0.2), text = 'Top side', color = color.white, + height = 0.2, depth = 0.05, align = 'center', up = vector(0, 0, -1)) + + # Rings with arrows indicating yaw, pitch and roll direction of rotation + rotInfoY = rotationInfo(vector(2.4, 0, 0), vector( 0, 0, 0.2), 'roll') + rotInfoX = rotationInfo(vector(0, 0, 1.3), vector( 0.2, 0, 0), 'pitch') + rotInfoZ = rotationInfo(vector(0, -1, 0), vector( -0.2, 0, 0), 'yaw') + + return compound( [mpu, Xaxis, Yaxis, XaxisLabel, YaxisLabel, topSideLabel, + rotInfoY, rotInfoX, rotInfoZ]) + +def rodriquesRotation(v, k, angle): + return v * cos(angle) + cross(k, v) * sin(angle) + +title = setScene() +angles = label(pos=vec(-2.5, 2, 1), text='yaw= 0\npitch= 0\nroll= 0\n', align='center', color=color.black, height = 30, depth = 0) +rotatingObjects = createRotatingObjects() + +# initialize serial port +ArduinoSerial = serial.Serial('/dev/cu.usbmodem14401', 115200) + + +while (True): + # data might not be available at first so keep on trying + try: + # grab angles from arduino + while(ArduinoSerial.inWaiting() == 0): + pass + dataPacket = ArduinoSerial.readline() + dataPacket = str(dataPacket, 'utf-8') + splitPacket = dataPacket.split("\t") + yaw = float(splitPacket[0]) + pitch = float(splitPacket[1]) + roll = float(splitPacket[2]) + + # update display based on rotation + rate(50) # vpython needs this, it is sort of display update rate. + + # display values while they are still in degrees + angles.text = f'yaw: {round(yaw)}\npitch: {round(pitch)}\n roll: {round(roll)}' + + yaw = math.radians(yaw) + pitch = math.radians(pitch) + roll = math.radians(roll) + + # Let x and y point along the MPU's X and Y axes respectively. Let's stipulate + # that at zero yaw, pitch and roll - x and y are given by: + x = vector(0, 0, 1) + y = vector(1, 0, 0) + + # Let's apply the yaw rotation to x and y using the rodriques formula: + x = rodriquesRotation(x, vector(0, -1, 0), yaw) + y = rodriquesRotation(y, vector(0, -1, 0), yaw) + + # note x is invariant under pitch rotaion. Apply the pitch rotation to y; + y = rodriquesRotation(y, x, pitch) + + # y is invariant to the roll rotation. Apply the roll rotation to x: + x = rodriquesRotation(x, y, roll) + + rotatingObjects.axis = y + # draw the result by updating the up vector + rotatingObjects.up = cross(x, y) + except: + pass + diff --git a/mpu6050/digital_motion_processor/python-animation/activate_python3_vitual_env.sh b/mpu6050/digital_motion_processor/python-animation/activate_python3_vitual_env.sh new file mode 100755 index 000000000..142f85d2e --- /dev/null +++ b/mpu6050/digital_motion_processor/python-animation/activate_python3_vitual_env.sh @@ -0,0 +1,4 @@ +#! /bin/sh + +echo "Run the following command in your console to enable and activate the Python Virtual env." +echo "source ./bin/activate" diff --git a/mpu6050/digital_motion_processor/python-animation/create_python3_vitual_env.sh b/mpu6050/digital_motion_processor/python-animation/create_python3_vitual_env.sh new file mode 100755 index 000000000..f4009b256 --- /dev/null +++ b/mpu6050/digital_motion_processor/python-animation/create_python3_vitual_env.sh @@ -0,0 +1,4 @@ +#! /bin/sh + +echo "Run the following command in your console to create a new Python Virtual env." +echo "python3 -m venv ." diff --git a/mpu6050/digital_motion_processor/python-animation/install_python_modules.sh b/mpu6050/digital_motion_processor/python-animation/install_python_modules.sh new file mode 100644 index 000000000..b44d2a96d --- /dev/null +++ b/mpu6050/digital_motion_processor/python-animation/install_python_modules.sh @@ -0,0 +1,8 @@ +#! /bin/sh +# +echo "This script MUST run in the Python Virtual Environment." +echo "ie. ACTIVATE the environment with.." +echo "Run source ./bin/activate in your shell" + +pip3 install vpython +pip3 install pyserial diff --git a/mpu6050/digital_motion_processor/python-animation/kill_animation.sh b/mpu6050/digital_motion_processor/python-animation/kill_animation.sh new file mode 100755 index 000000000..819143446 --- /dev/null +++ b/mpu6050/digital_motion_processor/python-animation/kill_animation.sh @@ -0,0 +1,7 @@ +#! /bin/sh + +# stop the simulation that is running in the +# Python virtual enviroment - assunme no other python scripts +# are running +pkill -KILL -i python + diff --git a/mpu6050/digital_motion_processor/python-animation/listUSBports.py b/mpu6050/digital_motion_processor/python-animation/listUSBports.py new file mode 100644 index 000000000..216a08002 --- /dev/null +++ b/mpu6050/digital_motion_processor/python-animation/listUSBports.py @@ -0,0 +1,4 @@ +from serial.tools import list_ports +port = list(list_ports.comports()) +for p in port: + print(p.device) diff --git a/mpu6050/digital_motion_processor/python-animation/run_animation.sh b/mpu6050/digital_motion_processor/python-animation/run_animation.sh new file mode 100755 index 000000000..d2a8a6507 --- /dev/null +++ b/mpu6050/digital_motion_processor/python-animation/run_animation.sh @@ -0,0 +1,4 @@ +#! /bin/sh + +echo "Note: please run me in a Python Virtual Environment." +python3 Animation.py