1

I have been using MPU6050 modules for some time now for yaw measurements using the integrated DMP chip. Everything has gone swimmingly until yesterday, when my initialization code started reporting that I can connect to my MPU6050 chip but DMP init has failed with code 1.

Thinking I had somehow blown up the DMP chip (don't know how, as there is no direct connection), I replaced the module, but got the same result, with three different MPU6050 modules.

As an experiment, I modified my code to read raw Z-axis acceleration values instead of DMP quaternions, and this was successful. Plotting the values in Excel showed that the values responded as expected when I manually rotated the sensor around the Z-axis.

So now I know I can connect to the MPU6050 module and read sensor values, but I somehow lost the ability to use the DMP. Any ideas as to why this might be happening?

Here are two versions of my code. The first version is the original, and the second one has been modified to just show the Z-axis acceleration values.

TIA,

Frank

Version 1 (original):

// Visual Micro is in vMicro>General>Tutorial Mode
// 
/*
    Name:       MPU6050_DRV8871_NoiseTest.ino
    Created:    12/8/2019 10:20:28 AM
    Author:     FRANKNEWXPS15\Frank
*/

/*
    Name:       Adafruit DRV8871 Motor Driver Test.ino
    Created:    5/30/2019 6:43:18 PM
    Author:     FRANKWIN10\Frank
*/
//#include <Adafruit_INA219.h>
#pragma region INCLUDES
#include <elapsedMillis.h>
#include <PrintEx.h> //allows printf-style printout syntax
#include "MPU6050_6Axis_MotionApps_V6_12.h"  //changed to this version 10/05/19
#include "I2Cdev.h" //02/19/19: this includes SBWire.h
#include <NewPing.h> //added 01/15/15
#pragma endregion INCLUDES

StreamEx mySerial = Serial; //added 03/18/18 for printf-style printing
elapsedMillis   sinceLastNavUpdateMsec; //added 10/15/18 to replace lastmillisec

//MPU6050 mpu(0x69); //06/23/18 chg to AD0 high addr on 2-motor robot
MPU6050 mpu(0x68); //06/23/18 chg to AD0 high addr on 2-motor robot

void(*resetFunc) (void) = 0; //added 12/08/19 to handle MPU6050/I2C reset failure issue

#pragma region MPU6050_SUPPORT
uint8_t mpuIntStatus;   // holds actual interrupt status byte from MPU. Used in Homer's Overflow routine
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
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
int GetPacketLoopCount = 0;
int OuterGetPacketLoopCount = 0;

//RTC/FRAM/MPU6050 status flags
bool bMPU6050Ready = true;
bool dmpReady = false;  // set true if DMP init was successful
volatile float global_yawval = 0; //updated by GetIMUHeadingDeg()
const uint16_t MAX_GETPACKET_LOOPS = 100; //10/30/19 added for backup loop exit condition in GetCurrentFIFOPacket()
uint8_t GetCurrentFIFOPacket(uint8_t* data, uint8_t length, uint16_t max_loops = MAX_GETPACKET_LOOPS); //prototype here so can define a default param
bool bFirstTime = true;
#pragma endregion MPU6050 Support




// Basic sketch for trying out the Adafruit DRV8871 Breakout

#define MOTOR1_IN1 9
#define MOTOR1_IN2 10
#define MOTOR2_IN1 6
#define MOTOR2_IN2 5

const int INTER_STEP_DELAY_MS = 100;

const int NAV_UPDATE_INTERVAL_MSEC = 200;

const int MIN_MOTOR_SPEED = 60; //empirically determined

enum MotorStates
{
    MOTOR_FWD_INCREASING,
    MOTOR_FWD_DECREASING,
    MOTOR_REV_INCREASING,
    MOTOR_REV_DECREASING
};

enum MotorStates motState = MOTOR_FWD_INCREASING;
const int MOTOR_SPEED_MAX = 255;
//const int MOTOR_SPEED_MAX = 100;
const int MOTOR_SPEED_MIN = 60; //empirically determined
int motor_speed = 0;
int pgmloops = 0;


void setup() {
    Serial.begin(115200);
    Wire.setClock(50000);

    // initialize MPU6050 added 09/03/18
    Serial.println(F("Initializing MPU6050 ..."));
    mpu.initialize();

    // verify connection
    Serial.println(F("Testing device connections..."));
    Serial.println(mpu.testConnection() ? F("MPU6050 connection successful") : F("MPU6050 connection failed"));

    Serial.println(F("Initializing DMP..."));
    devStatus = mpu.dmpInitialize();

    // supply your own gyro offsets here, scaled for min sensitivity
    ////12/03/19 settings from calibration routines
    //mpu.setXGyroOffset(58);
    //mpu.setYGyroOffset(20);
    //mpu.setZGyroOffset(12);
    //mpu.setXAccelOffset(1618);
    //mpu.setYAccelOffset(3046);
    //mpu.setZAccelOffset(4554);

    //12/16/19 settings from calibration routines
    mpu.setXAccelOffset(-3066);
    mpu.setYAccelOffset(2658);
    mpu.setZAccelOffset(872);
    mpu.setXGyroOffset(167);
    mpu.setYGyroOffset(-26);
    mpu.setZGyroOffset(73);

    // make sure it worked (returns 0 if successful)
    if (devStatus == 0)
    {
        //12/06/19 no longer needed; cal vals already acquired
        // Calibration Time: generate offsets and calibrate our MPU6050
        //mpu.CalibrateAccel(6); 
        //mpu.CalibrateGyro(6);
        //Serial.println();
        //mpu.PrintActiveOffsets();

        // turn on the DMP, now that it's ready
        Serial.println(F("Enabling DMP..."));
        mpu.setDMPEnabled(true);

        // set our DMP Ready flag so the main loop() function knows it's okay to use it
        Serial.println(F("DMP ready! Waiting for MPU6050 drift rate to settle..."));
        dmpReady = true;

        // get expected DMP packet size for later comparison
        packetSize = mpu.dmpGetFIFOPacketSize();
        bMPU6050Ready = true;
        mySerial.printf("\nMPU6050 Ready at %2.2f Sec\n", millis() / 1000.f);
    }
    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(")"));

        bMPU6050Ready = false;
    }


    Serial.println("DRV8871 test");

    pinMode(MOTOR1_IN1, OUTPUT);
    pinMode(MOTOR1_IN2, OUTPUT);
    pinMode(MOTOR2_IN1, OUTPUT);
    pinMode(MOTOR2_IN2, OUTPUT);

    //ina219.begin();
    //mySerial.printf("Speed\tVolts\tMAmps\tMWatts\n"); //column headers
    mySerial.printf("Speed\n"); //column headers

    TCCR2B = (TCCR2B & B11111000) | B00000011; // for PWM frequency of 980.39 Hz on pins 3,11

    sinceLastNavUpdateMsec = 0;
}

void loop() 
{
    if (sinceLastNavUpdateMsec >= NAV_UPDATE_INTERVAL_MSEC)
    {
        if (pgmloops % 50 == 0)
        {
            bFirstTime = false;
            mySerial.printf("Min\tYaw\tState\tSpd\tfifoC\tIloops\Oloops\n");
        }

        sinceLastNavUpdateMsec -= NAV_UPDATE_INTERVAL_MSEC;

        //12/10/19 added connection test and reset global 'GetPacket' params
        bool getYawResult = false;
        fifoCount = 0;
        GetPacketLoopCount = 0;
        OuterGetPacketLoopCount = 0;
        global_yawval = 999;
        if (mpu.testConnection())
        {
            getYawResult = GetIMUHeadingDeg(); //updates global_yawval if successful
        }
        else
        {
            mySerial.printf("MPU6050 Not Connected!\n");
        }

        if (getYawResult == 0)
        {
            mySerial.printf("MPU6050 and/or I2C failure - Stopping Test!\n");
            mySerial.printf("min\tyaw\tfifoC\tICount\tOCount\n");
            mySerial.printf("%3.2f\t%3.2f\t%d\t%d\t%d\n",
                millis() / 60000.0,global_yawval, fifoCount, GetPacketLoopCount, OuterGetPacketLoopCount);
            //resetFunc(); //this should completely reset the Arduino so we can start over

            //stop motor 1
            analogWrite(MOTOR1_IN1, 0);
            analogWrite(MOTOR1_IN2, 0);

            //stop motor 2
            analogWrite(MOTOR2_IN1, 0);
            analogWrite(MOTOR2_IN2, 0);
            while (1); //stop the system so I can see failure readout
        }

        mySerial.printf("%3.2f\t%3.2f\t%d\t%d\t%d\t%d\t%d\n", 
            millis() / 60000.0, global_yawval,motState,motor_speed,fifoCount, GetPacketLoopCount,
            OuterGetPacketLoopCount);

        //Motor speed/dir management
        switch (motState)
        {
        case MOTOR_FWD_INCREASING:
            // ramp up forward
            digitalWrite(MOTOR1_IN1, LOW);
            digitalWrite(MOTOR2_IN1, LOW);
            motor_speed = (motor_speed > MIN_MOTOR_SPEED) ? motor_speed : MIN_MOTOR_SPEED;
            //if (motor_speed <= MIN_MOTOR_SPEED)
            //{
            //  //Serial.println("give it a whack to start!");
            //  analogWrite(MOTOR1_IN2, MIN_MOTOR_SPEED);
            //  analogWrite(MOTOR2_IN2, MIN_MOTOR_SPEED);
            //}
            //else
            {
                analogWrite(MOTOR1_IN2, motor_speed);
                analogWrite(MOTOR2_IN2, motor_speed);
            }
            motor_speed++;
            if (motor_speed >= MOTOR_SPEED_MAX)
            {
                motState = MOTOR_FWD_DECREASING;
            }
            break;
        case MOTOR_FWD_DECREASING:
            // ramp down forward
            digitalWrite(MOTOR1_IN1, LOW);
            digitalWrite(MOTOR2_IN1, LOW);

            analogWrite(MOTOR1_IN2, motor_speed);
            analogWrite(MOTOR2_IN2, motor_speed);

            motor_speed--;

            if (motor_speed <= MOTOR_SPEED_MIN)
            {
                motState = MOTOR_REV_INCREASING;
            }
            break;
        case MOTOR_REV_INCREASING:
            digitalWrite(MOTOR1_IN2, LOW);
            digitalWrite(MOTOR2_IN2, LOW);

            motor_speed = (motor_speed > MIN_MOTOR_SPEED) ? motor_speed : MIN_MOTOR_SPEED;
            //if (motor_speed <= MIN_MOTOR_SPEED)
            //{
            //  //Serial.println("give it a whack to start!");
            //  analogWrite(MOTOR1_IN1, MIN_MOTOR_SPEED);
            //  analogWrite(MOTOR2_IN1, MIN_MOTOR_SPEED);
            //}
            //else
            {
                analogWrite(MOTOR1_IN1, motor_speed);
                analogWrite(MOTOR2_IN1, motor_speed);
            }

            motor_speed++;

            if (motor_speed >= MOTOR_SPEED_MAX)
            {
                motState = MOTOR_REV_DECREASING;
            }
            break;
        case MOTOR_REV_DECREASING:
            digitalWrite(MOTOR1_IN2, LOW);
            digitalWrite(MOTOR2_IN2, LOW);

            analogWrite(MOTOR1_IN1, motor_speed);
            analogWrite(MOTOR2_IN1, motor_speed);

            motor_speed--;

            if (motor_speed <= MOTOR_SPEED_MIN)
            {
                motState = MOTOR_FWD_INCREASING; //start over
            }
            break;
        default:
            break;
        }

        pgmloops++;
        if (pgmloops == 1000)
        {
            pgmloops = 0;
        }
    }

}


void PrintTelemetryData(int spd)
{

    ////get current, voltage, power readings
    //shuntvoltage = ina219.getShuntVoltage_mV();
    //busvoltage = ina219.getBusVoltage_V();
    //current_mA = ina219.getCurrent_mA();
    //power_mW = ina219.getPower_mW();
    //loadvoltage = busvoltage + (shuntvoltage / 1000);
    //mySerial.printf("%d\t%2.2f\t%2.2f\t%2.2f\n",
    //  spd, loadvoltage, current_mA, power_mW);
    mySerial.printf("%d\n", spd);
}

bool GetIMUHeadingDeg()
{
    //Purpose: Get latest yaw (heading) value from IMU
    //Inputs: None.  This function should only be called after mpu.dmpPacketAvailable() returns TRUE
    //Outputs: 
    //  returns true if successful, otherwise false
    //  global_yawval updated on success
    //Plan:
    //Step1: check for overflow and reset the FIFO if it occurs. In this case, wait for new packet
    //Step2: read all available packets to get to latest data
    //Step3: update global_yawval with latest value
    //Notes:
    //  10/08/19 changed return type to boolean
    //  10/08/19 no longer need mpuIntStatus

    bool retval = false;

    int flag = GetCurrentFIFOPacket(fifoBuffer, packetSize, MAX_GETPACKET_LOOPS); //get the latest mpu packet

    if (flag != 0) //0 = error exit, 1 = normal exit, 2 = recovered from an overflow
    {
        // display Euler angles in degrees
        mpu.dmpGetQuaternion(&q, fifoBuffer);
        mpu.dmpGetGravity(&gravity, &q);
        mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);

        //compute the yaw value
        global_yawval = ypr[0] * 180 / M_PI;

        //if (global_yawval == 180)
        //{
        //  mySerial.printf("erroneous yaw value detected!");
        //  return false;
        //}
        retval = true;
    }

    return retval;
}

uint8_t GetCurrentFIFOPacket(uint8_t* data, uint8_t length, uint16_t max_loops)
{
    max_loops = max(10, max_loops); //added 12/04/19 to ensure we loop at least 10 times. This really IS a hard-coded number!

    fifoCount = mpu.getFIFOCount();

    GetPacketLoopCount = 0;
    int max_outer_loops = 10;

    //11/10/19 I have seen this fail with fifoC > 28 on occasion, so now I loop max 10 times
    OuterGetPacketLoopCount = 0;
    while (fifoCount != packetSize && OuterGetPacketLoopCount < max_outer_loops)
    {
        mpu.resetFIFO();
        delay(1);

        fifoCount = mpu.getFIFOCount();
        GetPacketLoopCount = 0;

        while (fifoCount < packetSize && GetPacketLoopCount < max_loops)
        {
            GetPacketLoopCount++;
            fifoCount = mpu.getFIFOCount();
            delay(2);
        }

        OuterGetPacketLoopCount++;
    }

    //if we get to here, there should be exactly one packet in the FIFO
    if (OuterGetPacketLoopCount < max_outer_loops)
    {
        mpu.getFIFOBytes(data, packetSize);
        return 1;
    }
    return 0; //failed to get a good packet
}

Version 2 (modified to read raw Z-axis accel:

// Visual Micro is in vMicro>General>Tutorial Mode
// 
/*
    Name:       MPU6050_DRV8871_NoiseTest.ino
    Created:    12/8/2019 10:20:28 AM
    Author:     FRANKNEWXPS15\Frank
*/

/*
    Name:       Adafruit DRV8871 Motor Driver Test.ino
    Created:    5/30/2019 6:43:18 PM
    Author:     FRANKWIN10\Frank
*/
//#include <Adafruit_INA219.h>
#pragma region INCLUDES
#include <elapsedMillis.h>
#include <PrintEx.h> //allows printf-style printout syntax
#include "MPU6050_6Axis_MotionApps_V6_12.h"  //changed to this version 10/05/19
#include "I2Cdev.h" //02/19/19: this includes SBWire.h
#include <NewPing.h> //added 01/15/15
#pragma endregion INCLUDES

StreamEx mySerial = Serial; //added 03/18/18 for printf-style printing
elapsedMillis   sinceLastNavUpdateMsec; //added 10/15/18 to replace lastmillisec

//MPU6050 mpu(0x69); //06/23/18 chg to AD0 high addr on 2-motor robot
MPU6050 mpu(0x68); //06/23/18 chg to AD0 high addr on 2-motor robot

void(*resetFunc) (void) = 0; //added 12/08/19 to handle MPU6050/I2C reset failure issue

#pragma region MPU6050_SUPPORT
uint8_t mpuIntStatus;   // holds actual interrupt status byte from MPU. Used in Homer's Overflow routine
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
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
int GetPacketLoopCount = 0;
int OuterGetPacketLoopCount = 0;

//RTC/FRAM/MPU6050 status flags
bool bMPU6050Ready = true;
bool dmpReady = false;  // set true if DMP init was successful
volatile float global_yawval = 0; //updated by GetIMUHeadingDeg()
const uint16_t MAX_GETPACKET_LOOPS = 100; //10/30/19 added for backup loop exit condition in GetCurrentFIFOPacket()
uint8_t GetCurrentFIFOPacket(uint8_t* data, uint8_t length, uint16_t max_loops = MAX_GETPACKET_LOOPS); //prototype here so can define a default param
bool bFirstTime = true;
#pragma endregion MPU6050 Support




// Basic sketch for trying out the Adafruit DRV8871 Breakout

#define MOTOR1_IN1 9
#define MOTOR1_IN2 10
#define MOTOR2_IN1 6
#define MOTOR2_IN2 5

const int INTER_STEP_DELAY_MS = 100;

const int NAV_UPDATE_INTERVAL_MSEC = 200;

const int MIN_MOTOR_SPEED = 60; //empirically determined

enum MotorStates
{
    MOTOR_FWD_INCREASING,
    MOTOR_FWD_DECREASING,
    MOTOR_REV_INCREASING,
    MOTOR_REV_DECREASING
};

enum MotorStates motState = MOTOR_FWD_INCREASING;
const int MOTOR_SPEED_MAX = 255;
//const int MOTOR_SPEED_MAX = 100;
const int MOTOR_SPEED_MIN = 60; //empirically determined
int motor_speed = 0;
int pgmloops = 0;


void setup() {
    Serial.begin(115200);
    Wire.setClock(50000);

    // initialize MPU6050 added 09/03/18
    Serial.println(F("Initializing MPU6050 ..."));
    mpu.initialize();

    // verify connection
    Serial.println(F("Testing device connections..."));
    Serial.println(mpu.testConnection() ? F("MPU6050 connection successful") : F("MPU6050 connection failed"));

    Serial.println(F("Initializing DMP..."));
    devStatus = mpu.dmpInitialize();

    // supply your own gyro offsets here, scaled for min sensitivity
    ////12/03/19 settings from calibration routines
    //mpu.setXGyroOffset(58);
    //mpu.setYGyroOffset(20);
    //mpu.setZGyroOffset(12);
    //mpu.setXAccelOffset(1618);
    //mpu.setYAccelOffset(3046);
    //mpu.setZAccelOffset(4554);

    //12/16/19 settings from calibration routines
    mpu.setXAccelOffset(-3066);
    mpu.setYAccelOffset(2658);
    mpu.setZAccelOffset(872);
    mpu.setXGyroOffset(167);
    mpu.setYGyroOffset(-26);
    mpu.setZGyroOffset(73);

    // make sure it worked (returns 0 if successful)
    if (devStatus == 0)
    {
        //12/06/19 no longer needed; cal vals already acquired
        // Calibration Time: generate offsets and calibrate our MPU6050
        //mpu.CalibrateAccel(6); 
        //mpu.CalibrateGyro(6);
        //Serial.println();
        //mpu.PrintActiveOffsets();

        // turn on the DMP, now that it's ready
        Serial.println(F("Enabling DMP..."));
        mpu.setDMPEnabled(true);

        // set our DMP Ready flag so the main loop() function knows it's okay to use it
        Serial.println(F("DMP ready! Waiting for MPU6050 drift rate to settle..."));
        dmpReady = true;

        // get expected DMP packet size for later comparison
        packetSize = mpu.dmpGetFIFOPacketSize();
        bMPU6050Ready = true;
        mySerial.printf("\nMPU6050 Ready at %2.2f Sec\n", millis() / 1000.f);
    }
    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(")"));

        bMPU6050Ready = false;
    }


    Serial.println("DRV8871 test");

    pinMode(MOTOR1_IN1, OUTPUT);
    pinMode(MOTOR1_IN2, OUTPUT);
    pinMode(MOTOR2_IN1, OUTPUT);
    pinMode(MOTOR2_IN2, OUTPUT);

    //ina219.begin();
    //mySerial.printf("Speed\tVolts\tMAmps\tMWatts\n"); //column headers
    mySerial.printf("Speed\n"); //column headers

    TCCR2B = (TCCR2B & B11111000) | B00000011; // for PWM frequency of 980.39 Hz on pins 3,11

    sinceLastNavUpdateMsec = 0;
}

void loop() 
{
    if (sinceLastNavUpdateMsec >= NAV_UPDATE_INTERVAL_MSEC)
    {
        if (pgmloops % 50 == 0)
        {
            bFirstTime = false;
            mySerial.printf("Min\tYaw\tState\tSpd\tfifoC\tIloops\Oloops\n");
        }

        sinceLastNavUpdateMsec -= NAV_UPDATE_INTERVAL_MSEC;

        //12/10/19 added connection test and reset global 'GetPacket' params
        bool getYawResult = false;
        fifoCount = 0;
        GetPacketLoopCount = 0;
        OuterGetPacketLoopCount = 0;
        global_yawval = 999;
        if (mpu.testConnection())
        {
            getYawResult = GetIMUHeadingDeg(); //updates global_yawval if successful
            float Zaccel = mpu.getAccelerationZ();
            mySerial.printf("Zaccel = %3.2f\n", Zaccel);
        }
        else
        {
            mySerial.printf("MPU6050 Not Connected!\n");
        }

        //if (getYawResult == 0)
        //{
        //  mySerial.printf("MPU6050 and/or I2C failure - Stopping Test!\n");
        //  mySerial.printf("min\tyaw\tfifoC\tICount\tOCount\n");
        //  mySerial.printf("%3.2f\t%3.2f\t%d\t%d\t%d\n",
        //      millis() / 60000.0,global_yawval, fifoCount, GetPacketLoopCount, OuterGetPacketLoopCount);
        //  //resetFunc(); //this should completely reset the Arduino so we can start over

        //  //stop motor 1
        //  analogWrite(MOTOR1_IN1, 0);
        //  analogWrite(MOTOR1_IN2, 0);

        //  //stop motor 2
        //  analogWrite(MOTOR2_IN1, 0);
        //  analogWrite(MOTOR2_IN2, 0);
        //  while (1); //stop the system so I can see failure readout
        //}

        //mySerial.printf("%3.2f\t%3.2f\t%d\t%d\t%d\t%d\t%d\n", 
        //  millis() / 60000.0, global_yawval,motState,motor_speed,fifoCount, GetPacketLoopCount,
            //OuterGetPacketLoopCount);

        //Motor speed/dir management
        switch (motState)
        {
        case MOTOR_FWD_INCREASING:
            // ramp up forward
            digitalWrite(MOTOR1_IN1, LOW);
            digitalWrite(MOTOR2_IN1, LOW);
            motor_speed = (motor_speed > MIN_MOTOR_SPEED) ? motor_speed : MIN_MOTOR_SPEED;
            //if (motor_speed <= MIN_MOTOR_SPEED)
            //{
            //  //Serial.println("give it a whack to start!");
            //  analogWrite(MOTOR1_IN2, MIN_MOTOR_SPEED);
            //  analogWrite(MOTOR2_IN2, MIN_MOTOR_SPEED);
            //}
            //else
            {
                analogWrite(MOTOR1_IN2, motor_speed);
                analogWrite(MOTOR2_IN2, motor_speed);
            }
            motor_speed++;
            if (motor_speed >= MOTOR_SPEED_MAX)
            {
                motState = MOTOR_FWD_DECREASING;
            }
            break;
        case MOTOR_FWD_DECREASING:
            // ramp down forward
            digitalWrite(MOTOR1_IN1, LOW);
            digitalWrite(MOTOR2_IN1, LOW);

            analogWrite(MOTOR1_IN2, motor_speed);
            analogWrite(MOTOR2_IN2, motor_speed);

            motor_speed--;

            if (motor_speed <= MOTOR_SPEED_MIN)
            {
                motState = MOTOR_REV_INCREASING;
            }
            break;
        case MOTOR_REV_INCREASING:
            digitalWrite(MOTOR1_IN2, LOW);
            digitalWrite(MOTOR2_IN2, LOW);

            motor_speed = (motor_speed > MIN_MOTOR_SPEED) ? motor_speed : MIN_MOTOR_SPEED;
            //if (motor_speed <= MIN_MOTOR_SPEED)
            //{
            //  //Serial.println("give it a whack to start!");
            //  analogWrite(MOTOR1_IN1, MIN_MOTOR_SPEED);
            //  analogWrite(MOTOR2_IN1, MIN_MOTOR_SPEED);
            //}
            //else
            {
                analogWrite(MOTOR1_IN1, motor_speed);
                analogWrite(MOTOR2_IN1, motor_speed);
            }

            motor_speed++;

            if (motor_speed >= MOTOR_SPEED_MAX)
            {
                motState = MOTOR_REV_DECREASING;
            }
            break;
        case MOTOR_REV_DECREASING:
            digitalWrite(MOTOR1_IN2, LOW);
            digitalWrite(MOTOR2_IN2, LOW);

            analogWrite(MOTOR1_IN1, motor_speed);
            analogWrite(MOTOR2_IN1, motor_speed);

            motor_speed--;

            if (motor_speed <= MOTOR_SPEED_MIN)
            {
                motState = MOTOR_FWD_INCREASING; //start over
            }
            break;
        default:
            break;
        }

        pgmloops++;
        if (pgmloops == 1000)
        {
            pgmloops = 0;
        }
    }

}


void PrintTelemetryData(int spd)
{

    ////get current, voltage, power readings
    //shuntvoltage = ina219.getShuntVoltage_mV();
    //busvoltage = ina219.getBusVoltage_V();
    //current_mA = ina219.getCurrent_mA();
    //power_mW = ina219.getPower_mW();
    //loadvoltage = busvoltage + (shuntvoltage / 1000);
    //mySerial.printf("%d\t%2.2f\t%2.2f\t%2.2f\n",
    //  spd, loadvoltage, current_mA, power_mW);
    mySerial.printf("%d\n", spd);
}

bool GetIMUHeadingDeg()
{
    //Purpose: Get latest yaw (heading) value from IMU
    //Inputs: None.  This function should only be called after mpu.dmpPacketAvailable() returns TRUE
    //Outputs: 
    //  returns true if successful, otherwise false
    //  global_yawval updated on success
    //Plan:
    //Step1: check for overflow and reset the FIFO if it occurs. In this case, wait for new packet
    //Step2: read all available packets to get to latest data
    //Step3: update global_yawval with latest value
    //Notes:
    //  10/08/19 changed return type to boolean
    //  10/08/19 no longer need mpuIntStatus

    bool retval = false;

    int flag = GetCurrentFIFOPacket(fifoBuffer, packetSize, MAX_GETPACKET_LOOPS); //get the latest mpu packet

    if (flag != 0) //0 = error exit, 1 = normal exit, 2 = recovered from an overflow
    {
        // display Euler angles in degrees
        mpu.dmpGetQuaternion(&q, fifoBuffer);
        mpu.dmpGetGravity(&gravity, &q);
        mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);

        //compute the yaw value
        global_yawval = ypr[0] * 180 / M_PI;

        //if (global_yawval == 180)
        //{
        //  mySerial.printf("erroneous yaw value detected!");
        //  return false;
        //}
        retval = true;
    }

    return retval;
}

uint8_t GetCurrentFIFOPacket(uint8_t* data, uint8_t length, uint16_t max_loops)
{
    max_loops = max(10, max_loops); //added 12/04/19 to ensure we loop at least 10 times. This really IS a hard-coded number!

    fifoCount = mpu.getFIFOCount();

    GetPacketLoopCount = 0;
    int max_outer_loops = 10;

    //11/10/19 I have seen this fail with fifoC > 28 on occasion, so now I loop max 10 times
    OuterGetPacketLoopCount = 0;
    while (fifoCount != packetSize && OuterGetPacketLoopCount < max_outer_loops)
    {
        mpu.resetFIFO();
        delay(1);

        fifoCount = mpu.getFIFOCount();
        GetPacketLoopCount = 0;

        while (fifoCount < packetSize && GetPacketLoopCount < max_loops)
        {
            GetPacketLoopCount++;
            fifoCount = mpu.getFIFOCount();
            delay(2);
        }

        OuterGetPacketLoopCount++;
    }

    //if we get to here, there should be exactly one packet in the FIFO
    if (OuterGetPacketLoopCount < max_outer_loops)
    {
        mpu.getFIFOBytes(data, packetSize);
        return 1;
    }
    return 0; //failed to get a good packet
}
  • I have absolute no knowledge of the MPU6050 and the DMP therein. But, as noone reacted to your question. let me present you an idea. As this cite states "For this DMP, InvenSense has a discouragement policy, by not supplying enough information how to program the DMP. " it is hard for the lib creator (Jeff Rowberg in your case) to reverse engineer the firmware of the DMP. He did it manage but the firmware must be uploaded to the MCU with every start. Now, my idea is, what happens if you restart the Arduino prog. but do not unpower the MCU long enough to clear the memory? (e.g. only reset Ard.) – Peter Paul Kiefer Dec 19 '19 at 15:54
  • There was also a huge rewriting of the DMP firmware that was released in October 2019. But if I understood you correctly, the error occurs long after you have changed to the version "MPU6050_6Axis_MotionApps_V6_12.h". – Peter Paul Kiefer Dec 19 '19 at 15:56
  • I also found out the the Error 1 can only occur, if the comparison of the firmware that was written and the firmware that was read out of the DMP memory after the write were not equal. The comparison is made chunk wise. – Peter Paul Kiefer Dec 19 '19 at 16:02
  • Peter, thanks for the hint. I was able to determine that there is indeed a problem with the memory write process. When I uncommented some debugging code in the memory write function, I got this: Block write verification error, bank 0, address 0! Expected: 0x00 0xF8 0xF6 0x2A 0x3F 0x68 0xF5 0x7A 0x00 0x06 0xFF 0xFE 0x00 0x03 0x00 0x00 Received: 0x00 0xF8 0xF6 0x2A 0x3F 0x68 0xF5 0x7A 0x00 0x06 0xFF 0xFE 0x00 0x03 0x08 0x7B DMP memory write failed. Note the last two bytes differ – user3765883 Dec 19 '19 at 23:32
1

So, it turns out that the problem was located squarely between the programmer's ears, as usual.

As part of another experiment to use an I2C sniffer program to find out why the MPU6050 was failing intermittently, I had added the following line to my setup() function

Wire.setClock(50000);

To slow the I2C clock down enough so that a sniffer program could keep up. Apparently the slower than normal I2C clock speed was enough to screw up the DMP Initialization process.

The funny (weird, not 'ha ha') thing was the way I discovered this was by adding some 'Serial.print()' statements into the loop that writes the DMP firmware onto the DMP chip at each startup. For some reason, the print statements allowed the load to succeed, even with the lower I2C clock speed. Boy was I surprised!

Even weirder is that I could not substitute a 'delay(x)' in place of the print statement; any delay (even as small as 'delay(1);') caused the DMP load to fail, while a print statement worked fine.

Once I saw there was something weird going on with the rate at which the load operations were being executed, I remembered about the 'Wire.SetClock(50000)' command and commented it out to go back to the default I2C rate. Voila` problem solved!

I have NO clue as to why the DMP firmware load is clock speed sensitive, and even less clue why the print statements allowed the load to complete, but I'm back in business now, so I'm going to file this one in the "Who knew?" category and move on ;-)

Frank

| improve this answer | |

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service, privacy policy and cookie policy

Not the answer you're looking for? Browse other questions tagged or ask your own question.