Почему 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; //не удалось получить хороший пакет
}
@user3765883, 👍1
Обсуждение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 чувствительна к тактовой частоте, и еще меньше знаю, почему операторы печати позволили завершить загрузку, но теперь я снова в деле, поэтому я собираюсь подать это в файл. один в "Кто знал?" категорию и двигаться дальше ;-)
Фрэнк
,
@user3765883
Смотрите также:
- Как очистить буфер FIFO на MPU6050?
- Взаимодействие MPU6050 с Arduino через S-функцию Simulink
- Проводка для Arduino Mega и нескольких MPU 6050/Gy 521
- Проблема прерывания библиотеки MPU6050 Arduino Jeff Rowberg
- Проблема с библиотекой MPU6050
- MPU6050/DMP Прерывистый плохой вывод данных?
- Гироскоп MPU6050 не дает надежных показаний
- Невозможно заставить ЖК-дисплей и гироскоп работать вместе.
Я абсолютно ничего не знаю о 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