Top.Mail.Ru

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

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

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

L3GD20 axis.

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

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

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

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

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

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

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

43 комментариев
Старые
Новые
Межтекстовые Отзывы
Посмотреть все комментарии
Стас
Стас
10 лет назад

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

Andry
Andry
9 лет назад

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

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

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

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

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

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

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

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

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

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

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

lyalyatopolya
8 лет назад

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

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

Sergey
Sergey
8 лет назад

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

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

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

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

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

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

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

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

Nick
Nick
3 лет назад

Как у вас из адреса регистра 0x28 получилось 0xE8, не понимаю, разве не с 0x28 идет получение данных?

Nick
Nick
Ответ на комментарий  Aveal
3 лет назад

Дошло! Спасибо:)

Roman
1 год назад

Добры день! Подскажите пожалуйста как быть, когда адрес регистра состоит из 7 бит (и более). С учётом битов R/W и MS в байт уже не уложиться. Это про функцию: sendByte(0xE8);

43
0
Оставьте комментарий! Напишите, что думаете по поводу статьи.x