Справка считывание ориентации с гироскопа
Недавно я купил трехосевой гироскоп L3GD20H, чтобы использовать его для измерения ориентации, но обнаружил, что вместо этого он сообщает скорость вращения в градусах в секунду. Мне сказали, что это означает, что для определения полных градусов вращения мне нужно интегрировать скорость вращения во времени. Мне было интересно, может ли кто-нибудь помочь мне с этим, так как у меня возникли проблемы с тем, чтобы обернуть свою голову вокруг формулы для создания. вот мой код до сих пор, если это поможет:
#include <Wire.h>
#include <Adafruit_Sensor.h>
#include <Adafruit_L3GD20_U.h>
/* Одновременно назначьте этому датчику уникальный идентификатор */
Adafruit_L3GD20_Unified gyro = Adafruit_L3GD20_Unified(20);
void displaySensorDetails(void)
{
sensor_t sensor;
gyro.getSensor(&sensor);
Serial.println("------------------------------------");
Serial.print ("Sensor: "); Serial.println(sensor.name);
Serial.print ("Driver Ver: "); Serial.println(sensor.version);
Serial.print ("Unique ID: "); Serial.println(sensor.sensor_id);
Serial.print ("Max Value: "); Serial.print(sensor.max_value); Serial.println(" rad/s");
Serial.print ("Min Value: "); Serial.print(sensor.min_value); Serial.println(" rad/s");
Serial.print ("Resolution: "); Serial.print(sensor.resolution); Serial.println(" rad/s");
Serial.println("------------------------------------");
Serial.println("");
delay(500);
}
void setup(void)
{
Serial.begin(9600);
Serial.println("Gyroscope Test"); Serial.println("");
/* Инициализация датчика */
gyro.enableAutoRange(true);
/* Initialise the sensor */
if(!gyro.begin())
{
/* Возникла проблема с обнаружением L3GD20 ... проверьте свои соединения */
Serial.println("Упс, L3GD20 не обнаружен ... Проверьте проводку!");
while(1);
}
/* Отображение некоторой основной информации на этом датчике */
displaySensorDetails();
}
void loop(void)
{
/* Получить новое событие датчика */
sensors_event_t event;
gyro.getEvent(&event);
/* Отображение результатов (скорость измеряется в рад/с) */
Serial.print("X: "); Serial.print(event.gyro.x); Serial.print(" ");
Serial.print("Y: "); Serial.print(event.gyro.y); Serial.print(" ");
Serial.print("Z: "); Serial.print(event.gyro.z); Serial.print(" ");
Serial.println("rad/s ");
delay(1);
}
Итак, вкратце, мне нужна помощь в создании кода/формулы, которая будет использовать мой гироскоп для измерения его ориентации. Спасибо за уделенное время! Я очень растерян.
@Max, 👍1
Обсуждение1 ответ
Лучший ответ:
▲ 2
Взгляните на этот пример.
#include <Wire.h>
const int MPU = 0x68; // адрес MPU6050 I2C
float AccX, AccY, AccZ;
float GyroX, GyroY, GyroZ;
float accAngleX, accAngleY, gyroAngleX, gyroAngleY, gyroAngleZ;
float roll, pitch, yaw;
float AccErrorX, AccErrorY, GyroErrorX, GyroErrorY, GyroErrorZ;
float elapsedTime, currentTime, previousTime;
int c = 0;
void setup() {
Serial.begin(19200);
Wire.begin(); // Инициализировать соединение
Wire.beginTransmission(MPU); // Начало связи с MPU6050 // MPU=0x68
Wire.write(0x6B); // Поговорите с регистром 6B
Wire.write(0x00); // Сброс - поместите 0 в регистр 6B
Wire.endTransmission(true); //завершение передачи
/*
// Настройка Чувствительности Акселерометра - Диапазон полной Шкалы (по умолчанию +/- 2g)
Wire.beginTransmission(MPU);
Wire.write(0x1C); //Поговорите с регистром ACCEL_CONFIG (1C hex)
Wire.write(0x10); ///Установите биты регистра в 00010000 (+/- 8 г полный диапазон шкалы)
Wire.endTransmission(true);
// Configure Gyro Sensitivity - Full Scale Range (default +/- 250deg/s)
Wire.beginTransmission(MPU);
Wire.write(0x1B); // Поговорите с регистром GYRO_CONFIG (1B шестнадцатеричный)
Wire.write(0x10); // Установите биты регистра в 00010000 (1000 дег/с в полном масштабе)
Wire.endTransmission(true);
delay(20);
*/
// Вызовите эту функцию, если вам нужно получить значения ошибок IMU для вашего модуля
calculate_IMU_error();
delay(20);
}
void loop() {
// === Считывание данных акселерометра === //
Wire.beginTransmission(MPU);
Wire.write(0x3B); // Начните с регистра 0x3B (ACCEL_XOUT_H)
Wire.endTransmission(false);
Wire.requestFrom(MPU, 6, true); // Всего считано 6 регистров, каждое значение оси хранится в 2 регистрах
//For a range of +-2g, we need to divide the raw values by 16384, according to the datasheet
AccX = (Wire.read() << 8 | Wire.read()) / 16384.0; // Значение оси X
AccY = (Wire.read() << 8 | Wire.read()) / 16384.0; // Значение оси Y
AccZ = (Wire.read() << 8 | Wire.read()) / 16384.0; // Значение оси Z
// Расчет крена и тангажа от акселерометра сведения
accAngleX = (atan(AccY / sqrt(pow(AccX, 2) + pow(AccZ, 2))) * 180 / PI) - 0.58; // AccErrorX ~(0.58) See the calculate_IMU_error()custom function for more details
accAngleY = (atan(-1 * AccX / sqrt(pow(AccY, 2) + pow(AccZ, 2))) * 180 / PI) + 1.58; // AccErrorY ~(-1.58)
// === читывание данных гироскопа === //
previousTime = currentTime; // Предыдущее время сохраняется до фактического времени чтения
currentTime = millis(); // Текущее время истекло фактическое время чтения
elapsedTime = (currentTime - previousTime) / 1000; // Разделить на 1000, чтобы получить секунды
Wire.beginTransmission(MPU);
Wire.write(0x43); // Адрес первого регистра данных гироскопа 0x43
Wire.endTransmission(false);
Wire.requestFrom(MPU, 6, true); // Всего считано 4 регистра, значение каждой оси хранится в 2 регистрах
GyroX = (Wire.read() << 8 | Wire.read()) / 131.0; // For a 250deg/s range we have to divide first the raw value by 131.0, according to the datasheet
GyroY = (Wire.read() << 8 | Wire.read()) / 131.0;
GyroZ = (Wire.read() << 8 | Wire.read()) / 131.0;
// Исправьте выходные данные с помощью рассчитанных значений ошибок
GyroX = GyroX + 0.56; // GyroErrorX ~(-0.56)
GyroY = GyroY - 2; // GyroErrorY ~(2)
GyroZ = GyroZ + 0.79; // GyroErrorZ ~ (-0.8)
// В настоящее время исходные значения выражены в градусах в секунду, град/с, поэтому нам нужно умножить на sendonds, чтобы получить угол в градусах
gyroAngleX = gyroAngleX + GyroX * elapsedTime; // deg/s * s = deg
gyroAngleY = gyroAngleY + GyroY * elapsedTime;
yaw = yaw + GyroZ * elapsedTime;
// Дополнительный фильтр - комбинирует значения акселерометра и угла поворота гироскопа
roll = 0.96 * gyroAngleX + 0.04 * accAngleX;
pitch = 0.96 * gyroAngleY + 0.04 * accAngleY;
// Вывод значений на последовательный монитор
Serial.print(roll);
Serial.print("/");
Serial.print(pitch);
Serial.print("/");
Serial.println(yaw);
}
void calculate_IMU_error() {
// Мы можем вызвать эту функцию в разделе настройки, чтобы рассчитать погрешность данных акселерометра и гироскопа. Отсюда мы получим значения ошибок, используемые в приведенных выше уравнениях, напечатанными на последовательном мониторе.
// Обратите внимание, что мы должны разместить ИДУ ровно, чтобы получить правильные значения, чтобы затем мы могли получить правильные значения
// Считывание значений акселерометра 200 раз
while (c < 200) {
Wire.beginTransmission(MPU);
Wire.write(0x3B);
Wire.endTransmission(false);
Wire.requestFrom(MPU, 6, true);
AccX = (Wire.read() << 8 | Wire.read()) / 16384.0 ;
AccY = (Wire.read() << 8 | Wire.read()) / 16384.0 ;
AccZ = (Wire.read() << 8 | Wire.read()) / 16384.0 ;
// Суммировать все показания
AccErrorX = AccErrorX + ((atan((AccY) / sqrt(pow((AccX), 2) + pow((AccZ), 2))) * 180 / PI));
AccErrorY = AccErrorY + ((atan(-1 * (AccX) / sqrt(pow((AccY), 2) + pow((AccZ), 2))) * 180 / PI));
c++;
}
//Разделите сумму на 200, чтобы получить значение ошибки
AccErrorX = AccErrorX / 200;
AccErrorY = AccErrorY / 200;
c = 0;
// Считывайте значения гироскопа 200 раз
while (c < 200) {
Wire.beginTransmission(MPU);
Wire.write(0x43);
Wire.endTransmission(false);
Wire.requestFrom(MPU, 6, true);
GyroX = Wire.read() << 8 | Wire.read();
GyroY = Wire.read() << 8 | Wire.read();
GyroZ = Wire.read() << 8 | Wire.read();
// Суммировать все показания
GyroErrorX = GyroErrorX + (GyroX / 131.0);
GyroErrorY = GyroErrorY + (GyroY / 131.0);
GyroErrorZ = GyroErrorZ + (GyroZ / 131.0);
c++;
}
//Разделите сумму на 200, чтобы получить значение ошибки
GyroErrorX = GyroErrorX / 200;
GyroErrorY = GyroErrorY / 200;
GyroErrorZ = GyroErrorZ / 200;
// Выведите значения ошибок на последовательный монитор
Serial.print("AccErrorX: ");
Serial.println(AccErrorX);
Serial.print("AccErrorY: ");
Serial.println(AccErrorY);
Serial.print("GyroErrorX: ");
Serial.println(GyroErrorX);
Serial.print("GyroErrorY: ");
Serial.println(GyroErrorY);
Serial.print("GyroErrorZ: ");
Serial.println(GyroErrorZ);
}
https://howtomechatronics.com/tutorials/arduino/arduino-and-mpu6050-accelerometer-and-gyroscope-tutorial/
,
@RedDogAlpha
Не делайте " Провод.прочитайте()<<8|Wire.read(): язык C++ не гарантирует порядок оценки двух вызовов функции " Wire.read(). Do val=Провод.читать()<<8;val|=Провод.вместо этого прочитайте (). Кроме того, эта наивная интеграция будет работать только в том случае, если углы крена и тангажа останутся небольшими., @Edgar Bonet
Смотрите также:
- Что означает “LSB на градус в секунду” (LSB per degree per second)?
- В чем разница между акселерометром, гироскопом и датчиком магнитометра?
- MPU-6050 - угловой дрейф
- Линейное ускорение от MPU 6050
- Правильный способ получить значения крена, тангажа и перемещения
- Снять гравитацию с акселерометра MPU-6050
- Arduino Nano IOT LSM6DS3 получить угол гироскопа в градусах
- Помощь с MPU-6050
“Интеграция” - это небольшое упрощение: вам придется составить много небольших вращений, чтобы получить текущую ориентацию. Вы, вероятно, должны искать библиотеку, которая делает это за вас, если вы не готовы копаться в [тяжелой математике](https://en.wikipedia.org/wiki/Quaternions_and_spatial_rotation)., @Edgar Bonet