Перейти к содержанию
Поиск
    

Проблема с L3G4200

Ответить

bva_rzn    0

bva_rzn
Опубликовано · Жалоба

Здравструйте.

На плате есть два датчика: L3G4200 и LSM303. Схема подключения взята из документаций на эти датчики. Датчики подключенны к

PIC18. Интерфейс обмена i2c. Реализован программно. Скорость менял от 30 кбис/с до 400 кбит/с.

Настройки датчика:

 

Write_byte(CTRL_REG4,0x80);

Write_byte(CTRL_REG3,0x08);

Write_byte(CTRL_REG2,0x06);

Write_byte(CTRL_REG1,0x0F);

 

Данные считываю блоком начиная с адреса STATUS_REG: GetI2C(STATUS_REG|0x80);

Данные считываются. Регистр WHO_AM_I так же читается, т.е. i2c работает.

 

Проблема в том, что в данных, считанных из датчика наблюдаются большие выбросы, хотя он неподвижен.

В чём может быть проблема?

 

 

 

 

Поделиться сообщением

Ссылка на сообщение
Поделиться на другие сайты

Alexey_1811    0

Alexey_1811
Опубликовано · Жалоба
Проблема в том, что в данных, считанных из датчика наблюдаются большие выбросы, хотя он неподвижен.

В чём может быть проблема?

У меня работает так

void IMU_Gyro_Init(void)
{
  L3G_WriteReg(0xD2+0 ,0x20 ,0x8F ); // CTRL_REG1   400Hz ODR, 20hz filter, run!
  L3G_WriteReg(0xD2+0 ,0x24 ,0x02 ); // CTRL_REG5   low pass filter enable
}

// -------------------------------- //
void IMU_Read_Gyro(void)
{
    unsigned char GIR[6];

    SENS_ReadData(0xD2, (0x28|(1<<7)), GIR);
    IMU_GYRO.X  = GIR[1]<<8;
    IMU_GYRO.X |= GIR[0];
    IMU_GYRO.Y  = GIR[3]<<8;
    IMU_GYRO.Y |= GIR[2];
    IMU_GYRO.Z  = GIR[5]<<8;
    IMU_GYRO.Z |= GIR[4];
}

// -------------------------------- //
unsigned short SENS_ReadData(unsigned char addr, unsigned char reg, unsigned char *data)
{
    volatile unsigned short SR2;
    //volatile int tmp;
    unsigned short rsp = 0;

    I2C1->CR1 |= I2C_CR1_START;
    rsp += I2C_Wait_Response(I2C_SR1_SB);
    I2C1->DR = addr;
    rsp += I2C_Wait_Response(I2C_SR1_ADDR);
    SR2  = I2C1->SR2;
    I2C1->DR = reg;
    rsp += I2C_Wait_Response(I2C_SR1_TXE);
    I2C1->CR1 |= I2C_CR1_START;
    rsp += I2C_Wait_Response(I2C_SR1_SB);
    I2C1->DR = addr|(1<<0);
    rsp += I2C_Wait_Response(I2C_SR1_ADDR);
    SR2  = I2C1->SR2;
    I2C1->CR1 |= I2C_CR1_ACK;
    rsp += I2C_Wait_Response(I2C_SR1_RXNE);
    *data++ = I2C1->DR; 
    rsp += I2C_Wait_Response(I2C_SR1_RXNE);
    *data++ = I2C1->DR;
    rsp += I2C_Wait_Response(I2C_SR1_RXNE);
    *data++ = I2C1->DR;
    rsp += I2C_Wait_Response(I2C_SR1_RXNE);
    *data++ = I2C1->DR;
    rsp += I2C_Wait_Response(I2C_SR1_RXNE);
    *data++ = I2C1->DR;
    rsp += I2C_Wait_Response(I2C_SR1_RXNE);
    *data++ = I2C1->DR;
    I2C1->CR1 &= ~I2C_CR1_ACK;
    I2C1->CR1 |= I2C_CR1_STOP;
    return rsp;
}

Ну и мой вопрос.

 

Как посчиталь угол наклона по полученным данным?

Данные читаю каждые 10ms.

Гироскоп настроен на 2000 dps.

Предполагаю что так:

angel_X = angel_X + gyroX*0.01;

 

Поделиться сообщением

Ссылка на сообщение
Поделиться на другие сайты

bva_rzn    0

bva_rzn
Опубликовано · Жалоба

Добрый день.

 

Какой конкретно нужно посчитать угол: крен или тангаж?

Вычисление угла зависит от задачи.

Если в двух словах, то углы ориентации (крен, тангаж, курс) по данным с гироскопа считаются так:

1) начальная выставка. По данным других датчиков (акселерометры, магнитометры, спутниковый приёмник) вычисляются начальные углы ориентации.

2) при каждом новом поступлении данных с гироскопа вычислятся или матрица направляющих косинусов, или вектор с параметрами собственного кватерниона и по ним определяются углы ориентации.

 

Особенность этих датчиков - наличие дрейфа гироскопов.

Я работаю с ADIS16405BMLZ, так вот если не делать никаких телодвижений по компенсации этого дрейфа, то уход углов ориетнации около 1 градуса за минуту.

 

Поэтому как вычислять углы нужно в зависимости от задачи.

Для вычисления крена и тангажа проще использовать акселерометр (трёхосевой).

 

Если интересно, могу написать в личку.

Поделиться сообщением

Ссылка на сообщение
Поделиться на другие сайты

Alexey_1811    0

Alexey_1811
Опубликовано · Жалоба
Какой конкретно нужно посчитать угол: крен или тангаж?

Нужно считать крен, тангаж и курс.

Для вычисления крена и тангажа проще использовать акселерометр (трёхосевой).

Объект будет двигаться с ускорением так что предполагаю акселерометр не покатит.

Если интересно, могу написать в личку.

Очень интересно.

Поделиться сообщением

Ссылка на сообщение
Поделиться на другие сайты

bva_rzn    0

bva_rzn
Опубликовано · Жалоба

Нужно считать крен, тангаж и курс.

 

Для вычисления углов ориентации нужно на каждом такте поступления данных с гироскопа по 3 - м осям вычислять

матрицу направляющих косинусов DCM (с кватернионами тоже самое, только там вектор). Угловые скорости нужно подставлять в кососимметричную матрицу.

 

Углы ориентации вычисляются из DCM в зависимости от того, какой поворот был первым, второым, третьим.

Начальное значение DCM определяется по показаниям акселерометров и магнитометров, спутникового приёмника.

 

Объект будет двигаться с ускорением так что предполагаю акселерометр не покатит.

 

Прокатит. Только после того, как при помощи него и магнитометра компенсирован дрейф гироскопов и найдены углы ориентации (на каждом такте), нужно пересчитывать показания акселерометров в ошибки в углах ориентации (эти ошибки как раз и вызваны наличием ускорения) и

компенсировать их в матрице DCM, полученной по показаниям гироскопов. Это самый простой вариант. Есть ещё вариант, но в нём необходимо знать где мы находимся и знать напрящённость магнитного поля в этой точке.

 

Очень интересно.

 

Поделиться сообщением

Ссылка на сообщение
Поделиться на другие сайты

_noise    0

_noise
Опубликовано (изменено) · Жалоба

То ли частная консультация "эксперту задан вопрос - от эксперта получен ответ либо вопрос больше неактуален либо вопрошавший самостоятельно решил свою проблему - короче автор удовлетворён и закрыли проехали", то ли публичные прения на какую-то такую заданную тему... шёл мимо, увидел щель, подумал вставить тоже:

 

1.

Если в больших выбросах, о которых сверху заголовок, старший бит в старшем байте =1 - тогда это не большие по модулю положительные, а малые по модулю отрицательные значения. "Минус немножко". Иностранными словами называется "2's complement" такая запись чисел.

 

Иначе любопытно, чем дело кончилось и как проблема устранена.

 

2.

angel_X = angel_X + gyroX*0.01 *К

 

"Предполагаю, что" оригинальный подход к решению задачи... типа как ответ должен быть либо получен готовый уже от решавших её, либо угадан через интуицию?

 

gyroX (поп/сек) * период опроса (сек) = angel_X (поп) ... получили угол поворота в попугаях, один попугай ~ LSBit

angel_X (поп) * чувствительность (град/поп) = angel_X (град) ... получили угол в градусах

 

(для диапазона 2000dps чувствительность = 70 миллиградусов на каждого попугая = 0.07 градуса на попугая)

 

... а дальше угол в градусах умножаем на ещё что-нибудь, чтобы результат получился в удобных для дальнейшего его использования единицах. Например, в единицах "1/256 полного оборота".

 

---

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

Чтобы сначала все исходные данные, а потом на каждом этапе вычисления результат помещался(ись) в отведённое(ые) ему(им) место(а). Без переполнения, но/и с приемлемой точностью.

И чтобы алгоритмы соответственно...

... но это актуально только для задро ассемблерщиков красноглазых, а профессионалам можно не заморачиваться об такие мелочи, тупо всё сразу считая в супермегафлоатах @ стопицотмегагерцтактовой. Через готовые стандартные либы. Решая задачу вместо ковыряния байтиков под микроскопом.

 

3.

Прежде чем использовать полученный с сабжа очередной отсчёт, его полезно слегка обработать:

 

а). вычесть из него zero rate level (про который см. далее),

б). потом обнулить отсчёт, если он слишком близок к нулю (т.о. убираем шум, зато/и игнорируем медленные вращения).

 

4.

Zero rate level:

 

а). наше изделие находится в состоянии покоя,

б). собираем сколько-нибудь отсчётов, суммируем и делим на количество,

в). полученный zero rate level запоминаем, теперь нашему изделию можно начинать шевелиться.

 

г). время от времени перекалибровываемся повторно, начиная с п. 4а.

 

Обратить внимание на подчёркнутое в состоянии покоя, много думать.

 

5.

Почитать:

 

AN3393 L3G4200D: three axis digital output gyroscope ... от производителя сабжа про "как подключать, настраивать и получать какие-то данные"

TA0343 Everything about STMicroelectronics’ 3-axis digital MEMS gyroscopes ... от производителя сабжа про "как понимать полученные данные и что с ними делать дальше"

Helmet Locating and Tracking System Final Report ... учебный проект студентов, на соответствующем уровне подробненько и понятненько в сочетании с TA0343

Изменено пользователем нечитатель

Поделиться сообщением

Ссылка на сообщение
Поделиться на другие сайты

Alexey_1811    0

Alexey_1811
Опубликовано · Жалоба

Спасибо за ответы.

С гироскопом разобрался.

Крен, тангаж и курс получил в связке L3G4200+LSM303 по алгоритму FreeIMU.

 

 

Поделиться сообщением

Ссылка на сообщение
Поделиться на другие сайты

Присоединяйтесь к обсуждению

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

Гость
Ответить в этой теме...

×   Вставлено с форматированием.   Вставить как обычный текст

  Разрешено использовать не более 75 эмодзи.

×   Ваша ссылка была автоматически встроена.   Отображать как обычную ссылку

×   Ваш предыдущий контент был восстановлен.   Очистить редактор

×   Вы не можете вставлять изображения напрямую. Загружайте или вставляйте изображения по ссылке.

×
×
×
  • Создать...