STM32 и гироскоп L3GD20. Часть 2.

Доброго всем дня! Мы продолжаем экспериментировать с отладочной платой STM32F3Discovery. Сегодня продолжим работу с установленным на ней гироскопом L3GD20. Напомню, что мы уже писали пример программы для контроллера STM32F3 и разобрались с записью и чтением регистров гироскопа. Вот эта статья – работа с L3GD20. Ну а сегодня мы напишем еще одну программу для работы с этим замечательным устройством, только на этот раз не ограничимся опытами с чтением/записью регистров, а получим от гироскопа какие-нибудь полезные данные и переконвертируем их в нужный нам вид.
Гироскоп L3GD20

Итак, пусть нам необходимо получить данные о положении платы STM32F3Discovery относительно оси x. Для других осей все идентично, поэтому остановимся на рассмотрении одной 😉 Для начала нужно понять, где же вообще эта ось x )
Расположение осей гироскопа

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

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

Итак, мы хотим получить значение угла отклонения платы от оси х. А что же мы получаем от гироскопа? А гироскоп нам выдает значение угловой скорости при перемещении девайса. То есть наша задача в итоге сводится к следующему:

  1. Считываем данные из регистров OUT_X_L и OUT_X_H гироскопа. Адреса регистров соответственно – 0х28 и 0х29.
  2. Значение угловой скорости у нас 16-битное (точнее 15 бит и 1 бит на знак). Значит мы должны из двух 8-ми битных регистров склеить искомое значение угловой скорости.
  3. Значение получено – конвертируем его в нужный нам вид.

Вот примерно так должна быть построена работа нашей программы. Рассмотрим практическую реализацию каждого из этих шагов.

Считываем значения уже упомянутых регистров OUT_X_L и OUT_X_H. Для этого используем функцию мультибайтового чтения, реализованную в L3GD20. Как вы помните из предыдущей статьи (ссылка на нее есть в начале) в команде, которую мы отправляем гироскопу есть один замечательный бит – MS – именно он то нам сейчас и пригодится. Итак, давайте сформируем запрос, который мы должны отправить по SPI.

Посылаемая команда

Мы собираемся считывать значение регистров – следовательно бит RW должен быть выставлен в 1. Идем дальше. Будем считывать несколько байт сразу – бит MS тоже в 1. Следующие 6 бит – это адрес первого из читаемых регистров. В нашем случае это 0b101000 (0х28). Получаем команду – 0b11101000 – то есть 0хЕ8. Посылаем эту команду, затем отправляем два нулевых байта (для того, чтобы на линии присутствовало тактирование) и получаем от гироскопа значения двух регистров с адресами 0x28 и 0x29. Вот как это выглядит в коде:

GPIO_ResetBits(GPIOE, GPIO_Pin_3);
sendByte(0xE8);	
receiveData[0] = sendByte(0x00);
receiveData[1] = sendByte(0x00);
GPIO_SetBits(GPIOE, GPIO_Pin_3);

Не забываем дергать линию CS (вывод PE8). Функцию sendByte() мы реализовали в первой статье, посвященной работе с гироскопом. Вот еще раз ссылочка на нее – статья тут

В общем, получили мы значения регистров – теперь необходимо их обработать. И тут возможны два варианта..

Если у нас девайс вращается в положительном направлении оси x (розовая стрелка на рисунке в начале этой статьи), то достаточно следующего действия:

xResult = receiveData[0] | (receiveData[1] << 8);

И в переменной xResult мы имеем искомое значение угловой скорости. Ситуация немного другая, если вращение происходит в противоположном направлении. В этом случае 16-ый бит переменной xResult равен 1, поскольку он отвечает за знак переменной. Соответственно, для получения абсолютной величины скорости, нам нужны только младшие 15 бит переменной xResult:

xResult &= 0x7FFF;

На этом, казалось бы все, но на самом деле это еще не конец 😉 При вращении в противоположном направлении для получения искомой скорости необходимо вычесть из числа 0x8000 значение полученной переменной xResult. Таким образом:

xResult = receiveData[0] | (receiveData[1] << 8);
xResult &= 0x7FFF;
xResult = 0x8000 - xResult;

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

В строке Sensitivity надо найти число, соответствующее текущим настройкам гироскопа. У меня:

writeData(0x20, 0x0F);
writeData(0x23, 0x30);

То есть в соответствии с документацией – 2000 dps.
Настройка гироскопа
Значит для того, чтобы из данных, выдаваемых гироскопом, получить угловую скорость в градусах за секунду надо полученные данные умножить на 0.07. Но и это еще не все 😉 Мы же хотим знать отклонение! Для этого организуем опрос датчика через равные промежутки времени – например каждые 20 мс. И умножив угловую скорость на 20 мс мы получим то, что нам надо )

Для реализации всего этого используем таймер (статья про таймеры). Настроим таймер на генерацию прерывания каждые 20 мс и в прерывании займемся всей полезной работой:

void TIM2_IRQHandler()
{
    // Обнуляем флаг прерывания
    TIM_ClearITPendingBit(TIM2, TIM_IT_Update);
 
    // Опрашиваем датчик
    GPIO_ResetBits(GPIOE, GPIO_Pin_3);
    sendByte(0xE8);	
    receiveData[0] = sendByte(0x00);
    receiveData[1] = sendByte(0x00);
    GPIO_SetBits(GPIOE, GPIO_Pin_3);
 
    // Формируем результат
    xResult = receiveData[0] | (receiveData[1] << 8);
 
    // Смотрим, в каком направлении происходит вращение. Для этого 
    // анализируем 16-ый бит принятых данных
    if ((xResult & 0x8000) == 0)
    {
        // Переменная xSign равняется 1, если вращение в "отрицательном" 
        // направлении, и 0, если вращение в "положительном" направлении
	xSign = 0;					
    }	
    else
    {
	xSign = 1;
	xResult &= 0x7FFF;
	xResult = 0x8000 - xResult;
    }
 
    // Учитываем угловое отклонение за текущий промежуток времени, 
    // прибавляя/вычитая его 
    if (xSign == 0)
    {
	xPosition += 0.07 * xResult * 0.02;
    }
    else
    {
	xPosition -= 0.07 * xResult * 0.02;
    }
}

Почти готово 😉 Есть еще один небольшой момент. При считывании данных из регистров гироскопа обнаружилась следующая вещь. Значение угловой скорости колеблется относительно нуля, даже если плата не двигается с места. То есть вместо значения скорости, равного нулю, мы будем получать хаотично меняющиеся значения (0х02, 0х05….). В результате будет накапливаться ошибка определения местоположения платы. Для того, чтобы это убрать давайте в случае, если значение переменной xResult меньше величины 0x0A, будем обнулять эту переменную:

if (xResult < 0x0A)
{
    xResult = 0;
}

Вот теперь вроде бы все ) Прошиваем программу в микроконтроллер и видим, что при повороте платы из горизонтального положения в вертикальное (то есть поворот на 90 градусов) значение переменной xPosition определяется верно:
Работа программы

Такой вот получился ознакомительный пример, не затронули мы тему калибровки, но она необходима для получения более точных результатов. Это связано с тем, что показания датчика по одной или по нескольким осям могут быть смещены относительно нуля, соответственно, значение угловой скорости будет содержать в себе ошибку. Калибровку можно выполнять следующим образом:

  1. На неподвижной плате в течении некоторого времени собираем показания датчика по всем осям.
  2. Из этих показаний находим средние значения для каждой оси.
  3. Поскольку при отсутствии движения датчик должен выдавать нули, то те значения, которые мы получили являются ошибочными, фиксируем их и при дальнейшей работе с датчиком всегда учитываем при расчете показаний.

В завершение привожу полный код программы =)

/*******************************************************************/
#include "stm32f30x_gpio.h"
#include "stm32f30x_rcc.h"
#include "stm32f30x_spi.h"
#include "stm32f30x_tim.h"
#include "stm32f30x.h"
 
 
 
/*******************************************************************/
#define DUMMY				0x00
#define TIMEOUT_TIME			0x1000
 
SPI_InitTypeDef spi;
TIM_TimeBaseInitTypeDef timer;
GPIO_InitTypeDef gpio;
uint8_t receiveData[2];
uint16_t timeout;
uint8_t tempByte;
uint16_t xResult;
float xPosition;
uint8_t xSign;
 
 
 
/*******************************************************************/
void initAll()
{
    RCC_AHBPeriphClockCmd(RCC_AHBPeriph_GPIOA, ENABLE);
    RCC_AHBPeriphClockCmd(RCC_AHBPeriph_GPIOE, ENABLE);
    RCC_APB2PeriphClockCmd(RCC_APB2Periph_SPI1, ENABLE);
    RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM2, ENABLE);
 
    GPIO_PinAFConfig(GPIOA, GPIO_PinSource5, GPIO_AF_5);
    GPIO_PinAFConfig(GPIOA, GPIO_PinSource6, GPIO_AF_5);
    GPIO_PinAFConfig(GPIOA, GPIO_PinSource7, GPIO_AF_5);
 
    gpio.GPIO_Mode = GPIO_Mode_AF;
    gpio.GPIO_Pin = GPIO_Pin_5 | GPIO_Pin_6 | GPIO_Pin_7;
    gpio.GPIO_OType = GPIO_OType_PP;
    gpio.GPIO_PuPd  = GPIO_PuPd_NOPULL;
    gpio.GPIO_Speed = GPIO_Speed_50MHz;
    GPIO_Init(GPIOA, &gpio);
 
    gpio.GPIO_Mode = GPIO_Mode_OUT;
    gpio.GPIO_Pin = GPIO_Pin_3;
    gpio.GPIO_OType = GPIO_OType_PP;
    gpio.GPIO_Speed = GPIO_Speed_50MHz;
    GPIO_Init(GPIOE, &gpio);	
 
    spi.SPI_Direction = SPI_Direction_2Lines_FullDuplex;
    spi.SPI_DataSize = SPI_DataSize_8b;
    spi.SPI_CPOL = SPI_CPOL_High;
    spi.SPI_CPHA = SPI_CPHA_2Edge;
    spi.SPI_NSS = SPI_NSS_Soft;
    spi.SPI_BaudRatePrescaler = SPI_BaudRatePrescaler_8;
    spi.SPI_FirstBit = SPI_FirstBit_MSB;
    spi.SPI_CRCPolynomial = 7;
    spi.SPI_Mode = SPI_Mode_Master;
    SPI_Init(SPI1, &spi);
 
    SPI_Cmd(SPI1, ENABLE);
 
    SPI_RxFIFOThresholdConfig(SPI1, SPI_RxFIFOThreshold_QF);
 
    SPI_DataSizeConfig(SPI1, ENABLE);
 
    TIM_TimeBaseStructInit(&timer);
    timer.TIM_Prescaler = 720 - 1;
    timer.TIM_Period = 2000;
    TIM_TimeBaseInit(TIM2, &timer);
}
 
 
 
/*******************************************************************/
uint8_t sendByte(uint8_t byteToSend)
{
    timeout = TIMEOUT_TIME;
    while ((SPI_I2S_GetFlagStatus(SPI1, SPI_I2S_FLAG_TXE) == RESET) & (timeout != 0))
    {
	timeout--;
    }		
 
    SPI_SendData8(SPI1, byteToSend);
 
    timeout = TIMEOUT_TIME;	
    while ((SPI_I2S_GetFlagStatus(SPI1, SPI_I2S_FLAG_RXNE) == RESET) & (timeout != 0))
    {
    	timeout--;
    }	
 
    return (uint8_t)SPI_ReceiveData8(SPI1);
}
 
 
 
/*******************************************************************/
int main()
{
    __enable_irq();
    initAll();
    xPosition = 0;
 
    TIM_ITConfig(TIM2, TIM_IT_Update, ENABLE);
    TIM_Cmd(TIM2, ENABLE);
    NVIC_EnableIRQ(TIM2_IRQn);
 
    writeData(0x20, 0x0F);
    writeData(0x23, 0x30);
 
    while(1)
    {			
    }
}
 
void TIM2_IRQHandler()
{
    TIM_ClearITPendingBit(TIM2, TIM_IT_Update);
 
    GPIO_ResetBits(GPIOE, GPIO_Pin_3);
    sendByte(0xE8);	
    receiveData[0] = sendByte(0x00);
    receiveData[1] = sendByte(0x00);
    GPIO_SetBits(GPIOE, GPIO_Pin_3);
 
    xResult = receiveData[0] | (receiveData[1] << 8);
 
    if ((xResult & 0x8000) == 0)
    {
	xSign = 0;					
    }	
    else
    {
	xSign = 1;
    	xResult &= 0x7FFF;
	xResult = 0x8000 - xResult;
    }
 
    if (xResult < 0x0A)
    {
	xResult = 0;
    }
 
    if (xSign == 0)
    {
	xPosition += 0.07 * xResult * 0.02;
    }
    else
    {
	xPosition -= 0.07 * xResult * 0.02;
    }
}
 
 
 
/*******************************************************************/

Понравилась статья? Поделись с друзьями!

STM32 и гироскоп L3GD20. Часть 2.: 37 комментариев
  1. А зачем проверять 15й бит знака у значения оси? Значение положительное если движение в одну сторону и отрицательным если в другую. Проверка бита в данном примере ни чего не даёт, после
    xResult = receiveData[0] | (receiveData[1] << 8);
    достаточно
    xPosition += xResult * 0.0014;

  2. За статью спасибо, было бы интересно про акселерометр, у него значение оси даёт угол наклона, а при движении, а не вращении, значения меняются, так вот работа в паре с гироскопом даёт понять о движении платы с датчиками. Т.е. если гироскоп молчит, а аксель колбасит, то плату двигают а не крутят. Да и магнитометр с акселем тоже любят вместе работать.

  3. Мне кажется углы по X и Y надо постоянно устанавливать в 0, исходя из данных ускорения по оси Z. Когда это ускорение будет равно ускорению свободного падения (т.е плата идеально параллельна горизонту) установить xPosition=0, yPosition=0. Поправьте если бред.

    • Ну у меня вообще всегда только одна ось задействована, еще не было задачи, когда требовались бы данные с разных осей )

  4. Статья полезная, спасибо большое. Было бы замечательно еще сделать примерчик с акселерометром для получения ускорений по осям. ))

  5. “и получаем от гироскопа значения двух регистров с адресами 0xA8 и 0xA9.”
    Мне кажется, что должны быть другие адреса: 0x28 и 0x29, т.к. именно с 0x28 регистра идет чтение.

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

    int16_t data[3]; //массив для результатов

    //вызов функции. Описание ниже.
    spi_multiple_read(0x28, (uint8_t *)data, 6);

    x_value = data[0]; //как содержимое массива соотносится
    y_value = data[1]; // с осями
    z_value = data[1];

    //adr адрес первого регистра, который нужно считать
    //dst адрес куда записывать результат
    //length количество байт, которые нужно считать.
    spi_multiple_read(uint8_t adr, uint8_t* dst, uint8_t length)

    //В теле функции
    //Передача упраляющего бита. 0x80 обозначает операцию
    // чтения. 0x40 указывает, что адрес регистра нужно
    //увеличивать
    spi_exchange(adr | 0xC0);
    //читаем запрошенное количество байт.
    for (;length;length–)
    *(dst++) = spi_exchange(0x8F);
    //передаваемый бит игнорируется гироскопом. Можно что
    // угодно передавать. Каждый раз увеличиваем адрес для
    //записи. Функция spi_exchange возвращает байт пришедший
    //по проводу MISO

  7. За статью спасибо, а с акселерометром нет примера чтобы линейное перемещение рассчитывать?

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

  8. Буду признателен за помощь! У меня компилятор CooCox ругается :
    >undefined reference to `writeData’
    >undefined reference to `writeData’
    строки:
    writeData(0x20, 0x0F);
    writeData(0x23, 0x30);

    • Функция writeData() не определена. Посмотри в первой статье про гироскоп – я там ее код приводил. Просто в этот проект ее добавь.

  9. Компилятор ругается function “TIM2_IRQHandler” has no prototype в чем проблема? плата stm32f3discovery, чип стоит stm32f303

  10. Разобрался , нехватало обьявления процедуры вначале программы
    void TIM2_IRQHandler(void);

  11. Подскажите пожалуйста зачем после того как вы отправляете адрес регистра для чтения, следом отправляете еще 2 пустых байта? Недопонял тот момент…..
    sendByte(0xE8);
    receiveData[0] = sendByte(0x00);
    receiveData[1] = sendByte(0x00);

    • Просто для того, чтобы принять данные от Slave, Master должен обеспечить тактирование на линии. Для этого мы отправляем пустые байты.

  12. Есть одно замечание. Если я не прав, прошу простить. У ST есть документ TA0343 “Everything about STMicroelectronics’ 3-axis digital MEMS
    gyroscopes”. В разделе 5.2 “How to get meaningful information” есть такой пример:

    Please note that the 16-bit gyroscope’s output data are in 2’s complement format (signed integer) and the typical sensitivity at ±250 dps is 0.00875 dps/LSB from the datasheet. For example, when the gyroscope is stationary, the X-, Y- and Z-axis outputs may look like the
    following:
    X-axis: FF96 LSBs = -106 LSBs = -106 * 0.00875 = -0.93 dps
    Y-axis: 0045 LSBs = 69 LSBs = 69 * 0.00875 = 0.6 dps
    Z-axis: FFCC LSBs = -52 LSBs = -0.46 dps

    Если подставить значение из примера 0xFF96 в вашу часть кода:

    xSign = 1;
    xResult &= 0x7FFF; (0x7F96)
    xResult = 0x7FFF – xResult; (0x7FFF – 0x7F96 = 0x69 = 105)

    что на 1 меньше чем в примере. Изучал данный вопрос, действительно при переводе из дополнительного кода нужно делать +1.

  13. Добрый день. Подскажите пожалуйста, может в гироскопе есть какие нибудь настройки – калибровки. У меня проблема, при запуске гироскопа показания угловой скорости выдаются со смещением. Отсюда постоянное изменение углов по осям, как будто гироскоп кто-то поворачивает. Т.е. все происходит в точности как у вас, значения выдаются корректные. При неподвижном положении гироскопа есть колебания скоростей. Но эти колебания как бы не от нулевого значения скорости, а от некоторого смещенного значения в положительную, или отрицательную сторону. После выключения/включения питания смещения меняются, до следующего выключения/включения. Как его откалибровать? Пробовал несколько микросхем – тоже самое. ПДФ читал.

    • Ну если в пдф нет никакого механизма калибровки, то нужно добавить программный. Что-то вроде такого – при включении пару секунд нужно держать плату ровно и не двигать, за это время считать показания гироскопа, затем их усреднить и получится поправка, которую необходимо вносить.

  14. Понятно. Функцию написал – заработала (не с первого раза конечно). Ток все равно погрешность углов нарастает. Видимо одного простенького гироскопа маловато.

  15. Привет, тоже работаю с гироскопом, подобным (LIS3DSH, регистры совпадают), но у меня чего-то значения не меняются, после перезапуска получаю какие-то данные, все следующие получаю такие же.
    инициализация такая же как и тут, записиваю 0x0F в регистр 0x20, жду 100мс, считываю данные. у кого-то было подобное?

  16. Добрый день. У меня xPosition постоянно растет. даже если плата просто лежит. Если ресетнуть и сразу повернуть, то значение 9,xxx, а не 90, как у вас.

    Буду признателен 🙂

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

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

Добавить комментарий

Ваш e-mail не будет опубликован. Обязательные поля помечены *