ESP32 Serial1 not working with the Sparkfun Mpu9250 DMP Library

[yo90bosses]

Hardware:

Board: NodeMCU_32
Core Installation/update date: 27/Aug/2018
IDE name: Arduino IDE
Flash Frequency: 80Mhz
PSRAM enabled: ?
Upload Speed: 115200
Computer OS: Windows 10

Description:

I'm using the ESP32 and if I use a different Library for the MPU9250 such as the one from BolderFlight then it works flawlessly. But when I use the sparkfun DMP Library with the DMP then it too works fine except for the serial1 where I have an SBUS receiver hooked up. It looks as if when the first bytes are recieved it works but after the first complete set of data I don't get anymore readings from the serial1. Everything else keeps working except for Serial1. And as I said other libraries for the mpu9250 do work with Serial1.
Also the Mpu9250 is connected over I2C

Sketch: (leave the backquotes for code formatting)

//Change the code below by your sketch
#include "UBLOX.h"
#include <string.h>
#include <SparkFunMPU9250-DMP.h>
#include <HMC5883L.h>
#include "BMP280.h"
#include "Wire.h"

#define IBUS_MAXCHANNELS 14
#define IBUS_BUFFSIZE 32


//##### Name Definitions for Placeholders #####
#define QUE 0
#define HOH 1
#define MOT 2
#define SEI 3
#define SWB 4
#define SWC 5
#define SWD 6
#define SWE 7
#define VRA 8
#define VRB 9
#define SWA 10
#define AUX8 11
#define AUX9 12
#define AUX10 13


#define QUEL 0
#define QUER 4
#define FLAL 5
#define FLAR 6
#define AUX1 7
#define AUX2 8


#define VACHSE  2
#define LACHSE  0
#define QACHSE  1
#define HEADING 4

#define W 3
#define X 0
#define Y 1
#define Z 2
//#define M 3

#define P0 1024.7

#define VACHSE 2
#define LACHSE 0
#define QACHSE 1

// an MPU-9250 object on SPI bus 0 with chip select 10
MPU9250_DMP imu;
UBLOX gps(Serial2,115200);
bool RxUpdate = false;
int status;
float Angle[3];
float Altitude;
float Lat, Lon;
float M[3];
float MinH[3] = {4800, 4800, 4800};
float MaxH[3] = {-4800, -4800, -4800};
float BH[3];
bool GpsUpdate = false;
// a uNavAHRS object

BMP280 bmp;
unsigned long FilterTime = 0;
unsigned long IMUTime = 0;
unsigned long BMPTime = 0;
// a flag for when the MPU-9250 has new data
volatile int newData;
// EEPROM buffer and variables to load accel and mag bias 
// and scale factors from CalibrateMPU9250.ino
int rcInput[IBUS_MAXCHANNELS];

float axb = -0.027103901, 
      axs = 0.998626173,
      ayb = -0.027103901, 
      ays = 1.006643891, 
      azb = -0.020797253, 
      azs = 1.002618909;

float hxb = -21.026420593, 
      hxs = 0.857798100, 
      hyb = 0.806013107, 
      hys = 1.132546306, 
      hzb = 25.876937866, 
      hzs = 1.051239014;

// timers to measure performance
unsigned long tstart, tstop;

void setup() {
  
  gps.begin();
  
  // serial to display data
  Serial.begin(115200);
  while(!Serial) {}

  // start communication with IMU 
  if (imu.begin() != INV_SUCCESS)
  {
    while (1)
    {
      Serial.println("Unable to communicate with MPU-9250");
      Serial.println("Check connections, and try again.");
      Serial.println();
      delay(5000);
    }
  }
  
  imu.dmpBegin(DMP_FEATURE_6X_LP_QUAT | // Enable 6-axis quat
               DMP_FEATURE_GYRO_CAL, // Use gyro calibration
              100); // Set DMP FIFO rate to 10 Hz
  // DMP_FEATURE_LP_QUAT
  

  //setupBME();

  //magCal();
  
  
  
  /*while (!compass.begin())
  {
    delay(500);
  }

  // Set measurement range
  compass.setRange(HMC5883L_RANGE_1_3GA);

  // Set measurement mode
  compass.setMeasurementMode(HMC5883L_CONTINOUS);

  // Set data rate
  compass.setDataRate(HMC5883L_DATARATE_30HZ);

  // Set number of samples averaged
  compass.setSamples(HMC5883L_SAMPLES_8);

  // Set calibration offset. See HMC5883L_calibration.ino
  compass.setOffset(0, 0);*/


  setupReceiver();
  
}



void setupBME(){

  //Wire.begin();

  for (int n = 0; !bmp.begin(); n++) {
    
    Serial.println("Could not find BME280 sensor!");
    delay(500);
    
    if (n >= 10) {
      return;
    }
  
  }

  bmp.setOversampling(16);
  
  /*for (int n = 0; n < 10; n++) {
    
    PosSt[Z] += getAltitude();
    delay(200);
    
  }
  
  PosSt[Z] /= 10;*/
  
  
  //SensorsStatus |= _BV(ALTIMETER_OK);
  
  
}



void setupReceiver() {
  
  //Serial1.begin(38400, SERIAL_8N1, 18, 4);

  Serial1.begin(115200, SERIAL_8N1, 18, 4);
  
  delay(10);
  
  
  unsigned long Start = millis();
  
  bool ReceiverOk = false;
  
  while (!ReceiverOk) {
    
    readRx();
    
    if (RxUpdate) {
      RxUpdate = false;
      
      for (int n = 0; n < 11; n++) {
      
        if (rcInput[n] != 0) {
          ReceiverOk = true;
        }
      
      }
      
    }
    
    if (millis() - Start > 2000) {
      return;
    }
    
  }
  
  //SensorsStatus |= _BV(RECEIVER_OK);
  
}



void loop() {
  
  //magRT();

  readRx();
  
  IMURT();

  //BMPRT();

  //GPSRT();

  dataOut();
  
}



void GPSRT() {

  if (gps.readSensor()) GpsUpdate = true;
  
}



void dataOut() {

  static unsigned long RT = millis();

  if (millis() < RT) return;
  RT = millis() + 50;

  Serial.print("Sats: ");
  Serial.print(gps.getNumSatellites());
  Serial.print("  Lat: ");

  Serial.print(gps.getLatitude_deg(), 10);
  Serial.print("  Lon: ");
  
  Serial.print(gps.getLongitude_deg(), 10);
  Serial.print("  R: ");

  Serial.print(Angle[LACHSE]);
  Serial.print("  P: ");

  Serial.print(Angle[QACHSE]);
  Serial.print("  Y:");

  Serial.print(Angle[VACHSE]);
  Serial.print("  Alt(BMP): ");

  Serial.print(Altitude);
  Serial.print(" ");

  Serial.print(M[0], 3);
  Serial.print(", ");
  Serial.print(M[1], 3);
  Serial.print(", ");
  Serial.print(M[2], 3);
  Serial.print(" ");
  
  float Heading = -atan2(M[1], M[0])*180/M_PI;
  Serial.print(Heading);
  Serial.print(", ");

  Serial.print(BH[0], 3);
  Serial.print(", ");
  Serial.print(BH[1], 3);
  Serial.print(", ");
  Serial.print(BH[2], 3);
  Serial.print(" RX: Mot: ");

  Serial.print(rcInput[MOT]);
  Serial.println(" ");

  
}



void BMPRT() {

  static unsigned long RT = millis();

  if (millis() < RT) return;
  RT = millis() + 100;
  
  double T,P;
  char result = bmp.startMeasurment();
 
  if(result!=0){
    //delay(result);
    result = bmp.getTemperatureAndPressure(T,P);
    
      if(result!=0)
      {
        Altitude = bmp.altitude(P,P0);
        
        //Serial.print("T = \t");Serial.print(T,2); Serial.print(" degC\t");
        //Serial.print("P = \t");Serial.print(P,2); Serial.print(" mBar\t");
        //Serial.print("A = \t");Serial.print(A,2); Serial.println(" m");
       
      }
      else {
        Serial.println("Error.");
      }
  }
  else {
    Serial.println("Error.");
  }
  
  //delay(100);
}



void IMURT() {

  static float y = 0;

  if ( imu.fifoAvailable() )
  {
    // Use dmpUpdateFifo to update the ax, gx, mx, etc. values
    if ( imu.dmpUpdateFifo() == INV_SUCCESS)
    {
      // computeEulerAngles can be used -- after updating the
      // quaternion values -- to estimate roll, pitch, and yaw
      imu.computeEulerAngles();
      
      Angle[QACHSE] = imu.pitch-180;
      Angle[LACHSE] = imu.roll-180;
      Angle[VACHSE] = imu.yaw-180;
      
      Angle[HEADING] += Angle[VACHSE] - y;
      
      y = Angle[VACHSE];
    
    }
  }
  
  /*if (newData == 1) {
    newData = 0;
    
    
    //tstart = micros();
    // read the sensor
    Imu.readSensor();
    
    
    M[0] = Imu.getMagY_uT();
    M[1] = -Imu.getMagX_uT();
    M[2] = Imu.getMagZ_uT();
    
    for (int n = 0; n < 3; n++) {
      
      if (M[n] > MaxH[n]) MaxH[n] = M[n];
      if (M[n] < MinH[n]) MinH[n] = M[n];
      
      BH[n] = (MinH[n] + MaxH[n])/2;
      
      M[n] -= BH[n];
    
    }
    
    
    //tstop = micros();
    
    //IMUTime = tstop - tstart;
    
    //tstart = micros();
    // update the filter
    if (Filter.update(Imu.getGyroY_rads(),-Imu.getGyroX_rads(),Imu.getGyroZ_rads(),Imu.getAccelY_mss(),-Imu.getAccelX_mss(),Imu.getAccelZ_mss(),1,1,1)) {
      //tstop = micros();
      
      Angle[QACHSE] = Filter.getPitch_rad()*180.0f/PI;
      Angle[LACHSE] = Filter.getRoll_rad()*180.0f/PI;
      Angle[VACHSE] = Filter.getHeading_rad()*180.0f/PI-180;
      
      //FilterTime = tstop - tstart;
      
    }
  }*/
  
}



void runFilter() {
  newData = 1;
}



void readRx() {

  static uint8_t ibusIndex = 0;

  static uint8_t ibus[IBUS_BUFFSIZE] = {0};

  static uint8_t i;

  static uint16_t chksum, rxsum;

  static uint16_t rcBuffer[14];
  
  static unsigned long LastAvail = millis();
  static unsigned long LastRun = millis();

  
  
  uint8_t avail = Serial1.available();

  if (avail) {
    
    LastAvail = millis();
    
    //Count++;

    uint8_t val = Serial1.read();

    // Look for 0x2040 as start of packet

    if (ibusIndex == 0 && val != 0x20) {

      return;

    }

    if (ibusIndex == 1 && val != 0x40) {

      ibusIndex = 0;

      return;

    }

    if (ibusIndex < IBUS_BUFFSIZE) ibus[ibusIndex] = val;

    ibusIndex++;


    if (ibusIndex == IBUS_BUFFSIZE) {

      ibusIndex = 0;

      chksum = 0xFFFF;

      for (i = 0; i < 30; i++) chksum -= ibus[i];

      rxsum = ibus[30] + (ibus[31] << 8);
      

      if (chksum == rxsum) {
        
        //Count1++;

        //Unrolled loop for 10 channels - no need to copy more than needed.

        // MODIFY IF MORE CHANNELS NEEDED

        rcBuffer[QUE] = (ibus[ 3] << 8) + ibus[ 2];

        rcBuffer[HOH] = (ibus[ 5] << 8) + ibus[ 4];

        rcBuffer[MOT] = (ibus[ 7] << 8) + ibus[ 6];

        rcBuffer[SEI] = (ibus[ 9] << 8) + ibus[ 8];

        rcBuffer[SWE] = (ibus[11] << 8) + ibus[10];

        rcBuffer[SWC] = (ibus[13] << 8) + ibus[12];

        rcBuffer[VRB] = (ibus[15] << 8) + ibus[14];

        rcBuffer[VRA] = (ibus[17] << 8) + ibus[16];

        rcBuffer[SWB] = (ibus[19] << 8) + ibus[18];

        rcBuffer[SWD] = (ibus[21] << 8) + ibus[20];

        rcBuffer[SWA] = (ibus[23] << 8) + ibus[22];

        for (int n = 0; n < 11; n++) {
          rcInput[n] = int(rcBuffer[n]) - 1500;
        }
        
        RxUpdate = true;
        
        
        bool Failsafe = false;
        
        // if Inputs in odd range then assume 
        // no signal
        for (int n = 0; n < 11; n++) {
          if (rcInput[n]>700 || rcInput[n]<-700) {
            Failsafe = true;
          }
        }
        
       

        digitalWrite(LED_BUILTIN, LOW); // OK packet - Clear error LED

      } else {

        digitalWrite(LED_BUILTIN, HIGH); // Checksum error - turn on error LED

      }

      return;

    }

  } else if (millis() - LastAvail > 500) {
    
    // if no available info for 500ms then assume 
    // connection failure
    if (millis() - LastRun < 500) {
      
    }
  
  }
  
  LastRun = millis();

}

Debug Messages:

Enable Core debug level: Debug on tools menu of Arduino IDE, then put the serial output here 

[mehtajainam]

Hi there,

Did you manage to get the Sparkfun DMP library working with the ESP32?

[yo90bosses]

@mehtajainam
I got the sensor working great. The DMP from the mpu9250 only does 6dof calculations so it's not in reference to North and drifts. So I couldn't use it anyway.

I used the RTIMU library which worked great when correctly setup. I would send you the modified library if I knew a way.

[mehtajainam]

Ah alright. I was having issues getting the Sparkfun DMP library to compile for the Feather ESP32 board. Managed to get it working yesterday though - there was a missing min function in the library...
Thanks anyways!

[alexandresvifpb]

Ah alright. I was having issues getting the Sparkfun DMP library to compile for the Feather ESP32 board. Managed to get it working yesterday though - there was a missing min function in the library...
Thanks anyways!

Dear mehtajainam, could you explain what it takes to make Sparkfun DMP work on ESP32?

Thanks!

[mehtajainam]

@alexandresvifpb just add these lines into inv_mup.c

#ifdef ESP32
#define min(a,b) ((a)<(b)?(a):(b))
#endif

sparkfun/SparkFun_MPU-9250-DMP_Arduino_Library#27

[stale]

This issue has been automatically marked as stale because it has not had recent activity. It will be closed if no further activity occurs. Thank you for your contributions.

[stale]

This stale issue has been automatically closed. Thank you for your contributions.

[laukik-hase]

@yo90bosses @mehtajainam Could you help with the connections for this library? Does the DMP functioning need an additional interrupt pin with SDA and SCL?