Почему MPU6050 DMP не инициализируется, но я могу получить необработанные значения ускорения от MPU6050

Я уже некоторое время использую модули MPU6050 для измерения рыскания с помощью встроенного чипа DMP. Все шло гладко до вчерашнего дня, когда мой код инициализации начал сообщать, что я могу подключиться к своему чипу MPU6050, но инициализация DMP не удалась с кодом 1.

Подумав, что я каким-то образом сжёг чип DMP (не знаю как, так как прямого подключения нет), я заменил модуль, но получил тот же результат, но с тремя разными модулями MPU6050.

В качестве эксперимента я модифицировал свой код, чтобы он считывал необработанные значения ускорения по оси Z вместо кватернионов DMP, и это было успешно. Нанесение значений в Excel показало, что значения реагировали, как и ожидалось, когда я вручную вращал датчик вокруг оси Z.

Теперь я знаю, что могу подключиться к модулю MPU6050 и считывать показания датчиков, но каким-то образом потерял возможность использовать DMP. Есть идеи, почему это может происходить?

Вот две версии моего кода. Первая версия является исходной, а вторая была изменена, чтобы просто отображать значения ускорения по оси Z.

ТИА

Фрэнк

Версия 1 (исходная):

// Visual Micro находится в vMicro>General>Обучающем режиме
//
/*
    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> //позволяет распечатывать синтаксис в стиле printf
#include "MPU6050_6Axis_MotionApps_V6_12.h"  //изменено на эту версию 05.10.19
#include "I2Cdev.h" //19.02.19: сюда входит SBWire.h
#include <NewPing.h> //добавлено 15.01.15
#pragma endregion INCLUDES

StreamEx mySerial = Serial; //добавлено 18.03.18 для печати в стиле printf
elapsedMillis   sinceLastNavUpdateMsec; //добавлено 15.10.18 вместо lastmillisec

//MPU6050 mpu(0x69); //23.06.18 chg to AD0 high addr на двухмоторном роботе
MPU6050 mpu(0x68); //23.06.18 chg to AD0 high addr на двухмоторном роботе

void(*resetFunc) (void) = 0; //добавлено 08.12.19 для решения проблемы сбоя сброса MPU6050/I2C

#pragma region MPU6050_SUPPORT
uint8_t mpuIntStatus;   // содержит фактический байт состояния прерывания от MPU. Используется в рутине Гомера «Переполнение».
uint8_t devStatus;      // возвращаем статус после каждой операции с устройством (0 = успех, !0 = ошибка)
uint16_t packetSize;    // ожидаемый размер пакета DMP (по умолчанию 42 байта)
uint16_t fifoCount;     // подсчет всех байтов в настоящее время в FIFO
uint8_t fifoBuffer[64]; // Буфер хранения FIFO

                        // переменные ориентации/движения
Quaternion q;           // [w, x, y, z] контейнер кватернионов
VectorInt16 aa;         // [x, y, z] измерения датчика ускорения
VectorInt16 aaReal;     // [x, y, z] измерения датчика ускорения без гравитации
VectorInt16 aaWorld;    // [x, y, z] измерения датчика ускорения мирового кадра
VectorFloat gravity;    // [x, y, z] вектор гравитации
float euler[3];         // [пси, тета, фи] Контейнер угла Эйлера
float ypr[3];           // [рыскание, тангаж, крен] контейнер рыскания/тангажа/крена и вектор гравитации
int GetPacketLoopCount = 0;
int OuterGetPacketLoopCount = 0;

// флаги состояния RTC/FRAM/MPU6050
bool bMPU6050Ready = true;
bool dmpReady = false;  // устанавливаем true, если инициализация DMP прошла успешно
volatile float global_yawval = 0; //обновлено GetIMUHeadingDeg()
const uint16_t MAX_GETPACKET_LOOPS = 100; //30.10.19 добавлено условие выхода из цикла резервного копирования в GetCurrentFIFOPacket()
uint8_t GetCurrentFIFOPacket(uint8_t* data, uint8_t length, uint16_t max_loops = MAX_GETPACKET_LOOPS); //прототип здесь, чтобы можно было определить параметр по умолчанию
bool bFirstTime = true;
#pragma endregion MPU6050 Support




// Базовый скетч для опробования 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; //определено опытным путем

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; //определено опытным путем
int motor_speed = 0;
int pgmloops = 0;


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

    // инициализируем MPU6050 добавлено 03.09.18
    Serial.println(F("Initializing MPU6050 ..."));
    mpu.initialize();

    // проверяем соединение
    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();

    // укажите здесь собственные смещения гироскопа, масштабированные для минимальной чувствительности
    ////12/03/19 настройки из процедур калибровки
    //mpu.setXGyroOffset(58);
    //mpu.setYGyroOffset(20);
    //mpu.setZGyroOffset(12);
    //mpu.setXAccelOffset(1618);
    //mpu.setYAccelOffset(3046);
    //mpu.setZAccelOffset(4554);

    //12/16/19 настройки из процедур калибровки
    mpu.setXAccelOffset(-3066);
    mpu.setYAccelOffset(2658);
    mpu.setZAccelOffset(872);
    mpu.setXGyroOffset(167);
    mpu.setYGyroOffset(-26);
    mpu.setZGyroOffset(73);

    // убедиться, что это сработало (возвращает 0 в случае успеха)
    if (devStatus == 0)
    {
        //06.12.19 больше не нужен; Калибровка уже приобретена
        // Время калибровки: генерируем смещения и калибруем наш MPU6050
        //mpu.CalibrateAccel(6);
        //mpu.CalibrateGyro(6);
        //Серийный.println();
        //mpu.PrintActiveOffsets();

        // включаем DMP, теперь, когда он готов
        Serial.println(F("Enabling DMP..."));
        mpu.setDMPEnabled(true);

        // устанавливаем наш флаг готовности DMP, чтобы основная функция loop() знала, что ее можно использовать
        Serial.println(F("DMP ready! Waiting for MPU6050 drift rate to settle..."));
        dmpReady = true;

        // получаем ожидаемый размер пакета DMP для последующего сравнения
        packetSize = mpu.dmpGetFIFOPacketSize();
        bMPU6050Ready = true;
        mySerial.printf("\nMPU6050 Ready at %2.2f Sec\n", millis() / 1000.f);
    }
    else
    {
        // ОШИБКА!
        // 1 = первоначальная загрузка памяти не удалась
        // 2 = не удалось обновить конфигурацию DMP
        // (если он сломается, обычно код будет 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("Скорость\tVolts\tMAmps\tMWatts\n"); //заголовки столбцов
    mySerial.printf("Speed\n"); //заголовки столбцов

    TCCR2B = (TCCR2B & B11111000) | B00000011; // для частоты ШИМ 980,39 Гц на выводах 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;

        //10.12.19 добавлен тест подключения и сброс глобальных параметров 'GetPacket'
        bool getYawResult = false;
        fifoCount = 0;
        GetPacketLoopCount = 0;
        OuterGetPacketLoopCount = 0;
        global_yawval = 999;
        if (mpu.testConnection())
        {
            getYawResult = GetIMUHeadingDeg(); //обновляет global_yawval в случае успеха
        }
        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(); // это должно полностью перезагрузить Arduino, чтобы мы могли начать сначала

            //остановить мотор 1
            analogWrite(MOTOR1_IN1, 0);
            analogWrite(MOTOR1_IN2, 0);

            // остановить двигатель 2
            analogWrite(MOTOR2_IN1, 0);
            analogWrite(MOTOR2_IN2, 0);
            while (1); // остановим систему, чтобы я мог увидеть индикацию ошибки
        }

        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);

        //Управление скоростью двигателя/направлением
        switch (motState)
        {
        case MOTOR_FWD_INCREASING:
            // ускорение вперед
            digitalWrite(MOTOR1_IN1, LOW);
            digitalWrite(MOTOR2_IN1, LOW);
            motor_speed = (motor_speed > MIN_MOTOR_SPEED) ? motor_speed : MIN_MOTOR_SPEED;
            //если (motor_speed <= MIN_MOTOR_SPEED)
            //{
            // //Serial.println("Ударь его, чтобы начать!");
            // аналоговая запись (MOTOR1_IN2, MIN_MOTOR_SPEED);
            // аналоговая запись (MOTOR2_IN2, MIN_MOTOR_SPEED);
            //}
            //еще
            {
                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:
            // замедление вперед
            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;
            //если (motor_speed <= MIN_MOTOR_SPEED)
            //{
            // //Serial.println("Ударь его, чтобы начать!");
            // аналоговая запись (MOTOR1_IN1, MIN_MOTOR_SPEED);
            // аналоговая запись (MOTOR2_IN1, MIN_MOTOR_SPEED);
            //}
            //еще
            {
                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; //начать сначала
            }
            break;
        default:
            break;
        }

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

}


void PrintTelemetryData(int spd)
{

    ////получить показания тока, напряжения, мощности
    //shuntvoltage = ina219.getShuntVoltage_mV();
    //busvoltage = ina219.getBusVoltage_V();
    //current_mA = ina219.getCurrent_mA();
    //power_mW = ina219.getPower_mW();
    //напряжение нагрузки = напряжение шины + (напряжение шунта / 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()
{
    //Цель: Получить последнее значение рыскания (курса) от IMU
    //Входы: нет. Эту функцию следует вызывать только после того, как mpu.dmpPacketAvailable() вернет TRUE.
    //Выводы:
    // возвращает true в случае успеха, иначе false
    // global_yawval обновляется в случае успеха
    //План:
    //Шаг 1: проверьте наличие переполнения и сбросьте FIFO, если это произойдет. В этом случае дождитесь нового пакета
    //Шаг 2: прочитать все доступные пакеты, чтобы получить последние данные
    //Шаг 3: обновить global_yawval последним значением
    //Заметки:
    // 08.10.19 изменил тип возвращаемого значения на логический
    // 08.10.19 больше не нужен mpuIntStatus

    bool retval = false;

    int flag = GetCurrentFIFOPacket(fifoBuffer, packetSize, MAX_GETPACKET_LOOPS); //получить последний пакет mpu

    if (flag != 0) //0 = выход с ошибкой, 1 = нормальный выход, 2 = восстановление после переполнения
    {
        // отображаем углы Эйлера в градусах
        mpu.dmpGetQuaternion(&q, fifoBuffer);
        mpu.dmpGetGravity(&gravity, &q);
        mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);

        // вычисляем значение рыскания
        global_yawval = ypr[0] * 180 / M_PI;

        //если (global_yawval == 180)
        //{
        // mySerial.printf("Обнаружено ошибочное значение рыскания!");
        // вернуть ложь;
        //}
        retval = true;
    }

    return retval;
}

uint8_t GetCurrentFIFOPacket(uint8_t* data, uint8_t length, uint16_t max_loops)
{
    max_loops = max(10, max_loops); // добавлено 04.12.19, чтобы гарантировать, что мы зациклимся не менее 10 раз. Это действительно жестко запрограммированный номер!

    fifoCount = mpu.getFIFOCount();

    GetPacketLoopCount = 0;
    int max_outer_loops = 10;

    //10.11.19 Я видел этот сбой с fifoC > Иногда 28, так что теперь я зацикливаюсь максимум 10 раз.
    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++;
    }

    // если мы дойдем до этого места, то в FIFO должен быть ровно один пакет
    if (OuterGetPacketLoopCount < max_outer_loops)
    {
        mpu.getFIFOBytes(data, packetSize);
        return 1;
    }
    return 0; //не удалось получить хороший пакет
}

Версия 2 (изменено для чтения необработанного ускорения по оси Z:

// Visual Micro находится в vMicro>General>Обучающем режиме
//
/*
    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> //позволяет распечатывать синтаксис в стиле printf
#include "MPU6050_6Axis_MotionApps_V6_12.h"  //изменено на эту версию 05.10.19
#include "I2Cdev.h" //19.02.19: сюда входит SBWire.h
#include <NewPing.h> //добавлено 15.01.15
#pragma endregion INCLUDES

StreamEx mySerial = Serial; //добавлено 18.03.18 для печати в стиле printf
elapsedMillis   sinceLastNavUpdateMsec; //добавлено 15.10.18 вместо lastmillisec

//MPU6050 mpu(0x69); //23.06.18 chg to AD0 high addr на двухмоторном роботе
MPU6050 mpu(0x68); //23.06.18 chg to AD0 high addr на двухмоторном роботе

void(*resetFunc) (void) = 0; //добавлено 08.12.19 для решения проблемы сбоя сброса MPU6050/I2C

#pragma region MPU6050_SUPPORT
uint8_t mpuIntStatus;   // содержит фактический байт состояния прерывания от MPU. Используется в рутине Гомера «Переполнение».
uint8_t devStatus;      // возвращаем статус после каждой операции с устройством (0 = успех, !0 = ошибка)
uint16_t packetSize;    // ожидаемый размер пакета DMP (по умолчанию 42 байта)
uint16_t fifoCount;     // подсчет всех байтов в настоящее время в FIFO
uint8_t fifoBuffer[64]; // Буфер хранения FIFO

                        // переменные ориентации/движения
Quaternion q;           // [w, x, y, z] контейнер кватернионов
VectorInt16 aa;         // [x, y, z] измерения датчика ускорения
VectorInt16 aaReal;     // [x, y, z] измерения датчика ускорения без гравитации
VectorInt16 aaWorld;    // [x, y, z] измерения датчика ускорения мирового кадра
VectorFloat gravity;    // [x, y, z] вектор гравитации
float euler[3];         // [пси, тета, фи] Контейнер угла Эйлера
float ypr[3];           // [рыскание, тангаж, крен] контейнер рыскания/тангажа/крена и вектор гравитации
int GetPacketLoopCount = 0;
int OuterGetPacketLoopCount = 0;

// флаги состояния RTC/FRAM/MPU6050
bool bMPU6050Ready = true;
bool dmpReady = false;  // устанавливаем true, если инициализация DMP прошла успешно
volatile float global_yawval = 0; //обновлено GetIMUHeadingDeg()
const uint16_t MAX_GETPACKET_LOOPS = 100; //30.10.19 добавлено условие выхода из цикла резервного копирования в GetCurrentFIFOPacket()
uint8_t GetCurrentFIFOPacket(uint8_t* data, uint8_t length, uint16_t max_loops = MAX_GETPACKET_LOOPS); //прототип здесь, чтобы можно было определить параметр по умолчанию
bool bFirstTime = true;
#pragma endregion MPU6050 Support




// Базовый скетч для опробования 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; //определено опытным путем

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; //определено опытным путем
int motor_speed = 0;
int pgmloops = 0;


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

    // инициализируем MPU6050 добавлено 03.09.18
    Serial.println(F("Initializing MPU6050 ..."));
    mpu.initialize();

    // проверяем соединение
    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();

    // укажите здесь собственные смещения гироскопа, масштабированные для минимальной чувствительности
    ////12/03/19 настройки из процедур калибровки
    //mpu.setXGyroOffset(58);
    //mpu.setYGyroOffset(20);
    //mpu.setZGyroOffset(12);
    //mpu.setXAccelOffset(1618);
    //mpu.setYAccelOffset(3046);
    //mpu.setZAccelOffset(4554);

    //12/16/19 настройки из процедур калибровки
    mpu.setXAccelOffset(-3066);
    mpu.setYAccelOffset(2658);
    mpu.setZAccelOffset(872);
    mpu.setXGyroOffset(167);
    mpu.setYGyroOffset(-26);
    mpu.setZGyroOffset(73);

    // убедиться, что это сработало (возвращает 0 в случае успеха)
    if (devStatus == 0)
    {
        //06.12.19 больше не нужен; Калибровка уже приобретена
        // Время калибровки: генерируем смещения и калибруем наш MPU6050
        //mpu.CalibrateAccel(6);
        //mpu.CalibrateGyro(6);
        //Серийный.println();
        //mpu.PrintActiveOffsets();

        // включаем DMP, теперь, когда он готов
        Serial.println(F("Enabling DMP..."));
        mpu.setDMPEnabled(true);

        // устанавливаем наш флаг готовности DMP, чтобы основная функция loop() знала, что ее можно использовать
        Serial.println(F("DMP ready! Waiting for MPU6050 drift rate to settle..."));
        dmpReady = true;

        // получаем ожидаемый размер пакета DMP для последующего сравнения
        packetSize = mpu.dmpGetFIFOPacketSize();
        bMPU6050Ready = true;
        mySerial.printf("\nMPU6050 Ready at %2.2f Sec\n", millis() / 1000.f);
    }
    else
    {
        // ОШИБКА!
        // 1 = первоначальная загрузка памяти не удалась
        // 2 = не удалось обновить конфигурацию DMP
        // (если он сломается, обычно код будет 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("Скорость\tVolts\tMAmps\tMWatts\n"); //заголовки столбцов
    mySerial.printf("Speed\n"); //заголовки столбцов

    TCCR2B = (TCCR2B & B11111000) | B00000011; // для частоты ШИМ 980,39 Гц на выводах 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;

        //10.12.19 добавлен тест подключения и сброс глобальных параметров 'GetPacket'
        bool getYawResult = false;
        fifoCount = 0;
        GetPacketLoopCount = 0;
        OuterGetPacketLoopCount = 0;
        global_yawval = 999;
        if (mpu.testConnection())
        {
            getYawResult = GetIMUHeadingDeg(); //обновляет global_yawval в случае успеха
            float Zaccel = mpu.getAccelerationZ();
            mySerial.printf("Zaccel = %3.2f\n", Zaccel);
        }
        else
        {
            mySerial.printf("MPU6050 Not Connected!\n");
        }

        //если (получитьYawResult == 0)
        //{
        // mySerial.printf("Сбой MPU6050 и/или I2C - Остановка теста!\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(); // это должно полностью перезагрузить Arduino, чтобы мы могли начать сначала

        // // остановить мотор 1
        // аналоговая запись (MOTOR1_IN1, 0);
        // аналоговая запись (MOTOR1_IN2, 0);

        // // остановить двигатель 2
        // аналоговая запись (MOTOR2_IN1, 0);
        // аналоговая запись (MOTOR2_IN2, 0);
        // пока (1); // остановим систему, чтобы я мог увидеть индикацию ошибки
        //}

        //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);

        //Управление скоростью двигателя/направлением
        switch (motState)
        {
        case MOTOR_FWD_INCREASING:
            // ускорение вперед
            digitalWrite(MOTOR1_IN1, LOW);
            digitalWrite(MOTOR2_IN1, LOW);
            motor_speed = (motor_speed > MIN_MOTOR_SPEED) ? motor_speed : MIN_MOTOR_SPEED;
            //если (motor_speed <= MIN_MOTOR_SPEED)
            //{
            // //Serial.println("Ударь его, чтобы начать!");
            // аналоговая запись (MOTOR1_IN2, MIN_MOTOR_SPEED);
            // аналоговая запись (MOTOR2_IN2, MIN_MOTOR_SPEED);
            //}
            //еще
            {
                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:
            // замедление вперед
            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;
            //если (motor_speed <= MIN_MOTOR_SPEED)
            //{
            // //Serial.println("Ударь его, чтобы начать!");
            // аналоговая запись (MOTOR1_IN1, MIN_MOTOR_SPEED);
            // аналоговая запись (MOTOR2_IN1, MIN_MOTOR_SPEED);
            //}
            //еще
            {
                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; //начать сначала
            }
            break;
        default:
            break;
        }

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

}


void PrintTelemetryData(int spd)
{

    ////получить показания тока, напряжения, мощности
    //shuntvoltage = ina219.getShuntVoltage_mV();
    //busvoltage = ina219.getBusVoltage_V();
    //current_mA = ina219.getCurrent_mA();
    //power_mW = ina219.getPower_mW();
    //напряжение нагрузки = напряжение шины + (напряжение шунта / 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()
{
    //Цель: Получить последнее значение рыскания (курса) от IMU
    //Входы: нет. Эту функцию следует вызывать только после того, как mpu.dmpPacketAvailable() вернет TRUE.
    //Выводы:
    // возвращает true в случае успеха, иначе false
    // global_yawval обновляется в случае успеха
    //План:
    //Шаг 1: проверьте наличие переполнения и сбросьте FIFO, если это произойдет. В этом случае дождитесь нового пакета
    //Шаг 2: прочитать все доступные пакеты, чтобы получить последние данные
    //Шаг 3: обновить global_yawval последним значением
    //Заметки:
    // 08.10.19 изменил тип возвращаемого значения на логический
    // 08.10.19 больше не нужен mpuIntStatus

    bool retval = false;

    int flag = GetCurrentFIFOPacket(fifoBuffer, packetSize, MAX_GETPACKET_LOOPS); //получить последний пакет mpu

    if (flag != 0) //0 = выход с ошибкой, 1 = нормальный выход, 2 = восстановление после переполнения
    {
        // отображаем углы Эйлера в градусах
        mpu.dmpGetQuaternion(&q, fifoBuffer);
        mpu.dmpGetGravity(&gravity, &q);
        mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);

        // вычисляем значение рыскания
        global_yawval = ypr[0] * 180 / M_PI;

        //если (global_yawval == 180)
        //{
        // mySerial.printf("Обнаружено ошибочное значение рыскания!");
        // вернуть ложь;
        //}
        retval = true;
    }

    return retval;
}

uint8_t GetCurrentFIFOPacket(uint8_t* data, uint8_t length, uint16_t max_loops)
{
    max_loops = max(10, max_loops); // добавлено 04.12.19, чтобы гарантировать, что мы зациклимся не менее 10 раз. Это действительно жестко запрограммированный номер!

    fifoCount = mpu.getFIFOCount();

    GetPacketLoopCount = 0;
    int max_outer_loops = 10;

    //10.11.19 Я видел этот сбой с fifoC > Иногда 28, так что теперь я зацикливаюсь максимум 10 раз.
    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++;
    }

    // если мы дойдем до этого места, то в FIFO должен быть ровно один пакет
    if (OuterGetPacketLoopCount < max_outer_loops)
    {
        mpu.getFIFOBytes(data, packetSize);
        return 1;
    }
    return 0; //не удалось получить хороший пакет
}

, 👍1

Обсуждение

Я абсолютно ничего не знаю о MPU6050 и DMP в нем. Но, как никто не отреагировал на ваш вопрос. позвольте мне представить вам идею. Как говорится в этой цитате: «Для этого DMP InvenSense придерживается политики разочаровывания, не предоставляя достаточно информации о том, как программировать DMP». Создателю библиотеки (в вашем случае Джеффу Роубергу) сложно перепроектировать прошивку DMP. У него это получилось но прошивку надо заливать в MCU при каждом запуске. Теперь моя идея заключается в том, что произойдет, если вы перезапустите программу Arduino. но не отключать MCU достаточно долго, чтобы очистить память? (например, только сбросить Ard.), @Peter Paul Kiefer

Также была проведена огромная переделка прошивки DMP, которая вышла в октябре 2019 года. Но если я вас правильно понял, то ошибка возникает еще долго после того, как вы перешли на версию "MPU6050_6Axis_MotionApps_V6_12.h"., @Peter Paul Kiefer

Я также обнаружил, что ошибка 1 может возникнуть только в том случае, если сравнение прошивки, которая была записана, и прошивки, которая была считана из памяти DMP после записи, не были равны. Сравнение производится по частям., @Peter Paul Kiefer

Петр, спасибо за подсказку. Я смог определить, что действительно существует проблема с процессом записи в память. Когда я раскомментировал некоторый отладочный код в функции записи в память, я получил следующее: Ошибка проверки записи блока, банк 0, адрес 0! Ожидается: 0x00 0xF8 0xF6 0x2A 0x3F 0x68 0xF5 0x7A 0x00 0x06 0xFF 0xFE 0x00 0x03 0x00 0x00 Получено: 0x00 0xF8 0xF6 0x2A 0x3F 0x68 0xF5 0x7A 0x00 0x06 0xFF 0xFE 0x00 0x03 0x08 0x7B Ошибка записи в память DMP. Обратите внимание, что последние два байта отличаются, @user3765883


1 ответ


1

Итак, получается, что проблема, как обычно, находилась прямо между ушами программиста.

В рамках другого эксперимента по использованию программы-сниффера I2C для выяснения причин периодических сбоев MPU6050 я добавил следующую строку в свою функцию setup()

Wire.setClock(50000);

Чтобы замедлить часы I2C настолько, чтобы программа-сниффер могла не отставать. Очевидно, более низкой, чем обычно, тактовой частоты I2C было достаточно, чтобы испортить процесс инициализации DMP.

Забавно (странно, а не «ха-ха»), что я обнаружил это, добавив несколько операторов «Serial.print()» в цикл, который записывает прошивку DMP на микросхему DMP при каждом запуске. По какой-то причине операторы печати позволили выполнить загрузку даже при более низкой тактовой частоте I2C. Боже, я был удивлен!

Еще более странно то, что я не мог заменить оператор печати на 'delay(x)'; любая задержка (даже такая маленькая, как 'delay(1);') приводила к сбою загрузки DMP, в то время как оператор печати работал нормально.

Как только я увидел, что происходит что-то странное со скоростью выполнения операций загрузки, я вспомнил о команде Wire.SetClock(50000) и закомментировал ее, чтобы вернуться к скорости I2C по умолчанию. Вуаля! Проблема решена!

Я понятия не имею, почему загрузка микропрограммы DMP чувствительна к тактовой частоте, и еще меньше знаю, почему операторы печати позволили завершить загрузку, но теперь я снова в деле, поэтому я собираюсь подать это в файл. один в "Кто знал?" категорию и двигаться дальше ;-)

Фрэнк

,