https://pastein.ru/t/VSZ
Загрузка данных
[span_4](start_span)#include "FastIMU.h"[span_4](end_span)
[span_5](start_span)#include "Madgwick.h"[span_5](end_span)
[span_6](start_span)#include "EEPROM.h"[span_6](end_span)
[span_7](start_span)#include <Wire.h>[span_7](end_span)
[span_8](start_span)#include "HID.h" // Используем стандартный HID для STM32[span_8](end_span)
// Адрес твоего MPU6050
[span_9](start_span)#define IMU_ADDRESS 0x68[span_9](end_span)
[span_10](start_span)MPU6050 IMU;[span_10](end_span)
// Стандартная геометрия из твоего файла
[span_11](start_span)#define IMU_GEOMETRY 0[span_11](end_span)
[span_12](start_span)#define FILTER_MAX_BETA 0.15[span_12](end_span)
[span_13](start_span)#define FILTER_MIN_BETA 0.015[span_13](end_span)
[span_14](start_span)#define FILTER_DROPOFF 0.85[span_14](end_span)
[span_15](start_span)calData calib = { 0 };[span_15](end_span)
[span_16](start_span)AccelData IMUAccel;[span_16](end_span)
[span_17](start_span)GyroData IMUGyro;[span_17](end_span)
[span_18](start_span)MagData IMUMag;[span_18](end_span)
[span_19](start_span)GyroData GyroVel;[span_19](end_span)
[span_20](start_span)Madgwick filter;[span_20](end_span)
[span_21](start_span)bool flag = false;[span_21](end_span)
// Тот самый HID-дескриптор, который ждет драйвер
[span_22](start_span)static const uint8_t _hidReportDescriptor[] PROGMEM = {[span_22](end_span)
[span_23](start_span)0x06, 0x03, 0x00, 0x09, 0x00, 0xa1, 0x01, 0x15, 0x00, 0x26, 0xff, 0x00,[span_23](end_span)
[span_24](start_span)0x85, 0x01, 0x75, 0x08, 0x95, 0x3f, 0x09, 0x00, 0x81, 0x02, 0xc0[span_24](end_span)
[span_25](start_span)};[span_25](end_span)
[span_26](start_span)float quat[4];[span_26](end_span)
uint8_t reportBuffer[63]; // Буфер, чтобы безопасно отправлять 63 байта
void setup() {
// Настройка I2C специально для STM32 Blue Pill
Wire.setSDA(PB7);
Wire.setSCL(PB6);
[span_27](start_span)Wire.begin();[span_27](end_span)
[span_28](start_span)Wire.setClock(400000);[span_28](end_span)
[span_29](start_span)int calStatus = 0;[span_29](end_span)
Serial.begin(115200); // 115200 стабильнее для STM32
[span_30](start_span)// Регистрация HID[span_30](end_span)
[span_31](start_span)static HIDSubDescriptor node (_hidReportDescriptor, sizeof(_hidReportDescriptor));[span_31](end_span)
[span_32](start_span)HID().AppendDescriptor(&node);[span_32](end_span)
[span_33](start_span)EEPROM.get(100, calStatus);[span_33](end_span)
[span_34](start_span)if (calStatus == 99) {[span_34](end_span)
[span_35](start_span)EEPROM.get(200, calib);[span_35](end_span)
[span_36](start_span)}
IMU.setIMUGeometry(IMU_GEOMETRY);[span_36](end_span)
[span_37](start_span)int err = IMU.init(calib, IMU_ADDRESS);[span_37](end_span)
[span_38](start_span)if (err != 0) {[span_38](end_span)
while(!Serial) { delay(10); [span_39](start_span)}
Serial.print("Error initializing IMU! e:");[span_39](end_span)
[span_40](start_span)Serial.println(err);[span_40](end_span)
while (true) { delay(100); [span_41](start_span)}
}[span_41](end_span)
[span_42](start_span)filter.begin(2.f);[span_42](end_span)
[span_43](start_span)for (int i = 0; i < 2000; i++) {[span_43](end_span)
[span_44](start_span)IMU.update();[span_44](end_span)
[span_45](start_span)IMU.getAccel(&IMUAccel);[span_45](end_span)
[span_46](start_span)IMU.getGyro(&IMUGyro);[span_46](end_span)
[span_47](start_span)if (IMU.hasMagnetometer()) {[span_47](end_span)
[span_48](start_span)IMU.getMag(&IMUMag);[span_48](end_span)
[span_49](start_span)filter.update(IMUGyro.gyroX, IMUGyro.gyroY, IMUGyro.gyroZ, IMUAccel.accelX, IMUAccel.accelY, IMUAccel.accelZ, IMUMag.magX, IMUMag.magY, IMUMag.magZ);[span_49](end_span)
[span_50](start_span)} else {[span_50](end_span)
[span_51](start_span)filter.updateIMU(IMUGyro.gyroX, IMUGyro.gyroY, IMUGyro.gyroZ, IMUAccel.accelX, IMUAccel.accelY, IMUAccel.accelZ);[span_51](end_span)
[span_52](start_span)}
}[span_52](end_span)
}
void loop() {
[span_53](start_span)if (Serial) {[span_53](end_span)
[span_54](start_span)if (!flag) {[span_54](end_span)
[span_55](start_span)Serial.println("Serial monitor open, do you want to enter calibration mode? (y/n)");[span_55](end_span)
[span_56](start_span)}
flag = true;[span_56](end_span)
// Добавлена проверка available, чтобы избежать случайного запуска
[span_57](start_span)if (Serial.available() > 0 && Serial.read() == 'y') {[span_57](end_span)
[span_58](start_span)calib = { 0 };[span_58](end_span)
[span_59](start_span)IMU.init(calib, IMU_ADDRESS);[span_59](end_span)
[span_60](start_span)Serial.println("Calibrating IMU... Keep headset still on a flat and level surface...");[span_60](end_span)
[span_61](start_span)delay(10000);[span_61](end_span)
[span_62](start_span)IMU.calibrateAccelGyro(&calib);[span_62](end_span)
[span_63](start_span)IMU.init(calib, IMU_ADDRESS);[span_63](end_span)
[span_64](start_span)Serial.println("Accelerometer and Gyroscope calibrated!");[span_64](end_span)
[span_65](start_span)if (IMU.hasMagnetometer()) {[span_65](end_span)
[span_66](start_span)delay(1000);[span_66](end_span)
[span_67](start_span)Serial.println("Magnetometer calibration: move IMU in figure 8 pattern until done.");[span_67](end_span)
[span_68](start_span)delay(5000);[span_68](end_span)
[span_69](start_span)IMU.calibrateMag(&calib);[span_69](end_span)
[span_70](start_span)Serial.println("Magnetic calibration done!");[span_70](end_span)
[span_71](start_span)}
// Я убрал вывод огромного количества логов в порт, чтобы не тормозить процесс
Serial.println("IMU Calibration complete!");[span_71](end_span)
[span_72](start_span)Serial.println("Saving Calibration values to EEPROM!");[span_72](end_span)
[span_73](start_span)EEPROM.put(200, calib);[span_73](end_span)
[span_74](start_span)EEPROM.put(100, 99);[span_74](end_span)
[span_75](start_span)delay(1000);[span_75](end_span)
[span_76](start_span)Serial.println("Please remember to set hmdIMUdmpPackets to false in the driver settings.");[span_76](end_span)
[span_77](start_span)Serial.println("You can now close the Serial monitor.");[span_77](end_span)
[span_78](start_span)delay(2000);[span_78](end_span)
// Важно для STM32: аппаратный сброс после записи калибровки
NVIC_SystemReset();
[span_79](start_span)}
}[span_79](end_span)
[span_80](start_span)IMU.update();[span_80](end_span)
[span_81](start_span)IMU.getAccel(&IMUAccel);[span_81](end_span)
[span_82](start_span)IMU.getGyro(&IMUGyro);[span_82](end_span)
[span_83](start_span)float Av = GyroVel.gyroX * GyroVel.gyroX + GyroVel.gyroY * GyroVel.gyroY + GyroVel.gyroZ * GyroVel.gyroZ;[span_83](end_span)
[span_84](start_span)if (Av > 100.f) Av = 100.f;[span_84](end_span)
[span_85](start_span)filter.changeBeta(Av * (FILTER_MAX_BETA - FILTER_MIN_BETA) / 100 + FILTER_MIN_BETA);[span_85](end_span)
[span_86](start_span)if (IMU.hasMagnetometer()) {[span_86](end_span)
[span_87](start_span)IMU.getMag(&IMUMag);[span_87](end_span)
[span_88](start_span)filter.update(IMUGyro.gyroX, IMUGyro.gyroY, IMUGyro.gyroZ, IMUAccel.accelX, IMUAccel.accelY, IMUAccel.accelZ, IMUMag.magX, IMUMag.magY, IMUMag.magZ);[span_88](end_span)
[span_89](start_span)} else {[span_89](end_span)
[span_90](start_span)filter.updateIMU(IMUGyro.gyroX, IMUGyro.gyroY, IMUGyro.gyroZ, IMUAccel.accelX, IMUAccel.accelY, IMUAccel.accelZ);[span_90](end_span)
[span_91](start_span)}
GyroVel.gyroX += IMUGyro.gyroX * filter.delta_t;[span_91](end_span)
[span_92](start_span)GyroVel.gyroY += IMUGyro.gyroY * filter.delta_t;[span_92](end_span)
[span_93](start_span)GyroVel.gyroZ += IMUGyro.gyroZ * filter.delta_t;[span_93](end_span)
[span_94](start_span)GyroVel.gyroX *= FILTER_DROPOFF;[span_94](end_span)
[span_95](start_span)GyroVel.gyroY *= FILTER_DROPOFF;[span_95](end_span)
[span_96](start_span)GyroVel.gyroZ *= FILTER_DROPOFF;[span_96](end_span)
[span_97](start_span)quat[0] = filter.getQuatW();[span_97](end_span)
[span_98](start_span)quat[1] = filter.getQuatY();[span_98](end_span)
[span_99](start_span)quat[2] = filter.getQuatZ();[span_99](end_span)
[span_100](start_span)quat[3] = filter.getQuatX();[span_100](end_span)
// Зануляем буфер и копируем туда 16 байт (4 флоата)
memset(reportBuffer, 0, sizeof(reportBuffer));
memcpy(reportBuffer, quat, sizeof(quat));
// Отправляем буфер в USB-HID
[span_101](start_span)HID().SendReport(1, reportBuffer, 63);[span_101](end_span)
}
