STM32 и гироскоп L3GD20. Часть 2. Определение положения платы.

Гироскоп L3GD20

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

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

Гироскоп L3GD20.

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

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

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

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

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

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

Обмен данными по 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);

Не забываем дергать линию Chip Select (вывод 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);
Настройка гироскопа L3GD20.

То есть в соответствии с документацией – 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 определяется верно:

Результат выполнения программы.

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

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

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

/***************************************************************************************/
#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;
	}
}


/***************************************************************************************/

Поделиться!

Подписаться
Уведомление о
guest
37 Комментарий
старее
новее большинство голосов
Inline Feedbacks
View all comments
Стас
Стас
6 лет назад

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

Стас
Стас
6 лет назад

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

Андрей
Андрей
6 лет назад

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

Саша
Саша
6 лет назад

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

Александр
Александр
6 лет назад

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

Вадим
Вадим
5 лет назад

Если байты положить в память в том же порядке, как они у гироскопа лежат, то никаких операций сдвига не потребуется. Если же ещё использовать многократное чтение, то для чтения значения по всем 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

Дима
Дима
5 лет назад

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

Паша
Паша
5 лет назад

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

Паша
Паша
5 лет назад

Спасибо. Не внимателен был.

Andry
Andry
5 лет назад

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

Павел
Павел
4 лет назад

Разобрался с function «TIM2_IRQHandler» has no prototype

Павел
Павел
4 лет назад

У меня таже проблема, как решил?

Павел
Павел
4 лет назад

На какой адрес скинуть можно?

Павел
Павел
4 лет назад

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

Алексей
Алексей
4 лет назад

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

lyalyatopolya
4 лет назад

Есть одно замечание. Если я не прав, прошу простить. У 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.

Sergey
Sergey
4 лет назад

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

Sergey
Sergey
4 лет назад

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

Sergey
Sergey
4 лет назад
Tygran
Tygran
4 лет назад

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

Александр
3 лет назад

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

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

Стас
Стас
2 лет назад

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

Присоединяйтесь!

Profile Profile Profile Profile Profile
Vkontakte
Twitter

Язык сайта

Июнь 2020
Пн Вт Ср Чт Пт Сб Вс
« Май    
1234567
891011121314
15161718192021
22232425262728
2930  

© 2013-2020 MicroTechnics.ru