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

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

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

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

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

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

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

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

Время традиционной вставки: поскольку компания STMicroelectronics прекратила поддержку библиотеки SPL, которая использовалась в этом курсе, я создал новый, посвященный работе уже с новыми инструментами, так что буду рад видеть вас там - STM32CubeMx. Кроме того, вот глобальная рубрика по STM32, а также небольшая подборка на тему датчиков:

• ПИД-регулятор. Пример ПИД-регулятора температуры на STM32.
• Настройка ПИД-регулятора. Метод Циглера-Никольса.
• Подключение датчика температуры DS18B20 к STM32. Модуль KY-001.
• Подключение магнитного энкодера AS5048 к микроконтроллеру.

Считываем значения уже упомянутых регистров 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);

Не забываем дергать линию 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);

То есть в соответствии с документацией - 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;
	}
}


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