Arduino Yun Mini + MPU9250

   iconI2C.png    volts33.png

Photo

Arduino Yun Mini + MPU9250
Arduino Yun Mini + MPU9250
Arduino Yun Mini + MPU9250
Arduino Yun Mini + MPU9250
Arduino Yun Mini + MPU9250

Sketch

// Arduino YUN mini connected to a MPU9250 motion processing unit
// Copyright (C) 2021 https://www.roboticboat.uk
// dc4cd91a-54f0-43c4-8393-094881d9812d
//
// This program is free software: you can redistribute it and/or modify
// it under the terms of the GNU General Public License as published by
// the Free Software Foundation, either version 3 of the License, or
// (at your option) any later version.
//
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
// GNU General Public License for more details.
//
// You should have received a copy of the GNU General Public License
// along with this program. If not, see <https://www.gnu.org/licenses/>.
// These Terms shall be governed and construed in accordance with the laws of
// England and Wales, without regard to its conflict of law provisions.
#include "Arduino.h"
#include <Wire.h>
#include <Bridge.h>
#include <YunServer.h>
#include <YunClient.h>
// The one global server object listening for incoming connections.
#define PORT 23
YunServer Server(PORT);
// Define an array of simultaneous clients
#define NUM_CLIENTS 2
YunClient clients[NUM_CLIENTS];
// an MPU9250 object with its I2C address
// of 0x68 (ADDR to GRND) and on Teensy bus 0
//MPU9250 IMU(0x68, 0);
float ax, ay, az;
float gx, gy, gz;
float hx, hy, hz, t;
int beginStatus;
int nReceived;
//---------------------------------
enum mpu9250_gyro_range
{
GYRO_RANGE_250DPS,
GYRO_RANGE_500DPS,
GYRO_RANGE_1000DPS,
GYRO_RANGE_2000DPS
};
enum mpu9250_accel_range
{
ACCEL_RANGE_2G,
ACCEL_RANGE_4G,
ACCEL_RANGE_8G,
ACCEL_RANGE_16G
};
enum mpu9250_dlpf_bandwidth
{
DLPF_BANDWIDTH_184HZ,
DLPF_BANDWIDTH_92HZ,
DLPF_BANDWIDTH_41HZ,
DLPF_BANDWIDTH_20HZ,
DLPF_BANDWIDTH_10HZ,
DLPF_BANDWIDTH_5HZ
};
uint8_t _address;
uint8_t _bus;
bool _userDefI2C;
uint8_t _csPin;
bool _useSPI;
bool _useSPIHS;
float _accelScale;
float _gyroScale;
float _magScaleX, _magScaleY, _magScaleZ;
const float _tempScale = 333.87f;
const float _tempOffset = 21.0f;
// i2c bus frequency
const uint32_t _i2cRate = 400000;
// constants
const float G = 9.807f;
const float _d2r = 3.14159265359f/180.0f;
// MPU9250 registers
const uint8_t ACCEL_OUT = 0x3B;
const uint8_t GYRO_OUT = 0x43;
const uint8_t TEMP_OUT = 0x41;
const uint8_t EXT_SENS_DATA_00 = 0x49;
const uint8_t ACCEL_CONFIG = 0x1C;
const uint8_t ACCEL_FS_SEL_2G = 0x00;
const uint8_t ACCEL_FS_SEL_4G = 0x08;
const uint8_t ACCEL_FS_SEL_8G = 0x10;
const uint8_t ACCEL_FS_SEL_16G = 0x18;
const uint8_t GYRO_CONFIG = 0x1B;
const uint8_t GYRO_FS_SEL_250DPS = 0x00;
const uint8_t GYRO_FS_SEL_500DPS = 0x08;
const uint8_t GYRO_FS_SEL_1000DPS = 0x10;
const uint8_t GYRO_FS_SEL_2000DPS = 0x18;
const uint8_t ACCEL_CONFIG2 = 0x1D;
const uint8_t ACCEL_DLPF_184 = 0x01;
const uint8_t ACCEL_DLPF_92 = 0x02;
const uint8_t ACCEL_DLPF_41 = 0x03;
const uint8_t ACCEL_DLPF_20 = 0x04;
const uint8_t ACCEL_DLPF_10 = 0x05;
const uint8_t ACCEL_DLPF_5 = 0x06;
const uint8_t CONFIG = 0x1A;
const uint8_t GYRO_DLPF_184 = 0x01;
const uint8_t GYRO_DLPF_92 = 0x02;
const uint8_t GYRO_DLPF_41 = 0x03;
const uint8_t GYRO_DLPF_20 = 0x04;
const uint8_t GYRO_DLPF_10 = 0x05;
const uint8_t GYRO_DLPF_5 = 0x06;
const uint8_t SMPDIV = 0x19;
const uint8_t INT_PIN_CFG = 0x37;
const uint8_t INT_ENABLE = 0x38;
const uint8_t INT_DISABLE = 0x00;
const uint8_t INT_PULSE_50US = 0x00;
const uint8_t INT_RAW_RDY_EN = 0x01;
const uint8_t PWR_MGMNT_1 = 0x6B;
const uint8_t PWR_RESET = 0x80;
const uint8_t CLOCK_SEL_PLL = 0x01;
const uint8_t PWR_MGMNT_2 = 0x6C;
const uint8_t SEN_ENABLE = 0x00;
const uint8_t USER_CTRL = 0x6A;
const uint8_t I2C_MST_EN = 0x20;
const uint8_t I2C_MST_CLK = 0x0D;
const uint8_t I2C_MST_CTRL = 0x24;
const uint8_t I2C_SLV0_ADDR = 0x25;
const uint8_t I2C_SLV0_REG = 0x26;
const uint8_t I2C_SLV0_DO = 0x63;
const uint8_t I2C_SLV0_CTRL = 0x27;
const uint8_t I2C_SLV0_EN = 0x80;
const uint8_t I2C_READ_FLAG = 0x80;
const uint8_t WHO_AM_I = 0x75;
// AK8963 registers
const uint8_t AK8963_I2C_ADDR = 0x0C;
const uint8_t AK8963_HXL = 0x03;
const uint8_t AK8963_CNTL1 = 0x0A;
const uint8_t AK8963_PWR_DOWN = 0x00;
const uint8_t AK8963_CNT_MEAS1 = 0x12;
const uint8_t AK8963_CNT_MEAS2 = 0x16;
const uint8_t AK8963_FUSE_ROM = 0x0F;
const uint8_t AK8963_CNTL2 = 0x0B;
const uint8_t AK8963_RESET = 0x01;
const uint8_t AK8963_ASA = 0x10;
const uint8_t AK8963_WHO_AM_I = 0x00;
// transformation matrix
/* transform the accel and gyro axes to match the magnetometer axes */
const int16_t tX[3] = {0, 1, 0};
const int16_t tY[3] = {1, 0, 0};
const int16_t tZ[3] = {0, 0, -1};
//------------------------------
void setup() {
Bridge.begin();
_address = 0x68; // I2C address
_bus = 0; // I2C bus
_userDefI2C = false; // automatic I2C setup
_useSPI = false; // set to use I2C instead of SPI
// serial to display data
Serial.begin(9600);
// start communication with IMU and
// set the accelerometer and gyro ranges.
// ACCELEROMETER 2G 4G 8G 16G
// GYRO 250DPS 500DPS 1000DPS 2000DPS
beginStatus = begin(ACCEL_RANGE_4G,GYRO_RANGE_250DPS);
Server.noListenOnLocalhost(); // Listen for external connections
Server.begin();
}
void loop() {
// Process every client
for (byte i = 0; i<NUM_CLIENTS; i++)
{
if (clients[i].connected()) // Is it connected?
process(i); // It's connected, process the client
else
{
if ((bool)clients[i]) // Not connected. Is the client open?
{
// The client is not connected, but is currently open.
Serial.print("Lost connection to client ");
Serial.println(i);
clients[i].stop(); // Close and clean up the old connection
}
// At this point, the client is not connected, and is closed.
clients[i] = Server.accept(); // Try to establish a connection.
if (clients[i].connected()) // Was a connection made?
{
Serial.print("New connection on client ");
Serial.println(i);
// get the accel (m/s/s), gyro (rad/s), and magnetometer (uT), and temperature (C) data
getMotion10(&ax, &ay, &az, &gx, &gy, &gz, &hx, &hy, &hz, &t);
// print the data
clients[i].print("$ACC,");
clients[i].print(ax,6);
clients[i].print(",");
clients[i].print(ay,6);
clients[i].print(",");
clients[i].print(az,6);
clients[i].print("\t");
clients[i].print("$GYR,");
clients[i].print(gx,6);
clients[i].print(",");
clients[i].print(gy,6);
clients[i].print(",");
clients[i].print(gz,6);
clients[i].print("\t");
clients[i].print("$MAG,");
clients[i].print(hx,6);
clients[i].print(",");
clients[i].print(hy,6);
clients[i].print(",");
clients[i].print(hz,6);
clients[i].print("\t");
clients[i].print("$TMP,");
clients[i].println(t,6);
clients[i].stop();
}
}
}
}
void getMotion10Counts(int16_t* ax, int16_t* ay, int16_t* az, int16_t* gx, int16_t* gy, int16_t* gz, int16_t* hx, int16_t* hy, int16_t* hz, int16_t* t){
uint8_t buff[21];
int16_t axx, ayy, azz, gxx, gyy, gzz;
_useSPIHS = true; // use the high speed SPI for data readout
readRegisters(ACCEL_OUT, sizeof(buff), &buff[0]); // grab the data from the MPU9250
axx = (((int16_t)buff[0]) << 8) | buff[1]; // combine into 16 bit values
ayy = (((int16_t)buff[2]) << 8) | buff[3];
azz = (((int16_t)buff[4]) << 8) | buff[5];
*t = (((int16_t)buff[6]) << 8) | buff[7];
gxx = (((int16_t)buff[8]) << 8) | buff[9];
gyy = (((int16_t)buff[10]) << 8) | buff[11];
gzz = (((int16_t)buff[12]) << 8) | buff[13];
*hx = (((int16_t)buff[15]) << 8) | buff[14];
*hy = (((int16_t)buff[17]) << 8) | buff[16];
*hz = (((int16_t)buff[19]) << 8) | buff[18];
*ax = tX[0]*axx + tX[1]*ayy + tX[2]*azz; // transform axes
*ay = tY[0]*axx + tY[1]*ayy + tY[2]*azz;
*az = tZ[0]*axx + tZ[1]*ayy + tZ[2]*azz;
*gx = tX[0]*gxx + tX[1]*gyy + tX[2]*gzz;
*gy = tY[0]*gxx + tY[1]*gyy + tY[2]*gzz;
*gz = tZ[0]*gxx + tZ[1]*gyy + tZ[2]*gzz;
}
void getMotion10(float* ax, float* ay, float* az, float* gx, float* gy, float* gz, float* hx, float* hy, float* hz, float* t){
int16_t accel[3];
int16_t gyro[3];
int16_t mag[3];
int16_t tempCount;
getMotion10Counts(&accel[0], &accel[1], &accel[2], &gyro[0], &gyro[1], &gyro[2], &mag[0], &mag[1], &mag[2], &tempCount);
*ax = ((float) accel[0]) * _accelScale; // typecast and scale to values
*ay = ((float) accel[1]) * _accelScale;
*az = ((float) accel[2]) * _accelScale;
*gx = ((float) gyro[0]) * _gyroScale;
*gy = ((float) gyro[1]) * _gyroScale;
*gz = ((float) gyro[2]) * _gyroScale;
*hx = ((float) mag[0]) * _magScaleX;
*hy = ((float) mag[1]) * _magScaleY;
*hz = ((float) mag[2]) * _magScaleZ;
*t = (( ((float) tempCount) - _tempOffset )/_tempScale) + _tempOffset;
}
void readRegisters(uint8_t subAddress, uint8_t count, uint8_t* dest){
if( _useSPI ){
}
else{
Wire.beginTransmission(_address); // open the device
Wire.write(subAddress); // specify the starting register address
Wire.endTransmission(false);
nReceived = Wire.requestFrom(_address, count); // specify the number of bytes to receive
// Something has gone wrong
if (nReceived != count) return;
uint8_t i = 0; // read the data into the buffer
while( Wire.available() ){
dest[i++] = Wire.read();
}
}
}
void readAK8963Registers(uint8_t subAddress, uint8_t count, uint8_t* dest){
writeRegister(I2C_SLV0_ADDR,AK8963_I2C_ADDR | I2C_READ_FLAG); // set slave 0 to the AK8963 and set for read
writeRegister(I2C_SLV0_REG,subAddress); // set the register to the desired AK8963 sub address
writeRegister(I2C_SLV0_CTRL,I2C_SLV0_EN | count); // enable I2C and request the bytes
delayMicroseconds(100); // takes some time for these registers to fill
readRegisters(EXT_SENS_DATA_00,count,dest); // read the bytes off the MPU9250 EXT_SENS_DATA registers
}
bool writeRegister(uint8_t subAddress, uint8_t data){
uint8_t buff[1];
/* write data to device */
if( _useSPI ){
}
else{
Wire.beginTransmission(_address); // open the device
Wire.write(subAddress); // write the register address
Wire.write(data); // write the data
Wire.endTransmission();
}
delay(10); // need to slow down how fast I write to MPU9250
/* read back the register */
readRegisters(subAddress,sizeof(buff),&buff[0]);
/* check the read back register against the written register */
if(buff[0] == data) {
return true;
}
else{
return false;
}
}
bool writeAK8963Register(uint8_t subAddress, uint8_t data){
uint8_t count = 1;
uint8_t buff[1];
writeRegister(I2C_SLV0_ADDR,AK8963_I2C_ADDR); // set slave 0 to the AK8963 and set for write
writeRegister(I2C_SLV0_REG,subAddress); // set the register to the desired AK8963 sub address
writeRegister(I2C_SLV0_DO,data); // store the data for write
writeRegister(I2C_SLV0_CTRL,I2C_SLV0_EN | count); // enable I2C and send 1 byte
// read the register and confirm
readAK8963Registers(subAddress, sizeof(buff), &buff[0]);
if(buff[0] == data) {
return true;
}
else{
return false;
}
}
//------------------------------ WHO AM I ------------------
uint8_t whoAmI(){
uint8_t buff[1];
// read the WHO AM I register
readRegisters(WHO_AM_I,sizeof(buff),&buff[0]);
// return the register value
return buff[0];
}
uint8_t whoAmIAK8963(){
uint8_t buff[1];
// read the WHO AM I register
readAK8963Registers(AK8963_WHO_AM_I,sizeof(buff),&buff[0]);
// return the register value
return buff[0];
}
//----------------------------- BEGIN ----------------------
/* starts I2C communication and sets up the MPU-9250 */
int begin(mpu9250_accel_range accelRange, mpu9250_gyro_range gyroRange){
uint8_t buff[3];
uint8_t data[7];
if( _useSPI ){ // using SPI for communication
}
else{ // using I2C for communication
Wire.begin();
}
// select clock source to gyro
if( !writeRegister(PWR_MGMNT_1,CLOCK_SEL_PLL) ){
return -1;
}
// enable I2C master mode
if( !writeRegister(USER_CTRL,I2C_MST_EN) ){
return -1;
}
// set the I2C bus speed to 400 kHz
if( !writeRegister(I2C_MST_CTRL,I2C_MST_CLK) ){
return -1;
}
// set AK8963 to Power Down
if( !writeAK8963Register(AK8963_CNTL1,AK8963_PWR_DOWN) ){
return -1;
}
// reset the MPU9250
writeRegister(PWR_MGMNT_1,PWR_RESET);
// wait for MPU-9250 to come back up
delay(1);
// reset the AK8963
writeAK8963Register(AK8963_CNTL2,AK8963_RESET);
// select clock source to gyro
if( !writeRegister(PWR_MGMNT_1,CLOCK_SEL_PLL) ){
return -1;
}
// check the WHO AM I byte, expected value is 0x71 (decimal 113)
if( whoAmI() != 113 ){
return -1;
}
// enable accelerometer and gyro
if( !writeRegister(PWR_MGMNT_2,SEN_ENABLE) ){
return -1;
}
/* setup the accel and gyro ranges */
switch(accelRange) {
case ACCEL_RANGE_2G:
// setting the accel range to 2G
if( !writeRegister(ACCEL_CONFIG,ACCEL_FS_SEL_2G) ){
return -1;
}
_accelScale = G * 2.0f/32768.0f; // setting the accel scale to 2G
break;
case ACCEL_RANGE_4G:
// setting the accel range to 4G
if( !writeRegister(ACCEL_CONFIG,ACCEL_FS_SEL_4G) ){
return -1;
}
_accelScale = G * 4.0f/32768.0f; // setting the accel scale to 4G
break;
case ACCEL_RANGE_8G:
// setting the accel range to 8G
if( !writeRegister(ACCEL_CONFIG,ACCEL_FS_SEL_8G) ){
return -1;
}
_accelScale = G * 8.0f/32768.0f; // setting the accel scale to 8G
break;
case ACCEL_RANGE_16G:
// setting the accel range to 16G
if( !writeRegister(ACCEL_CONFIG,ACCEL_FS_SEL_16G) ){
return -1;
}
_accelScale = G * 16.0f/32768.0f; // setting the accel scale to 16G
break;
}
switch(gyroRange) {
case GYRO_RANGE_250DPS:
// setting the gyro range to 250DPS
if( !writeRegister(GYRO_CONFIG,GYRO_FS_SEL_250DPS) ){
return -1;
}
_gyroScale = 250.0f/32768.0f * _d2r; // setting the gyro scale to 250DPS
break;
case GYRO_RANGE_500DPS:
// setting the gyro range to 500DPS
if( !writeRegister(GYRO_CONFIG,GYRO_FS_SEL_500DPS) ){
return -1;
}
_gyroScale = 500.0f/32768.0f * _d2r; // setting the gyro scale to 500DPS
break;
case GYRO_RANGE_1000DPS:
// setting the gyro range to 1000DPS
if( !writeRegister(GYRO_CONFIG,GYRO_FS_SEL_1000DPS) ){
return -1;
}
_gyroScale = 1000.0f/32768.0f * _d2r; // setting the gyro scale to 1000DPS
break;
case GYRO_RANGE_2000DPS:
// setting the gyro range to 2000DPS
if( !writeRegister(GYRO_CONFIG,GYRO_FS_SEL_2000DPS) ){
return -1;
}
_gyroScale = 2000.0f/32768.0f * _d2r; // setting the gyro scale to 2000DPS
break;
}
// enable I2C master mode
if( !writeRegister(USER_CTRL,I2C_MST_EN) ){
return -1;
}
// set the I2C bus speed to 400 kHz
if( !writeRegister(I2C_MST_CTRL,I2C_MST_CLK) ){
return -1;
}
// check AK8963 WHO AM I register, expected value is 0x48 (decimal 72)
if( whoAmIAK8963() != 72 ){
return -1;
}
/* get the magnetometer calibration */
// set AK8963 to Power Down
if( !writeAK8963Register(AK8963_CNTL1,AK8963_PWR_DOWN) ){
return -1;
}
delay(100); // long wait between AK8963 mode changes
// set AK8963 to FUSE ROM access
if( !writeAK8963Register(AK8963_CNTL1,AK8963_FUSE_ROM) ){
return -1;
}
delay(100); // long wait between AK8963 mode changes
// read the AK8963 ASA registers and compute magnetometer scale factors
readAK8963Registers(AK8963_ASA,sizeof(buff),&buff[0]);
_magScaleX = ((((float)buff[0]) - 128.0f)/(256.0f) + 1.0f) * 4912.0f / 32760.0f; // micro Tesla
_magScaleY = ((((float)buff[1]) - 128.0f)/(256.0f) + 1.0f) * 4912.0f / 32760.0f; // micro Tesla
_magScaleZ = ((((float)buff[2]) - 128.0f)/(256.0f) + 1.0f) * 4912.0f / 32760.0f; // micro Tesla
// set AK8963 to Power Down
if( !writeAK8963Register(AK8963_CNTL1,AK8963_PWR_DOWN) ){
return -1;
}
delay(100); // long wait between AK8963 mode changes
// set AK8963 to 16 bit resolution, 100 Hz update rate
if( !writeAK8963Register(AK8963_CNTL1,AK8963_CNT_MEAS2) ){
return -1;
}
delay(100); // long wait between AK8963 mode changes
// select clock source to gyro
if( !writeRegister(PWR_MGMNT_1,CLOCK_SEL_PLL) ){
return -1;
}
// instruct the MPU9250 to get 7 bytes of data from the AK8963 at the sample rate
readAK8963Registers(AK8963_HXL,sizeof(data),&data[0]);
// successful init, return 0
return 0;
}
void process(byte index)
{
// Read and echo all available data from the client.
String str = "";
// Read all available data, append to string
while (clients[index].available())
str = str + (char)clients[index].read();
// Trim off non-printable characters.
str.trim();
// Got something? If so, print out what was recevied, send client ID back to other side
if (str.length() > 0)
{
Serial.print("Received from client ");
Serial.print(index);
Serial.println(": received \"" + str + "\"");
clients[index].print("Client ");
clients[index].println(index);
}
}
view raw sketch_YUNmini_MPU9250_v3.ino hosted with ❤ by GitHub
RoboticBoat.uk