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;

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

xResult = receiveData[0] | (receiveData[1] << 8);
xResult &= 0x7FFF;
xResult = 0x7FFF - 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 = 0x7FFF - 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 определяется верно:
Работа программы

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

/*******************************************************************/
#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[8];;
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;
    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 = 0x7FFF - 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.: 36 комментариев
  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, как у вас.

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

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

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

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