I need your help for a project.
I have actually 2 parallel sensors of the type: MS5803-05BA, which supports I2C and SPI connection. One sensor is on I2C bus and one sensor is on SPI. They are both sending to my Arduino Mega. The I2C works perfect with short cables.
Here the datasheet of the sensor:
https://www.amsys.de/downloads/data/MS5803-05BA-AMSYS-datasheet.pdf
For some informations who wants to do the same with I2C. Here you can find a good code. (You can find the other MS580X typs there too). For the communication between the sensor and the Arduino Mega you need an logic converter like this txs0108e, which can be bought with a break out board (you need pull up resistors on the sensor side!):
https://github.com/millerlp/MS5803_05
But to my problem: I have an sensor distance for about 3-5 meters and the I2C connections doesnt work. Yes I can try to fix the pullup resistors but it doesnt worked for me (I have tried some different lower resistors between 3-10kOhm). Therefore I want to switch to the SPI bus.
I have edit the code from https://github.com/millerlp/MS5803_05, https://github.com/vic320/Arduino-MS5803-14BA and https://arduino.stackexchange.com/questions/13720/teensy-spi-and-pressure-sensor.
The File is added. (You have to put the .h and .cpp files in the folder of the arduino code (.spi).
I have problems with the code from the SPI (ccp and header). There is no right communication. I have checked my cables twice. I couldnt find a problem and the connection with the txs0108e works for parallel I2C sensor. Both sensors are working on I2C.
Here is the main code (arduino .spi) for SPI and I2C parallel:
/_____ I N C L U D E S
#include <stdio.h>
#include <math.h>
#include <SPI.h>
#include <Wire.h>
#include "MS5803_05.h"
#include "MS5803_05_SPI.h"
const int miso_port = 50; //SDI
const int mosi_port = 51; //SDO
const int sck_port = 52; //SLCK
const int slaveSelectPin = 53; // CSB
MS_5803 sensor = MS_5803(512);
MS_5803_SPI sensor_spi = MS_5803_SPI(4096, slaveSelectPin);
void setup()
{
pinMode(miso_port, INPUT);
pinMode(mosi_port, OUTPUT);
pinMode(slaveSelectPin, OUTPUT);
pinMode(sck_port, OUTPUT);
Serial.begin(9600);
//SPI BUS
if (sensor_spi.initializeMS_5803_SPI()) {
Serial.println( "MS5803 SPI CRC check OK." );
}
else {
Serial.println( "MS5803 SPI CRC check FAILED!" );
}
//I2C BUS
delay(1000);
if (sensor.initializeMS_5803()) {
Serial.println( "MS5803 I2C CRC check OK." );
}
else {
Serial.println( "MS5803 I2C CRC check FAILED!" );
}
}
void loop()
{
Serial.println("SPI Sensor first pressure [mbar], than temperature[°C]:");
sensor_spi.readSensor();
// Show pressure
Serial.print("Pressure = ");
Serial.print(sensor_spi.pressure());
Serial.println(" mbar");
// Show temperature
Serial.print("Temperature = ");
Serial.print(sensor_spi.temperature());
Serial.println("C");
////********************************************************
Serial.println("");
Serial.println("I2C Sensor first pressure [mbar], than temperature[°C]:");
sensor.readSensor();
// Show pressure
Serial.print("Pressure = ");
Serial.print(sensor.pressure());
Serial.println(" mbar");
// Show temperature
Serial.print("Temperature = ");
Serial.print(sensor.temperature());
Serial.println("C");
delay(2000);
}
}
The first connection with SPI is here (.cpp):
#include "MS5803_05_SPI.h"
#include <SPI.h>
#define CMD_RESET 0x1E // ADC reset command
#define CMD_ADC_READ 0x00 // ADC read command
#define CMD_ADC_CONV 0x40 // ADC conversion command
#define CMD_ADC_D1 0x00 // ADC D1 conversion
#define CMD_ADC_D2 0x10 // ADC D2 conversion
#define CMD_ADC_256 0x00 // ADC resolution=256
#define CMD_ADC_512 0x02 // ADC resolution=512
#define CMD_ADC_1024 0x04 // ADC resolution=1024
#define CMD_ADC_2048 0x06 // ADC resolution=2048
#define CMD_ADC_4096 0x08 // ADC resolution=4096
#define CMD_PROM_RD 0xA0 // Prom read command
#define spi_write SPI_MODE3
#define spi_write2 SPI_MODE1
// Create array to hold the 8 sensor calibration coefficients
static unsigned int sensorCoeffs[8]; // unsigned 16-bit integer (0-65535)
// D1 and D2 need to be unsigned 32-bit integers (long 0-4294967295)
static uint32_t D1 = 0; // Store uncompensated pressure value
static uint32_t D2 = 0; // Store uncompensated temperature value
// These three variables are used for the conversion steps
// They should be signed 32-bit integer initially
// i.e. signed long from -2147483648 to 2147483647
static int32_t dT = 0;
static int32_t TEMP = 0;
// These values need to be signed 64 bit integers
// (long long = int64_t)
static int64_t Offset = 0;
static int64_t Sensitivity = 0;
static int64_t T2 = 0;
static int64_t OFF2 = 0;
static int64_t Sens2 = 0;
// Some constants used in calculations below
const uint64_t POW_2_33 = 8589934592ULL; // 2^33 = 8589934592
SPISettings settings_write(500000, MSBFIRST, spi_write);
SPISettings settings_write2(500000, MSBFIRST, spi_write2);
//-------------------------------------------------
// Constructor
MS_5803_SPI::MS_5803_SPI( uint16_t Resolution, uint16_t cs) {
// The argument is the oversampling resolution, which may have values
// of 256, 512, 1024, 2048, or 4096.
_Resolution = Resolution;
//Chip Select
_cs=cs;
}
boolean MS_5803_SPI::initializeMS_5803_SPI(boolean Verbose) {
digitalWrite( _cs, HIGH );
SPI.begin();
// Reset the sensor during startup
resetSensor();
if (Verbose)
{
// Display the oversampling resolution or an error message
if (_Resolution == 256 | _Resolution == 512 | _Resolution == 1024 | _Resolution == 2048 | _Resolution == 4096){
Serial.print("Oversampling setting: ");
Serial.println(_Resolution);
} else {
Serial.println("*******************************************");
Serial.println("Error: specify a valid oversampling value");
Serial.println("Choices are 256, 512, 1024, 2048, or 4096");
Serial.println("*******************************************");
}
}
// Read sensor coefficients
for (int i = 0; i < 8; i++ )
{
SPI.beginTransaction(settings_write2);
digitalWrite(_cs, LOW); //csb_lo(); // pull CSB low
unsigned int ret;
unsigned int rC = 0;
SPI.transfer(CMD_PROM_RD + i * 2); // send PROM READ command
/*
ret = SPI.transfer(0x00); // send 0 to read the MSB
rC = 256 * ret;
ret = SPI.transfer(0x00); // send 0 to read the LSB
rC = rC + ret;
*/
// send a value of 0 to read the first byte returned:
rC = SPI.transfer( 0x00 );
rC = rC << 8;
rC |= SPI.transfer( 0x00 ); // and the second byte
sensorCoeffs[i] = (rC);
digitalWrite( _cs, HIGH );
delay(3);
}
//SPI.endTransaction(); // interrupt can now be accepted
// The last 4 bits of the 7th coefficient form a CRC error checking code.
unsigned char p_crc = sensorCoeffs[7];
// Use a function to calculate the CRC value
unsigned char n_crc = MS_5803_CRC(sensorCoeffs);
if (Verbose) {
for (int i = 0; i < 8; i++ )
{
// Print out coefficients
Serial.print("C");
Serial.print(i);
Serial.print(" = ");
Serial.println(sensorCoeffs[i]);
delay(10);
}
Serial.print("p_crc: ");
Serial.println(p_crc);
Serial.print("n_crc: ");
Serial.println(n_crc);
}
// If the CRC value doesn't match the sensor's CRC value, then the
// connection can't be trusted. Check your wiring.
if (p_crc != n_crc) {
return false;
}
// Otherwise, return true when everything checks out OK.
return true;
}
// Sends a power on reset command to the sensor.
void MS_5803_SPI::resetSensor() {
SPI.beginTransaction(settings_write);
digitalWrite(_cs, LOW); //csb_lo(); // pull CSB low to start the command
SPI.transfer(CMD_RESET); // send reset sequence
delay(3); // wait for the reset sequence timing delay(3)
digitalWrite(_cs, HIGH); //csb_hi(); // pull CSB high to finish the command
SPI.endTransaction(); // interrupt can now be accepted
}
The Code can be downloaded at: https://forum.arduino.cc/index.php?topic=670661.0
There you can find the schematic and output picture too.
Thanks a lot :).
Related
I have an Arduino Nano 33 iot that outputs data via Serial at 38400 baud, connected via USB. Setup starts with Serial.Begin. The Raspberry Pi 4, running Raspian buster is set up to receive the data. It can see the correct port, /dev/ttyACM0, but nothing comes in.
I even installed the correct Arduino IDE and SAMD board package on the Raspberry Pi. It still does not find it until after the IDE uploads the replacement sketch and the CPU is reset. The IDE can grab the serial number and board type though. I can then exit out of the IDE and the Arduino is still pumping out serial to the Raspberry Pi.
The only other way to make it work is by pressing the reset button on the Arduino every time a reboot is done on the Raspberry Pi. Serial was tested on the Pi using screen.
Neither of these options are convenient. What am I missing?
/*
Connects via I2C to a CMPS14, outputs NMEA0183 HDM sentences via Serial (38400 baud)
By James Henderson, 2014, adapted to output NMEA sentences by Ian Van Schaick
*/
#include <Keyboard.h>
#include <Wire.h>
#define CMPS14_ADDRESS 0x60 // Address of CMPS14 shifted right one bit for arduino wire library
#define ANGLE_8 1 // Register to read 8bit angle from
unsigned char high_byte, low_byte, angle8;
signed char pitch, roll;
float angle16;
int fine;
float bearingH; // Holds whole degrees of bearing
float bearingL; // Holds decimal digits of bearing
int bearing;
char nbsp;
char mystring[25];
char mystring2[25];
char mystring3[25];
int software;
int cal;
unsigned int _last_status;
uint8_t checksum(char *s)
{
uint8_t c = 0;
while (*s)
c ^= *s++;
return c;
}
void CMPS14_eraseProfil()
{
Wire.beginTransmission(CMPS14_ADDRESS);
Wire.write(0x00);
Wire.write(0xE0);
_last_status = Wire.endTransmission();
delay(20); // 20ms delay after each of the three bytes send
Wire.beginTransmission(CMPS14_ADDRESS);
Wire.write(0x00);
Wire.write(0xE5);
_last_status = Wire.endTransmission();
delay(20); // 20ms delay after each of the three bytes send
Wire.beginTransmission(CMPS14_ADDRESS);
Wire.write(0x00);
Wire.write(0xE2);
_last_status = Wire.endTransmission();
delay(20); // 20ms delay after each of the three bytes send
}
//Correct heading for known deviation
int DeviationCorrect(int Head)
{
return 0;
}
void setup() {
Serial.begin(38400); // Start serial port
Wire.begin();
nbsp = 32;
// CMPS14_eraseProfil();
}
void loop() {
Wire.beginTransmission(CMPS14_ADDRESS); //starts communication with CMPS14
Wire.write(ANGLE_8); //Sends the register we wish to start reading from
Wire.endTransmission();
// Request 5 bytes from the CMPS14
// this will give us the 8 bit bearing,
// both bytes of the 16 bit bearing, pitch and roll
Wire.requestFrom(CMPS14_ADDRESS, 26);
while (Wire.available() < 26); // Wait for all bytes to come back
// software = Wire.read();
// Serial.print("Version: ");
// Serial.println(software);
angle8 = Wire.read(); // Read back the 5 bytes
high_byte = Wire.read();
low_byte = Wire.read();
pitch = Wire.read();
roll = Wire.read();
// int i = 6;
// while (i <= 25) {
// Wire.read();
// i++;
// }
//
// cal = Wire.read();
// Serial.print("Cal: ");
// Serial.println(cal);
bearing = ((high_byte << 8) + low_byte) / 10;
fine = ((high_byte << 8) + low_byte) % 10;
byte data[128] = "$HCHDM,";
data[8] = bearing;
// int deviation = 0;
//DeviationCorrect(bearing);
// bearing = bearing;
//+ deviation;
//Print out NMEA 0183 string HDM
snprintf(mystring, sizeof(mystring), "$HCHDM,%d.%d,M", bearing , fine);
uint8_t crc = checksum(mystring + 1);
Serial.print(mystring);
Serial.print("*");
if (crc < 16) Serial.print("0");
Serial.println(crc, HEX);
//Print out NMEA 0183 string XDR for Pitch
snprintf(mystring2, sizeof(mystring2), "$HCXDR,A,%d,D,PITCH", pitch);
uint8_t crc2 = checksum(mystring2 + 1);
Serial.print(mystring2);
Serial.print("*");
if(crc2 < 16) Serial.print("0");
Serial.println(crc2, HEX);
//Print out NMEA 0183 string XDR for Roll/Heel
snprintf(mystring3, sizeof(mystring3), "$HCXDR,A,%d,D,ROLL", roll);
uint8_t crc3 = checksum(mystring3 + 1);
Serial.print(mystring3);
Serial.print("*");
if(crc3 < 16) Serial.print("0");
Serial.println(crc3, HEX);
delay(100);
}
Please excuse me for the lengthiness of the problem but its beyond my knowhow, I dont know where else to go.
I am trying to send a PPM signal to a FlySkyi10 radio over the trainer cable. When using the PPM code below I am able to send the correct PPM signals to my radio.
//this programm will put out a PPM signal
//////////////////////CONFIGURATION///////////////////////////////
#define chanel_number 8 //set the number of chanels
#define default_servo_value 1500 //set the default servo value
#define PPM_FrLen 22500 //set the PPM frame length in microseconds (1ms = 1000µs)
#define PPM_PulseLen 300 //set the pulse length
#define onState 1 //set polarity of the pulses: 1 is positive, 0 is negative
#define sigPin 10 //set PPM signal output pin on the arduino
//////////////////////////////////////////////////////////////////
/*this array holds the servo values for the ppm signal
change theese values in your code (usually servo values move between 1000 and 2000)*/
int ppm[chanel_number];
void setup(){
//initiallize default ppm values
for(int i=0; i<chanel_number; i++){
ppm[i]= default_servo_value;
}
pinMode(sigPin, OUTPUT);
digitalWrite(sigPin, !onState); //set the PPM signal pin to the default state (off)
cli();
TCCR1A = 0; // set entire TCCR1 register to 0
TCCR1B = 0;
OCR1A = 100; // compare match register, change this
TCCR1B |= (1 << WGM12); // turn on CTC mode
TCCR1B |= (1 << CS11); // 8 prescaler: 0,5 microseconds at 16mhz
TIMSK1 |= (1 << OCIE1A); // enable timer compare interrupt
sei();
}
void loop(){
//put main code here
static int val = 1;
ppm[0] = ppm[0] + val;
if(ppm[0] >= 2000){ val = -1; }
if(ppm[0] <= 1000){ val = 1; }
delay(10);
}
ISR(TIMER1_COMPA_vect){ //leave this alone
static boolean state = true;
TCNT1 = 0;
if(state) { //start pulse
digitalWrite(sigPin, onState);
OCR1A = PPM_PulseLen * 2;
state = false;
}
else{ //end pulse and calculate when to start the next pulse
static byte cur_chan_numb;
static unsigned int calc_rest;
digitalWrite(sigPin, !onState);
state = true;
if(cur_chan_numb >= chanel_number){
cur_chan_numb = 0;
calc_rest = calc_rest + PPM_PulseLen;//
OCR1A = (PPM_FrLen - calc_rest) * 2;
calc_rest = 0;
}
else{
OCR1A = (ppm[cur_chan_numb] - PPM_PulseLen) * 2;
calc_rest = calc_rest + ppm[cur_chan_numb];
cur_chan_numb++;
}
}
}
But, the problem comes in when I read data from a sensor over i2c and some other functions. As soon as I use the i2c code, the PPM signal becomes noisy, and what should be a 1500ms signal, has noise of +/-20ms.
I dont want to make this post too bulky, I have used the i2c library for the LSM9DS0 to read sensor data. I have not used interrupts anywhere and I will have to have a closer look at the library. unfortunately i dont understand much of it. The simplified code for reading from the seensors is below. the website for the libraries is here: https://learn.adafruit.com/adafruit-lsm9ds0-accelerometer-gyro-magnetometer-9-dof-breakouts/arduino-code
#include <Wire.h>
#include <SPI.h>
#include <Adafruit_LSM9DS0.h>
#include <Adafruit_Sensor.h> // not used in this demo but required!
// i2c
Adafruit_LSM9DS0 lsm = Adafruit_LSM9DS0();
// You can also use software SPI
//Adafruit_LSM9DS0 lsm = Adafruit_LSM9DS0(13, 12, 11, 10, 9);
// Or hardware SPI! In this case, only CS pins are passed in
//Adafruit_LSM9DS0 lsm = Adafruit_LSM9DS0(10, 9);
void setupSensor()
{
// 1.) Set the accelerometer range
lsm.setupAccel(lsm.LSM9DS0_ACCELRANGE_2G);
//lsm.setupAccel(lsm.LSM9DS0_ACCELRANGE_4G);
//lsm.setupAccel(lsm.LSM9DS0_ACCELRANGE_6G);
//lsm.setupAccel(lsm.LSM9DS0_ACCELRANGE_8G);
//lsm.setupAccel(lsm.LSM9DS0_ACCELRANGE_16G);
// 2.) Set the magnetometer sensitivity
lsm.setupMag(lsm.LSM9DS0_MAGGAIN_2GAUSS);
//lsm.setupMag(lsm.LSM9DS0_MAGGAIN_4GAUSS);
//lsm.setupMag(lsm.LSM9DS0_MAGGAIN_8GAUSS);
//lsm.setupMag(lsm.LSM9DS0_MAGGAIN_12GAUSS);
// 3.) Setup the gyroscope
lsm.setupGyro(lsm.LSM9DS0_GYROSCALE_245DPS);
//lsm.setupGyro(lsm.LSM9DS0_GYROSCALE_500DPS);
//lsm.setupGyro(lsm.LSM9DS0_GYROSCALE_2000DPS);
}
void setup()
{
#ifndef ESP8266
while (!Serial); // will pause Zero, Leonardo, etc until serial console opens
#endif
Serial.begin(9600);
Serial.println("LSM raw read demo");
// Try to initialise and warn if we couldn't detect the chip
if (!lsm.begin())
{
Serial.println("Oops ... unable to initialize the LSM9DS0. Check your wiring!");
while (1);
}
Serial.println("Found LSM9DS0 9DOF");
Serial.println("");
Serial.println("");
}
void loop(void)
{
/* Get a new sensor event */
sensors_event_t accel, mag, gyro, temp;
lsm.getEvent(&accel, &mag, &gyro, &temp);
// print out accelleration data
Serial.print("Accel X: "); Serial.print(accel.acceleration.x); Serial.print(" ");
Serial.print(" \tY: "); Serial.print(accel.acceleration.y); Serial.print(" ");
Serial.print(" \tZ: "); Serial.print(accel.acceleration.z); Serial.println(" \tm/s^2");
// print out magnetometer data
Serial.print("Magn. X: "); Serial.print(mag.magnetic.x); Serial.print(" ");
Serial.print(" \tY: "); Serial.print(mag.magnetic.y); Serial.print(" ");
Serial.print(" \tZ: "); Serial.print(mag.magnetic.z); Serial.println(" \tgauss");
// print out gyroscopic data
Serial.print("Gyro X: "); Serial.print(gyro.gyro.x); Serial.print(" ");
Serial.print(" \tY: "); Serial.print(gyro.gyro.y); Serial.print(" ");
Serial.print(" \tZ: "); Serial.print(gyro.gyro.z); Serial.println(" \tdps");
// print out temperature data
Serial.print("Temp: "); Serial.print(temp.temperature); Serial.println(" *C");
Serial.println("**********************\n");
delay(250);
}
Does anyone have an suggestions?
I changed the method of reading data from the sensors to SPI, which solved the problem. All noise in the ppm signal has been eliminated.
I am currently trying to send a float value across two Arduinos via SPI. Currently I am working to send a static value of 2.25 across and then read it via the Serial.println() command. I would then want to pass a float value from a linear displacement sensor. My end goal is to be able to have the master ask for information, the slave gathers the appropriate data and packages it and then master receives said data and does what it needs with it.
Currently I am getting an error "call of overloaded 'println(byte [7])' is ambiguous" and I am not to sure why I am getting this error. I am currently a mechanical engineering student and I am crash-coursing myself through C/C++. I am not entirely positive about what I am doing. I know that a float is 4 bytes and I am attempting to create a buffer of 7 bytes to store the float and the '\n' char with room to spare. My current code is below.
Master:
#include <SPI.h>
void setup() {
pinMode(SS,OUTPUT);
digitalWrite(SS,HIGH);
SPI.begin();
SPI.setClockDivider(SPI_CLOCK_DIV4);
}
void loop() {
digitalWrite(SS,LOW);
float a = 2.25;
SPI.transfer(a);
SPI.transfer('\n');
digitalWrite(SS,HIGH);
}
My slave code is as follows:
#include <SPI.h>
byte buf[7];
volatile byte pos = 0;
volatile boolean process_it = false;
void setup() {
Serial.begin(9600);
pinMode(MISO,OUTPUT);
digitalWrite(MISO,LOW);
SPCR |= _BV(SPE); // SPI Enable, sets this Arduino to Slave
SPCR |= _BV(SPIE); // SPI interrupt enabled
}
ISR(SPI_STC_vect) {
// Interrupt Service Routine(SPI_(SPI Transfer Complete)_vector)
byte c = SPDR;
// SPDR = SPI Data Register, so you are saving the byte of information in that register to byte c
if (pos < sizeof buf) {
buf[pos++] = c;
if (c == '\n') {
process_it = true;
}
}
}
void loop() {
if (process_it = true) {
Serial.println(buf);
pos = 0;
process_it = false;
}
}
I figured out what I needed to do and I wanted to post my finished code. I also added an ability to transfer more than one float value.
Master:
#include <SPI.h>
float a = 3.14;
float b = 2.25;
uint8_t storage [12];
float buff[2] = {a, b};
void setup()
{
digitalWrite(SS, HIGH);
SPI.begin();
Serial.begin(9600);
SPI.setClockDivider(SPI_CLOCK_DIV8);
}
void loop()
{
digitalWrite(SS, LOW);
memcpy(storage, &buff, 8);
Serial.print("storage[0] = "); Serial.println(storage[0]); // the
following serial prints were to check i was getting the right decimal
numbers for the floats.
Serial.print("storage[1] = "); Serial.println(storage[1]);
Serial.print("storage[2] = "); Serial.println(storage[2]);
Serial.print("storage[3] = "); Serial.println(storage[3]);
Serial.print("storage[4] = "); Serial.println(storage[4]);
Serial.print("storage[5] = "); Serial.println(storage[5]);
Serial.print("storage[6] = "); Serial.println(storage[6]);
Serial.print("storage[7] = "); Serial.println(storage[7]);
SPI.transfer(storage, sizeof storage ); //SPI library allows a user to
transfer a whole array of bytes and you need to include the size of the
array.
digitalWrite(SS, HIGH);
delay(1000);
}
For my Slave code:
#include <SPI.h>
byte storage [8];
volatile byte pos;
volatile boolean process;
float buff[2];
void setup()
{
pinMode(MISO,OUTPUT);
SPCR |= _BV(SPE);
SPCR |= _BV(SPIE);
pos = 0;
process = false;
Serial.begin(9600);
}
ISR(SPI_STC_vect)
{
byte gathered = SPDR;
if( pos < sizeof storage)
{
storage[pos++] = gathered;
}
else
process = true;
}
void loop()
{
if( process )
{
Serial.print("storage[0] = "); Serial.println(storage[0]);
Serial.print("storage[1] = "); Serial.println(storage[1]);
Serial.print("storage[2] = "); Serial.println(storage[2]);
Serial.print("storage[3] = "); Serial.println(storage[3]);
Serial.print("storage[4] = "); Serial.println(storage[4]);
Serial.print("storage[5] = "); Serial.println(storage[5]);
Serial.print("storage[6] = "); Serial.println(storage[6]);
Serial.print("storage[7] = "); Serial.println(storage[7]);
memcpy(buff,&storage,8);
Serial.print("This is buff[0]");Serial.println(buff[0]);
Serial.print("This is buff[1]");Serial.println(buff[1]);
storage[pos] = 0;
pos = 0;
process = false;
}
}
The immediate problem is that Serial.print doesn't know what to do with a byte array. Either declare it as a char array or cast it in the print statement:
char buf[7];
OR
Serial.print((char*) buf);
Either way, though, it's not going to show up as a float like you want.
An easier way to do all this is to use memcpy or a union to go back and forth between float and bytes. On the master end:
uint8_t buf[4];
memcpy(buf, &a, 4);
Then use SPI to send 4 bytes. Reverse it on the peripheral end.
Note that sending '\n' as the termination byte is a bad idea because it can lead to weird behavior, since one of the bytes in the float could easily be 0x0a, the hexadecimal equivalent of '\n'.
I am using a ITG3200(Sparkfun breakout board) for my project. I was trying to boost the sample rate of ITG3200 to over 2K HZ. I have already soldered two 2.2K pull-up resistors on the sensor and close the clockin pads. I encountered a few problems here. It was connected to a Arduino Uno.
The highest sample rate I can achieve was around 500 Hz. I have changed the clock to 400K. However, without doing that, I should still get something over 1000 Hz, right? I attached my code below.
Any comments or suggestions would be greatly appriecated!
#include <SPI.h>
#include <Wire.h>
// Pin definitions - Shift registers:
int enPin = 13; // Shift registers' Output Enable pin
int latchPin = 12; // Shift registers' rclk pin
int clkPin = 11; // Shift registers' srclk pin
int clrPin = 10; // shift registers' srclr pin
int datPin = 8; // shift registers' SER pin
int show = 0;
int lastMax = 0;
//This is a list of registers in the ITG-3200. Registers are parameters that determine how the sensor will behave, or they can hold data that represent the
//sensors current status.
//To learn more about the registers on the ITG-3200, download and read the datasheet.
char WHO_AM_I = 0x00;
char SMPLRT_DIV= 0x15;//0x15
char DLPF_FS = 0x16;
char GYRO_XOUT_H = 0x1D;
char GYRO_XOUT_L = 0x1E;
char GYRO_YOUT_H = 0x1F;
char GYRO_YOUT_L = 0x20;
char GYRO_ZOUT_H = 0x21;
char GYRO_ZOUT_L = 0x22;
//This is a list of settings that can be loaded into the registers.
//DLPF, Full Scale Register Bits
//FS_SEL must be set to 3 for proper operation
//Set DLPF_CFG to 3 for 1kHz Fint and 42 Hz Low Pass Filter
char DLPF_CFG_0 = 0;//1
char DLPF_CFG_1 = 0;//2
char DLPF_CFG_2 = 0;//4
char DLPF_FS_SEL_0 = 8;
char DLPF_FS_SEL_1 = 16;
char itgAddress = 0x69;
// Some of the math we're doing in this example requires the number of bargraph boards
// you have connected together (normally this is one, but you can have a maximum of 8).
void setup()
// Runs once upon reboot
{
// Setup shift register pins
pinMode(enPin, OUTPUT); // Enable, active low, this'll always be LOW
digitalWrite(enPin, LOW); // Turn all outputs on
pinMode(latchPin, OUTPUT); // this must be set before calling shiftOut16()
digitalWrite(latchPin, LOW); // start latch low
pinMode(clkPin, OUTPUT); // we'll control this in shiftOut16()
digitalWrite(clkPin, LOW); // start sck low
pinMode(clrPin, OUTPUT); // master clear, this'll always be HIGH
digitalWrite(clrPin, HIGH); // disable master clear
pinMode(datPin, OUTPUT); // we'll control this in shiftOut16()
digitalWrite(datPin, LOW); // start ser low
// To begin, we'll turn all LEDs on the circular bar-graph OFF
digitalWrite(latchPin, LOW); // first send latch low
shiftOut16(0x0000);
digitalWrite(latchPin, HIGH); // send latch high to indicate data is done sending
Serial.begin(230400);
//Initialize the I2C communication. This will set the Arduino up as the 'Master' device.
Wire.begin();
//Read the WHO_AM_I register and print the result
char id=0;
id = itgRead(itgAddress, 0x00);
Serial.print("ID: ");
Serial.println(id, HEX);
//Configure the gyroscope
//Set the gyroscope scale for the outputs to +/-2000 degrees per second
itgWrite(itgAddress, DLPF_FS, (DLPF_FS_SEL_0|DLPF_FS_SEL_1|DLPF_CFG_0));
//Set the sample rate to 100 hz
itgWrite(itgAddress, SMPLRT_DIV, 0);
}
void loop()
// Runs continuously after setup() ends
{
static int zero = 0;
// Create variables to hold the output rates.
int xRate, yRate, zRate;
float range = 3000.0;
int divisor;
divisor = range / 8;
//Read the x,y and z output rates from the gyroscope.
xRate = int(float(readX()) / divisor - 0.5) * -1;
yRate = int(float(readY()) / divisor - 0.5) * -1;
zRate = int(float(readZ()) / divisor - 0.5);
//Print the output rates to the terminal, seperated by a TAB character.
Serial.print(xRate);
Serial.print('\t');
Serial.print(yRate);
Serial.print('\t');
Serial.println(zRate);
Serial.print('\t');
// Serial.println(zero);
// fillTo(zRate);
//Wait 10ms before reading the values again. (Remember, the output rate was set to 100hz and 1reading per 10ms = 100hz.)
// delay(10);
}
// This function will write a value to a register on the itg-3200.
// Parameters:
// char address: The I2C address of the sensor. For the ITG-3200 breakout the address is 0x69.
// char registerAddress: The address of the register on the sensor that should be written to.
// char data: The value to be written to the specified register.
void itgWrite(char address, char registerAddress, char data)
{
//Initiate a communication sequence with the desired i2c device
Wire.beginTransmission(address);
//Tell the I2C address which register we are writing to
Wire.write(registerAddress);
//Send the value to write to the specified register
Wire.write(data);
//End the communication sequence
Wire.endTransmission();
}
//This function will read the data from a specified register on the ITG-3200 and return the value.
//Parameters:
// char address: The I2C address of the sensor. For the ITG-3200 breakout the address is 0x69.
// char registerAddress: The address of the register on the sensor that should be read
//Return:
// unsigned char: The value currently residing in the specified register
unsigned char itgRead(char address, char registerAddress)
{
//This variable will hold the contents read from the i2c device.
unsigned char data=0;
//Send the register address to be read.
Wire.beginTransmission(address);
//Send the Register Address
Wire.write(registerAddress);
//End the communication sequence.
Wire.endTransmission();
//Ask the I2C device for data
Wire.beginTransmission(address);
Wire.requestFrom(address, 1);
//Wait for a response from the I2C device
if(Wire.available()){
//Save the data sent from the I2C device
data = Wire.read();
}
//End the communication sequence.
Wire.endTransmission();
//Return the data read during the operation
return data;
}
//This function is used to read the X-Axis rate of the gyroscope. The function returns the ADC value from the Gyroscope
//NOTE: This value is NOT in degrees per second.
//Usage: int xRate = readX();
int readX(void)
{
int data=0;
data = itgRead(itgAddress, GYRO_XOUT_H)<<8;
data |= itgRead(itgAddress, GYRO_XOUT_L);
return data;
}
//This function is used to read the Y-Axis rate of the gyroscope. The function returns the ADC value from the Gyroscope
//NOTE: This value is NOT in degrees per second.
//Usage: int yRate = readY();
int readY(void)
{
int data=0;
data = itgRead(itgAddress, GYRO_YOUT_H)<<8;
data |= itgRead(itgAddress, GYRO_YOUT_L);
return data;
}
//This function is used to read the Z-Axis rate of the gyroscope. The function returns the ADC value from the Gyroscope
//NOTE: This value is NOT in degrees per second.
//Usage: int zRate = readZ();
int readZ(void)
{
int data=0;
data = itgRead(itgAddress, GYRO_ZOUT_H)<<8;
data |= itgRead(itgAddress, GYRO_ZOUT_L);
return data;
}
void fillTo(int place) {
int ledOutput = 0;
if(place > 8)
place = 8;
if(place < -8)
place = -8;
if(place >= 0) {
for (int i = place; i >= 0; i--)
ledOutput |= 1 << i;
} else {
ledOutput = 32768;
for (int i = place; i <= 0; i++)
ledOutput |= (ledOutput >> 1);
}
// Serial.println(ledOutput);
digitalWrite(latchPin, LOW); // first send latch low
shiftOut16(ledOutput); // send the ledOutput value to shiftOut16
digitalWrite(latchPin, HIGH); // send latch high to indicate data is done sending
}
void shiftOut16(uint16_t data)
{
byte datamsb;
byte datalsb;
// Isolate the MSB and LSB
datamsb = (data & 0xFF00) >> 8; // mask out the MSB and shift it right 8 bits
datalsb = data & 0xFF; // Mask out the LSB
// First shift out the MSB, MSB first.
shiftOut(datPin, clkPin, MSBFIRST, datamsb);
// Then shift out the LSB
shiftOut(datPin, clkPin, MSBFIRST, datalsb);
}
500Hz means 2ms for each iteration of your loop() function. Your loop function is reading from Wire and writing to the Serial port, which may take more time than 2ms, depending on what you're sending and what your baud rate is.
Judging from your baud rate (230400), it may take roughly 0.5ms to send each measurement (estimated at 12 characters each) if there is no flow control from the other side. Try writing to serial less frequently to see if your performance goes up.
I tested the serial writes, the I2C port and the clock speed. Found the major issues were the redundant communication to i2c. For instance, the 6 bits data can be read in one round of i2c communication. I refered the code below:
https://raw.githubusercontent.com/ControlEverythingCommunity/ITG3200/master/Arduino/ITG-3200.ino
In addition, using Teensy is also helpful.
The speed of the output was checked by using the oscilloscope with the I2C debug function.
I'm multiplexing 3 IMU-6050 using a MUX4051. This is the original code:
#include "Wire.h"
const int MPU=0x68; // I2C address of the MPU-6050
int16_t AcX,AcY,AcZ,Tmp,GyX,GyY,GyZ;
int Acc_ctrl_1 = 9;
int Acc_ctrl_2 = 10;
int Acc_ctrl_3 = 11;
int chip_enable1 = 5;
void setup() {
Wire.begin(); // wake up I2C bus
// set I/O pins to outputs
Wire.beginTransmission(MPU);
Wire.write(0x6B); // PWR_MGMT_1 register
Wire.write(0); // set to zero (wakes up the MPU-6050)
Wire.endTransmission(true);
Serial.begin(115200);
pinMode(Acc_ctrl_1, OUTPUT); //S0
pinMode(Acc_ctrl_2, OUTPUT); //S1
pinMode(Acc_ctrl_3, OUTPUT); //S2 address lines
pinMode(chip_enable1, OUTPUT);
//S0=1, S1=2 and S2=4 so Y0= S0=0,S1=0,S2=0, Y4=S0=0,S1=0,S2=1
}
void loop() {
//Enable the MUX Chip 1 - Active Low
digitalWrite(chip_enable1, LOW);
// control signal for First Accelerometer
Serial.println("IMU 1");
digitalWrite(Acc_ctrl_1, LOW);
digitalWrite(Acc_ctrl_2, LOW);
digitalWrite(Acc_ctrl_3, LOW);
readAccele();
delay(500);
// control signal for SECOND Accelerometer
Serial.println("IMU 2");
digitalWrite(Acc_ctrl_1, HIGH);
digitalWrite(Acc_ctrl_2, LOW);
digitalWrite(Acc_ctrl_3, LOW);
readAccele();
delay(500);
// control signal for THIRD Accelerometer
Serial.println("IMU 3");
digitalWrite(Acc_ctrl_1, LOW);
digitalWrite(Acc_ctrl_2, HIGH);
digitalWrite(Acc_ctrl_3, LOW);
readAccele();
delay(500);
}
void readAccele()
{
Wire.beginTransmission(MPU);// I2C address code thanks to John Boxall
Wire.write(0x3B); // starting with register 0x3B (ACCEL_XOUT_H)
Wire.endTransmission(false);
Wire.requestFrom(MPU,14,true); // request a total of 14 registers
AcX=Wire.read()<<8|Wire.read(); // 0x3B (ACCEL_XOUT_H) & 0x3C (ACCEL_XOUT_L)
AcY=Wire.read()<<8|Wire.read(); // 0x3D (ACCEL_YOUT_H) & 0x3E (ACCEL_YOUT_L)
AcZ=Wire.read()<<8|Wire.read(); // 0x3F (ACCEL_ZOUT_H) & 0x40 (ACCEL_ZOUT_L)
Tmp=Wire.read()<<8|Wire.read(); // 0x41 (TEMP_OUT_H) & 0x42 (TEMP_OUT_L)
GyX=Wire.read()<<8|Wire.read(); // 0x43 (GYRO_XOUT_H) & 0x44 (GYRO_XOUT_L)
GyY=Wire.read()<<8|Wire.read(); // 0x45 (GYRO_YOUT_H) & 0x46 (GYRO_YOUT_L)
GyZ=Wire.read()<<8|Wire.read(); // 0x47 (GYRO_ZOUT_H) & 0x48 (GYRO_ZOUT_L)
Serial.print("AcX = "); Serial.print(AcX);
Serial.print(" | AcY = "); Serial.print(AcY);
Serial.print(" | AcZ = "); Serial.print(AcZ);
Serial.print(" | Tmp = "); Serial.print(Tmp/340.00+36.53); //equation for temperature in degrees C from datasheet
Serial.print(" | GyX = "); Serial.print(GyX);
Serial.print(" | GyY = "); Serial.print(GyY);
Serial.print(" | GyZ = "); Serial.println(GyZ);
delay(5);
}
I implemented it into the Jeff Rowberg example code:
// I2C device class (I2Cdev) demonstration Arduino sketch for MPU6050 class using DMP (MotionApps v2.0)
// 6/21/2012 by Jeff Rowberg <jeff#rowberg.net>
uint8_t devStatus; // return status after each device operation (0 = success, !0 = error)
uint8_t mpuIntStatus; // holds actual interrupt status byte from MPU
bool dmpReady = false; // set true if DMP init was successful
uint16_t packetSize; // expected DMP packet size (default is 42 bytes)
uint16_t fifoCount; // count of all bytes currently in FIFO
uint8_t fifoBuffer[64];
// I2Cdev and MPU6050 must be installed as libraries, or else the .cpp/.h files
// for both classes must be in the include path of your project
#include "I2Cdev.h"
#include "MPU6050_6Axis_MotionApps20.h"
//#include "MPU6050.h" // not necessary if using MotionApps include file
// Arduino Wire library is required if I2Cdev I2CDEV_ARDUINO_WIRE implementation
// is used in I2Cdev.h
#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
#include "Wire.h"
#endif
// class default I2C address is 0x68
// specific I2C addresses may be passed as a parameter here
// AD0 low = 0x68 (default for SparkFun breakout and InvenSense evaluation board)
MPU6050 mpu;
// uncomment "OUTPUT_READABLE_QUATERNION" if you want to see the actual
// quaternion components in a [w, x, y, z] format (not best for parsing
// on a remote host such as Processing or something though)
//#define OUTPUT_READABLE_QUATERNION
// uncomment "OUTPUT_READABLE_EULER" if you want to see Euler angles
// (in degrees) calculated from the quaternions coming from the FIFO.
// Note that Euler angles suffer from gimbal lock (for more info, see
// http://en.wikipedia.org/wiki/Gimbal_lock)
#define OUTPUT_READABLE_EULER
// uncomment "OUTPUT_READABLE_WORLDACCEL" if you want to see acceleration
// components with gravity removed and adjusted for the world frame of
// reference (yaw is relative to initial orientation, since no magnetometer
// is present in this case). Could be quite handy in some cases.
#define OUTPUT_READABLE_WORLDACCEL
int Acc_ctrl_1 = 9;
int Acc_ctrl_2 = 10;
int Acc_ctrl_3 = 11;
int chip_enable1 = 5;
int chip_enable2 = 6;
#define LED_PIN 13 // (Arduino is 13, Teensy is 11, Teensy++ is 6)
bool blinkState = false;
// MPU control/status vars
//uint8_t fifoBuffer[64]; // FIFO storage buffer
// orientation/motion vars
Quaternion q; // [w, x, y, z] quaternion container
VectorInt16 aa; // [x, y, z] accel sensor measurements
VectorInt16 aaReal; // [x, y, z] gravity-free accel sensor measurements
VectorInt16 aaWorld; // [x, y, z] world-frame accel sensor measurements
VectorFloat gravity; // [x, y, z] gravity vector
float euler[3]; // [psi, theta, phi] Euler angle container
float ypr[3]; // [yaw, pitch, roll] yaw/pitch/roll container and gravity vector
// packet structure for InvenSense teapot demo
uint8_t teapotPacket[14] = { '$', 0x02, 0,0, 0,0, 0,0, 0,0, 0x00, 0x00, '\r', '\n' };
// ================================================================
// === INTERRUPT DETECTION ROUTINE ===
// ================================================================
volatile bool mpuInterrupt = false; // indicates whether MPU interrupt pin has gone high
void dmpDataReady() {
mpuInterrupt = true;
}
// ================================================================
// === INITIAL SETUP ===
// ================================================================
void setup() {
pinMode(Acc_ctrl_1, OUTPUT); //S0
pinMode(Acc_ctrl_2, OUTPUT); //S1
pinMode(Acc_ctrl_3, OUTPUT); //S2 address lines
pinMode(chip_enable1, OUTPUT);
// join I2C bus (I2Cdev library doesn't do this automatically)
#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
Wire.begin();
TWBR = 24; // 400kHz I2C clock (200kHz if CPU is 8MHz)
#elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE
Fastwire::setup(400, true);
#endif
// initialize serial communication
// (115200 chosen because it is required for Teapot Demo output, but it's
// really up to you depending on your project)
Serial.begin(115200);
while (!Serial); // wait for Leonardo enumeration, others continue immediately
// initialize device
Serial.println(F("Initializing I2C devices..."));
mpu.initialize();
// verify connection
Serial.println(F("Testing device connections..."));
Serial.println(mpu.testConnection() ? F("MPU6050 connection successful") : F("MPU6050 connection failed"));
// wait for ready
Serial.println(F("\nSend any character to begin DMP programming and demo: "));
while (Serial.available() && Serial.read()); // empty buffer
while (!Serial.available()); // wait for data
while (Serial.available() && Serial.read()); // empty buffer again
// load and configure the DMP
Serial.println(F("Initializing DMP..."));
devStatus = mpu.dmpInitialize();
// supply your own gyro offsets here, scaled for min sensitivity
mpu.setXGyroOffset(220);
mpu.setYGyroOffset(76);
mpu.setZGyroOffset(-85);
mpu.setZAccelOffset(1788); // 1688 factory default for my test chip
// make sure it worked (returns 0 if so)
if (devStatus == 0) {
// turn on the DMP, now that it's ready
Serial.println(F("Enabling DMP..."));
mpu.setDMPEnabled(true);
// enable Arduino interrupt detection
Serial.println(F("Enabling interrupt detection (Arduino external interrupt 0)..."));
attachInterrupt(0, dmpDataReady, RISING);
mpuIntStatus = mpu.getIntStatus();
// set our DMP Ready flag so the main loop() function knows it's okay to use it
Serial.println(F("DMP ready! Waiting for first interrupt..."));
dmpReady = true;
// get expected DMP packet size for later comparison
packetSize = mpu.dmpGetFIFOPacketSize();
} else {
// ERROR!
// 1 = initial memory load failed
// 2 = DMP configuration updates failed
// (if it's going to break, usually the code will be 1)
Serial.print(F("DMP Initialization failed (code "));
Serial.print(devStatus);
Serial.println(F(")"));
}
// configure LED for output
pinMode(LED_PIN, OUTPUT);
}
// ================================================================
// === MAIN PROGRAM LOOP ===
// ================================================================
void loop() {
//Enable the MUX Chip 1 - Active Low
digitalWrite(chip_enable1, LOW);
// control signal for First Accelerometer
Serial.println("IMU 1");
digitalWrite(Acc_ctrl_1, LOW);
digitalWrite(Acc_ctrl_2, LOW);
digitalWrite(Acc_ctrl_3, LOW);
readAccele();
delay(500);
// control signal for SECOND Accelerometer
Serial.println("IMU 2");
digitalWrite(Acc_ctrl_1, HIGH);
digitalWrite(Acc_ctrl_2, LOW);
digitalWrite(Acc_ctrl_3, LOW);
readAccele();
delay(500);
// control signal for THIRD Accelerometer
Serial.println("IMU 3");
digitalWrite(Acc_ctrl_1, LOW);
digitalWrite(Acc_ctrl_2, HIGH);
digitalWrite(Acc_ctrl_3, LOW);
readAccele();
delay(500);
}
void readAccele(){
// if programming failed, don't try to do anything
if (!dmpReady) return;
// wait for MPU interrupt or extra packet(s) available
while (!mpuInterrupt && fifoCount < packetSize) {
// other program behavior stuff here
}
// reset interrupt flag and get INT_STATUS byte
mpuInterrupt = false;
mpuIntStatus = mpu.getIntStatus();
// get current FIFO count
fifoCount = mpu.getFIFOCount();
// check for overflow (this should never happen unless our code is too inefficient)
if ((mpuIntStatus & 0x10) || fifoCount == 1024) {
// reset so we can continue cleanly
mpu.resetFIFO();
Serial.println(F("FIFO overflow!"));
// otherwise, check for DMP data ready interrupt (this should happen frequently)
} else if (mpuIntStatus & 0x02) {
// wait for correct available data length, should be a VERY short wait
while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount();
// read a packet from FIFO
mpu.getFIFOBytes(fifoBuffer, packetSize);
// track FIFO count here in case there is > 1 packet available
// (this lets us immediately read more without waiting for an interrupt)
fifoCount -= packetSize;
#ifdef OUTPUT_READABLE_QUATERNION
// display quaternion values in easy matrix form: w x y z
mpu.dmpGetQuaternion(&q, fifoBuffer);
Serial.print("quat\t");
Serial.print(q.w);
Serial.print("\t");
Serial.print(q.x);
Serial.print("\t");
Serial.print(q.y);
Serial.print("\t");
Serial.println(q.z);
#endif
#ifdef OUTPUT_READABLE_EULER
// display Euler angles in degrees
mpu.dmpGetQuaternion(&q, fifoBuffer);
mpu.dmpGetEuler(euler, &q);
Serial.print("euler\t");
Serial.print(euler[0] * 180/M_PI);
Serial.print("\t");
Serial.print(euler[1] * 180/M_PI);
Serial.print("\t");
Serial.println(euler[2] * 180/M_PI);
#endif
#ifdef OUTPUT_READABLE_WORLDACCEL
// display initial world-frame acceleration, adjusted to remove gravity
// and rotated based on known orientation from quaternion
mpu.dmpGetQuaternion(&q, fifoBuffer);
mpu.dmpGetAccel(&aa, fifoBuffer);
mpu.dmpGetGravity(&gravity, &q);
mpu.dmpGetLinearAccel(&aaReal, &aa, &gravity);
mpu.dmpGetLinearAccelInWorld(&aaWorld, &aaReal, &q);
Serial.print("aworld\t");
Serial.print(aaWorld.x);
Serial.print("\t");
Serial.print(aaWorld.y);
Serial.print("\t");
Serial.println(aaWorld.z);
#endif
;
}
}
But this is what the serial prints, FIFO Overflow.. I tried to fix it but couldn't. I can't upload an image so Ill copy and paste the serial output here as code...
Send any character to begin DMP programming and demo:
Initializing DMP...
Enabling DMP...
Enabling interrupt detection (Arduino external interrupt 0)...
DMP ready! Waiting for first interrupt...
IMU 1
IMU 2
IMU 3
IMU 1
FIFO overflow!
IMU 2
IMU 3
IMU 1
FIFO overflow!
IMU 2
IMU 3
IMU 1
FIFO overflow!
IMU 2
IMU 3
IMU 1
FIFO overflow!
IMU 2
IMU 3
IMU 1
FIFO overflow!
IMU 2
IMU 3
IMU 1
FIFO overflow!
IMU 2
IMU 3
IMU 1
FIFO overflow!
IMU 2
IMU 3
IMU 1
I have been working on a stable library to get all the useful information using the MPU6050_6Axis_MotionApps20 provided by I2Cdev package. I don't know if it is useful for you, but I upload it for everyone having problems dealing with it:
GYRO.h:
#ifndef GYRO
#define GYRO
#include "Arduino.h"
#include "I2Cdev.h"
#include "MPU6050_6Axis_MotionApps20.h"
#include "Wire.h"
#define INTERRUPT_PIN 2 //24
#define CALIBRATION_LOOPS 500
//Error codes
#define EC_NO_ERROR 0
#define EC_DMP_MEMORY_WRITING_FAILED 1
#define EC_DMP_CONFIG_WRITING_FAILED 2
#define EC_NO_CONNECTION 3
//Offsets, put your own offsets here
#define GYRO_GX_OFFSET 93
#define GYRO_GY_OFFSET 0
#define GYRO_GZ_OFFSET 7
#define GYRO_AX_OFFSET -2550
#define GYRO_AY_OFFSET 1978
#define GYRO_AZ_OFFSET 499
class GYRO{
public:
GYRO(void);
uint8_t begin(void);
void update(void);
//All are made following the arrows with the right hand rule
inline VectorFloat getGyroscope(){return gyroscope; }
//All are made following the arrows
inline VectorDouble getAccelerometer(){return accelerometer; }
//A positive 1, indicates that the gravity is going in the oppsite way of the arrow drawed on the sensor
inline VectorFloat getGravity(){return gravity; }
//Returns the temperature in ºC
inline float getTemperature(){return temperature; }
//The time that has passed for calculating the speed with the acceleration
inline unsigned long getMicrosSpent(){return microsSpent; }
inline bool isGyroscopeUpdated(){return gyroscopeUpdated; }
private:
//Gyroscope
MPU6050 gyro; //Gyroscope configured with pin ADO-LOW
//Gyroscope readings
// MPU control/status vars
uint8_t gyroIntStatus; // holds actual interrupt status byte from MPU
uint8_t devStatus; // return status after each device operation (0 = success, !0 = error)
uint16_t packetSize; // expected DMP packet size (default is 42 bytes)
uint16_t fifoCount; // count of all bytes currently in FIFO
uint8_t fifoBuffer[64]; // FIFO storage buffer
// orientation/motion vars
Quaternion q; // [w, x, y, z] quaternion container
VectorFloat gravity; // [x, y, z] gravity vector
float ypr[3]; // [yaw, pitch, roll] yaw/pitch/roll container and gravity vector
VectorFloat gyroscope; // [x, y, z] gyroscope vector
VectorInt16 rawAccelerometer; // [x, y, z] raw acceleromer
VectorDouble accelerometer; // [x, y, z] accelerometer in m/s2
float temperature; // temperature in ºC
unsigned long lastMicros;
unsigned long actualMicros;
unsigned long microsSpent;
bool gyroscopeUpdated;
int loops_before_calibration;
};
#endif
GYRO.cpp:
#include "GYRO.h"
#include "Arduino.h"
#include "MPU6050_6Axis_MotionApps20.h"
#include "Wire.h"
volatile bool interrupt = false;
void dmpDataReady(void) {
interrupt = true;
}
GYRO::GYRO(void){
gyro = MPU6050(0x68); //Change to 0x69 if the AD0 pin of your MPU6050 is HIGH
}
uint8_t GYRO::begin(void){
Wire.begin();
Wire.setClock(400000);
uint8_t errorCode;
//Gyro initialization
gyro.initialize();
pinMode(INTERRUPT_PIN, INPUT);
if(gyro.testConnection()){
Serial.println(F("\nSend any character to begin the calibration and initialization of the gyro: "));
while (Serial.available() && Serial.read());
while (!Serial.available());
while (Serial.available() && Serial.read());
devStatus = gyro.dmpInitialize();
gyro.setXGyroOffset(GYRO_GX_OFFSET);
gyro.setYGyroOffset(GYRO_GY_OFFSET);
gyro.setZGyroOffset(GYRO_GZ_OFFSET);
gyro.setXAccelOffset(GYRO_AX_OFFSET);
gyro.setYAccelOffset(GYRO_AY_OFFSET);
gyro.setZAccelOffset(GYRO_AZ_OFFSET);
if (devStatus == 0) {
gyro.setDMPEnabled(true);
lastMicros = micros();
attachInterrupt(digitalPinToInterrupt(INTERRUPT_PIN), dmpDataReady, RISING);
gyroIntStatus = gyro.getIntStatus();
packetSize = gyro.dmpGetFIFOPacketSize();
}else if(devStatus == 1) return EC_DMP_MEMORY_WRITING_FAILED;
else if(devStatus == 2) return EC_DMP_CONFIG_WRITING_FAILED;
}else return EC_NO_CONNECTION;
loops_before_calibration = 0;
return EC_NO_ERROR;
}
void GYRO::update(void){
//temperature = gyro.getTemperature()/340.0+36.53; //When discommenting this line i get rarely FIFO overflows, but without it, i have not recived eaither one
gyroscopeUpdated = false;
while (!interrupt && fifoCount < packetSize){
if (interrupt && fifoCount < packetSize){
fifoCount = gyro.getFIFOCount();
}
}
interrupt = false;
gyroIntStatus = gyro.getIntStatus();
fifoCount = gyro.getFIFOCount();
if ((gyroIntStatus & _BV(MPU6050_INTERRUPT_FIFO_OFLOW_BIT)) || fifoCount >= 1024){
gyro.resetFIFO();
fifoCount = gyro.getFIFOCount();
Serial.println(F("FIFO overflow!"));
}else if (gyroIntStatus & _BV(MPU6050_INTERRUPT_DMP_INT_BIT)){
while (fifoCount < packetSize) fifoCount = gyro.getFIFOCount();
gyro.getFIFOBytes(fifoBuffer, packetSize);
actualMicros = micros();
microsSpent = actualMicros - lastMicros;
lastMicros = actualMicros;
fifoCount -= packetSize;
gyro.dmpGetQuaternion(&q, fifoBuffer);
gyro.dmpGetGravity(&gravity, &q);
gyro.dmpGetYawPitchRoll(ypr, &q, &gravity);
gyro.dmpGetAccel(&rawAccelerometer, fifoBuffer);
gyro.dmpGetLinearAccel(&rawAccelerometer, &rawAccelerometer, &gravity);
//gyro.dmpGetLinearAccelInWorld(&rawAccelerometer, &rawAccelerometer, &q);
accelerometer.x = rawAccelerometer.x/8192.0*9.8;
accelerometer.y = rawAccelerometer.y/8192.0*9.8;
accelerometer.z = rawAccelerometer.z/8192.0*9.8;
gyroscope.x = ypr[2];
gyroscope.y = ypr[1]*-1;
gyroscope.z = ypr[0]*-1;
if(loops_before_calibration > CALIBRATION_LOOPS){
gyroscopeUpdated = true;
}else loops_before_calibration++;
}
}
Example code(To upload to arduino):
#include <GYRO.h>
#include <helper_3dmath.h>
GYRO gyro = GYRO();
VectorFloat gravity;
VectorFloat gyroscope;
VectorDouble accelerometer;
void setup() {
Serial.begin(115200);
Serial.println(gyro.begin());
}
void loop() {
gyro.update();
if(gyro.isGyroscopeUpdated()){
gravity = gyro.getGravity();
accelerometer = gyro.getAccelerometer();
gyroscope = gyro.getGyroscope();
Serial.print("gyroscope\t");
Serial.print(gyroscope.x * 180/M_PI);
Serial.print("\t");
Serial.print(gyroscope.y * 180/M_PI);
Serial.print("\t");
Serial.print(gyroscope.z * 180/M_PI);
Serial.print("\taccelerometer\t");
Serial.print(accelerometer.x);
Serial.print("\t");
Serial.print(accelerometer.y);
Serial.print("\t");
Serial.print(accelerometer.z);
Serial.print("\tgravity\t");
Serial.print(gravity.x);
Serial.print("\t");
Serial.print(gravity.y);
Serial.print("\t");
Serial.print(gravity.z);
Serial.print("\ttemperature\t");
Serial.print(gyro.getTemperature());
Serial.print(", micros: ");
Serial.println(gyro.getMicrosSpent());
}
}
You have to provide your own offsets. To obtain it, you can use this link
I hope this code is useful.
I know it is too late but maybe will help others...
The first MPU6050 initialization part in the setup() is done before even the mux initialized.
So you have to do the following, inside the setup(), at the right order:
initialize wire
initialize the outputs for the mux
select the first MPU using the right output levels
initialize the first MPU6050
select the second MPU using the right output levels
initialize the second MPU6050
select the third MPU using the right output levels
initialize the third MPU6050