Pull to refresh

Компас с ШИМ на отладочной плате stm32f3discovery

Reading time 9 min
Views 9K
Идея реализации компаса на stm32 с ШИМ (широкоимпульсная модуляция) — такая фича, когда интенсивность между соседними светодиодами меняется в зависимости от угла поворота компаса. Но для начала необходимо просто закодить обычный компас (чтобы просто показывал на север).

Вся работа выполнялась в CooCox — среда программирования от Eclipse (довольно глючная). Можно было использовать STM32Cub MX — гораздо более удобная прога, но сложная (авторский коллектив так и не разобрался).

Для того, чтобы можно было работать с таким плюшками, как акселерометр, магнитометр (да всё это есть в маленьком чипе на stm-ке) нужно подключить множество сторонних библиотек (на самом деле авторский коллектив не хотел ручками писать огромное количество кода).

Секция include:
#include «stm32f30x.h» — для работы с микроконтроллером;
#include«stm32f30x_gpio.h» — для работы с переферией (светодиодики);
#include«stm32f30x_i2c.h» — для обмена данными между акселерометром-магнетрометром и микросхемой;
#include«stm32f30x_rcc.h» — для подачи питания на переферию;
#include«stm32f3_discovery_lsm303dlhc.h» — плюшка для работы с акселерометром-магнетрометром;
#include «stm32f30x_exti.h» — надо;
#include «stm32f30x_syscfg.h» — тоже надо;
#include <stdio.h> — функции из C;
#include «math.h» — для мат. функций (аля sin, cos);
#include «stm32f3_discovery.h» — для облегчения роботы коллективу с платой;
#include «stm32f30x_tim.h» — таймеры.

После нескольких дней поиска в интернете этого добра необходимо было всё подключить в CooCox — и тут-то начались первые проблемы.

Со стандартными библиотеками а-ля stm32f30x_gpio.h, stm32f30x_tim.h всё было прозаично, но со сторонними (stm32f3_discovery_lsm303dlhс и stm32f3_discovery) возникла маленькая трудность. Необходимо было вручную прописывать пути и кидать их в нужные папки, а там их много и не понятно, какие использовать.

В иерархической структуре Project нужно было их кинуть в cmis_lib/include (заголовочные). Второй путь: cmis_lib/source (с — файлы). Но после этого компилятор продолжит ругаться. Чтобы компилятор узпакоился, нужно продублировать в двух библиотеках, header файлах:

#include «stm32f30x.h»
#include «stm32f30x_exti.h»
#include «stm32f30x_syscfg.h»
#include«stm32f30x_gpio.h»
#include«stm32f30x_i2c.h»
#include«stm32f30x_rcc.h»
#include«stm32f30x_usart.h»
#include«stm32f30x_spi.h»
#include«stm32f30x_misc.h»

Теперь можно начинать работать…

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

Чтобы реализовать компас нужно работать со встроенной в плату микросхемой LSM303DLHC. Здесь есть всё для работы с будущим компасом: магнетрометр (для считывания данных о магнитном поле) и акселерометр (для корректировки показаний в связи с наклоном платы).

Для начала нужно вычислить азимут между направлением вектора магнитного поля земли и осью Х магнитометра. Для вычисления азимута потребуются данные о величине магнитного поля вдоль осей X и Y магнитометра. Это проекции вектора магнитного поля земли на оси X и Y. Тогда для вычисления угла можно вспомнить уроки геометрии и применить простейшую формулу: arctg(Y/X), где Y и X – величины проекций на оси Y и X соответственно.

Данные манипуляции подойдут для абсолютно ровной поверхности. Когда нет отклонений по различным углам. Но рука человка имеет свойство трястись и не знает о ровной поверхности абсолютно ничего. Для решения такой проблемы был использован акселерометр, который ведёт учёт этого наклона.

Также в проекте не обойтись без данных об ускорении по каждой из осей. Они позволят рассчитать углы крена и тангажа, которые впоследствии будут учитываться при вычислении направления на север (читай Википедию). Вычитывание данных из магнетометра и акселерометра происходит путем использования функций расписаны ниже:

Листинг 1. Функция для чтения данных акселерометра
void Demo_CompassReadAss(float *pfData)
{
  int16_t pnRawData[3];
  uint8_t ctrlx[2];
  uint8_t buffer[6], cDivider;
  uint8_t i = 0;
  float LSM_Acc_Sensitivity = LSM_Acc_Sensitivity_2g;


  LSM303DLHC_Read(ACC_I2C_ADDRESS, LSM303DLHC_CTRL_REG4_A, ctrlx,2);//чтение данных из регистров акселерометра
  LSM303DLHC_Read(ACC_I2C_ADDRESS, LSM303DLHC_OUT_X_L_A, buffer, 6);

  if(ctrlx[1]&0x40)
    cDivider=64;
  else
    cDivider=16;


  if(!(ctrlx[0] & 0x40) || (ctrlx[1] & 0x40))
  {
    for(i=0; i<3; i++)
    {
      pnRawData[i]=((int16_t)((uint16_t)buffer[2*i+1] << 8) + buffer[2*i])/cDivider;//заносим данные в массив (нужно же откуда-то считывать)
    }
  }
  else
  {
    for(i=0; i<3; i++)
      pnRawData[i]=((int16_t)((uint16_t)buffer[2*i] << 8) + buffer[2*i+1])/cDivider;
  }

  LSM303DLHC_Read(ACC_I2C_ADDRESS, LSM303DLHC_CTRL_REG4_A, ctrlx,2);


  if(ctrlx[1]&0x40)
  {

    LSM_Acc_Sensitivity = 0.25;
  }
  else
  {


    switch(ctrlx[0] & 0x30)//выбираем уровень чувствительности микросхемы
    {
    case LSM303DLHC_FULLSCALE_2G:
      LSM_Acc_Sensitivity = LSM_Acc_Sensitivity_2g;
      break;
    case LSM303DLHC_FULLSCALE_4G:
      LSM_Acc_Sensitivity = LSM_Acc_Sensitivity_4g;
      break;
    case LSM303DLHC_FULLSCALE_8G:
      LSM_Acc_Sensitivity = LSM_Acc_Sensitivity_8g;
      break;
    case LSM303DLHC_FULLSCALE_16G:
      LSM_Acc_Sensitivity = LSM_Acc_Sensitivity_16g;
      break;
    }
  }


  for(i=0; i<3; i++)
  {
    pfData[i]=(float)pnRawData[i]/LSM_Acc_Sensitivity;//преобразование данные с учётом чувствительности и заносим в итоговый массив 
  }

}<source>
<b>Листинг 2. Функция для чтения данных магнитометра</b>
<source>void Demo_CompassRegMag(float *pfData)
{
    static int16_t buffer[3]={0};
    uint8_t *ptr=buffer;
    uint8_t CTRLB=0;
    uint16_t Magn_Sensitivity_XY=0, Magn_Sensitivity_Z=0;
    uint8_t i=0;
    float fpfData[3]={0.0f};

    LSM303DLHC_Read(MAG_I2C_ADDRESS, LSM303DLHC_CRB_REG_M, &CTRLB, 1);//опять считываем данные
    LSM303DLHC_Read(MAG_I2C_ADDRESS, LSM303DLHC_OUT_X_H_M, ptr, 1);
    LSM303DLHC_Read(MAG_I2C_ADDRESS, LSM303DLHC_OUT_X_L_M, ptr+1, 1);
    LSM303DLHC_Read(MAG_I2C_ADDRESS, LSM303DLHC_OUT_Y_H_M, ptr+2, 1);
    LSM303DLHC_Read(MAG_I2C_ADDRESS, LSM303DLHC_OUT_Y_L_M, ptr+3, 1);
    LSM303DLHC_Read(MAG_I2C_ADDRESS, LSM303DLHC_OUT_Z_H_M, ptr+4, 1);
    LSM303DLHC_Read(MAG_I2C_ADDRESS, LSM303DLHC_OUT_Z_L_M, ptr+5, 1);

    switch(CTRLB & 0xE0)//чувствительность микросхемы
      {
      case LSM303DLHC_FS_1_3_GA:
        Magn_Sensitivity_XY = LSM303DLHC_M_SENSITIVITY_XY_1_3Ga;
        Magn_Sensitivity_Z = LSM303DLHC_M_SENSITIVITY_Z_1_3Ga;
        break;
      case LSM303DLHC_FS_1_9_GA:
        Magn_Sensitivity_XY = LSM303DLHC_M_SENSITIVITY_XY_1_9Ga;
        Magn_Sensitivity_Z = LSM303DLHC_M_SENSITIVITY_Z_1_9Ga;
        break;
      case LSM303DLHC_FS_2_5_GA:
        Magn_Sensitivity_XY = LSM303DLHC_M_SENSITIVITY_XY_2_5Ga;
        Magn_Sensitivity_Z = LSM303DLHC_M_SENSITIVITY_Z_2_5Ga;
        break;
      case LSM303DLHC_FS_4_0_GA:
        Magn_Sensitivity_XY = LSM303DLHC_M_SENSITIVITY_XY_4Ga;
        Magn_Sensitivity_Z = LSM303DLHC_M_SENSITIVITY_Z_4Ga;
        break;
      case LSM303DLHC_FS_4_7_GA:
        Magn_Sensitivity_XY = LSM303DLHC_M_SENSITIVITY_XY_4_7Ga;
        Magn_Sensitivity_Z = LSM303DLHC_M_SENSITIVITY_Z_4_7Ga;
        break;
      case LSM303DLHC_FS_5_6_GA:
        Magn_Sensitivity_XY = LSM303DLHC_M_SENSITIVITY_XY_5_6Ga;
        Magn_Sensitivity_Z = LSM303DLHC_M_SENSITIVITY_Z_5_6Ga;
        break;
      case LSM303DLHC_FS_8_1_GA:
        Magn_Sensitivity_XY = LSM303DLHC_M_SENSITIVITY_XY_8_1Ga;
        Magn_Sensitivity_Z = LSM303DLHC_M_SENSITIVITY_Z_8_1Ga;
        break;
      }
      fpfData[0]=(((float)buffer[0])/1000)/Magn_Sensitivity_XY;//Преобразуем данные с учётом чувствительности
      fpfData[1]=(((float)buffer[1])/1000)/Magn_Sensitivity_XY;
      fpfData[2]=(((float)buffer[2])/1000)/Magn_Sensitivity_Z;
      for(i=0;i<3;i++)
      {
     	 pfData[i]=fpfData[i]*(-1); //Заносим в итоговый массив. для правильного отображения "на север" умножаем на -1 
      }
}<source>

Для вычисления углов сначала производится расчет значения модуля ускорения: 

fNormAcc=sqrt((AccBuffer[0]*AccBuffer[0])+(AccBuffer[1]*AccBuffer[1])+(AccBuffer[2]*AccBuffer[2]));

Затем, используя стандартные тригонометрические формулы для расчета косинусов и синусов углов (зная значения длин катетов и гипотенузы), получаем выражения для углов крена (Roll) и тангажа (Pitch):

fSinRoll = -AccBuffer[1]/
fNormAcc;
fCosRoll = sqrt(1.0 - (fSinRoll *fSinRoll));
fSinPitch = AccBuffer[0]/fNormAcc;
fCosPitch = sqrt(1.0 -(fSinPitch * fSinPitch));
Потом рассчитывается значение самих углов крена и тангажа:
RollAng = acos(fCosRoll) * 180/PI;
PitchAng = acos(fCosPitch) *180/PI;

При этом в зависимости от знака синуса и косинуса необходимо прибавить 180 либо 360 градусов, чтобы угол оказался в нужном квадранте.

Зная величину углов крена и тангажа, можно внести соответствующие поправки в формулу для вычисления угла между осью Х и направлением на север. Значение величины магнитного поля вдоль оси Х с учетом поправок:

fTitledX=MagBuffer[0]*fCosPitch+MagBuffer[2]*fSinPitch;
fTitledY=MagBuffer[0]*fSinRoll*fSinPitch+MagBuffer[1]*fCosRoll-MagBuffer[1]*fSinRoll*fCosPitch;

Вычисленное значение угла:

HeadingValue= 180.0f - (float)((atan2f((float)fTitledY,(float)fTitledX))*180)/PI;//Раз бежим на юг, значит обращаем на север

Осталось всего-ничего.
Первый этап позади: есть данные о магнитном поле в 3D пространстве.
Теперь необходимо всё это наглядно продемонстрировать. Для этого отладочная плата снабжена светодиодным табло. Картинка ниже.

Займёмся ШИМ-модуляцией. 

Для управление интенсивность светодиодов нужно использовать таймеры. Нужно проинициализировать  структуру включения светодиодов (описать соответсвующие Pin-ы и Pin-ы таймера). Однако. если первые четыре светодиода аппаратно подключены на таймер1 (TIM1) - читай datasheet, то оставшиеся светодиоды этого счастья лишены и необходимо в ручную подключать их к Pin-ам другого таймера, ибо каналов таймера всего 4, а светодиодов - 8.

Лезем в datasheet и находим свободный таймер (TIM3). Его каналы: PC6, PC7, PC8, PC9. Теперь необходимо узнать каналы, на которые подключены светодиоды без таймера. Такую информацию мы получаем из user manual. Это светодиоды: led4, led5, led6, led9.  Соответственно, их Pin-ы: PE8, PE10, PE12, PE15.

Теперь остаётся дело за малым: нужно соеднить соответствующие Pin-ы светодиодов с Pin- ами таймера и проинициилизировать две структуры (для TIM1 и TIM3):

PE8-PC6
PE10-PC7
PE12-PC8
PE15-PC9.

Далее приведён кусок кода для инициализации таймера 1. таймер 3 инициализируется аналогично.

<source>{
	        GPIO_StructInit(&gpio2);
		 gpio2.GPIO_Mode = GPIO_Mode_AF;
		 gpio2.GPIO_Pin = GPIO_Pin_6 |  GPIO_Pin_7 |  GPIO_Pin_8 | GPIO_Pin_9;
	         gpio2.GPIO_Speed=GPIO_Speed_Level_1;
	         gpio2.GPIO_OType=GPIO_OType_PP;
		 gpio2.GPIO_PuPd=GPIO_PuPd_NOPULL;
		 RCC_AHBPeriphClockCmd(RCC_AHBPeriph_GPIOC,ENABLE);
		 GPIO_Init(GPIOC,&gpio2);
		 GPIO_PinAFConfig(GPIOC,GPIO_PinSource6,GPIO_AF_2);//PC6-PE8(LD4)
		  GPIO_PinAFConfig(GPIOC,GPIO_PinSource7,GPIO_AF_2);//PC7-PE10(LED5)
		  GPIO_PinAFConfig(GPIOC,GPIO_PinSource8,GPIO_AF_2);//PC8-PE12(LED9)
		  GPIO_PinAFConfig(GPIOC,GPIO_PinSource9,GPIO_AF_2);//PC9-PE15(LED6)
		    //тактирование
		  RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM3,ENABLE);
	    //настройка тимера
	    TIM_TimeBaseStructInit(&timer3);
	    timer3.TIM_Prescaler = 720;
	    timer3.TIM_Period = 100;
	    TIM_TimeBaseInit(TIM3, &timer3);
	
	    TIM_OCStructInit(&timer3_oc);
	    timer3_oc.TIM_OCMode=TIM_OCMode_PWM1;
	    timer3_oc.TIM_OutputState=TIM_OutputState_Enable;
	    TIM_OC1Init(TIM3, &timer3_oc);//LED4
	    TIM_OC2Init(TIM3, &timer3_oc);//LED5
	    TIM_OC3Init(TIM3, &timer3_oc);//LED9
	    TIM_OC4Init(TIM3, &timer3_oc);//LED6
	    TIM3->CCR1=10;
	    //конфигурация компаратора.
	    TIM_SetCompare1(TIM3,0);
	    TIM_SetCompare2(TIM3,0);
	    TIM_SetCompare3(TIM3,0);
	    TIM_SetCompare4(TIM3,0);

	    TIM_CCxCmd(TIM3,TIM_Channel_1,TIM_CCx_Enable);//привязка вывода первого таймера к LED4 светодиоду
	    TIM_CCxCmd(TIM3,TIM_Channel_2,TIM_CCx_Enable);//привязка вывода второго таймера к LED5 светодиоду
	    TIM_CCxCmd(TIM3,TIM_Channel_3,TIM_CCx_Enable);//привязка вывода третьего таймера к LED9 светодиоду
	    TIM_CCxCmd(TIM3,TIM_Channel_4,TIM_CCx_Enable);//привязка вывода четвертого таймера к LED8 светодиоду
	    TIM_Cmd(TIM3,ENABLE);
}<source>

На последнем этапе нужно написать не много - не мало 8 условий для зажигания соответствующих светодиодов в зависимости от угла поворота.

Будет использовано две формулы: для уменьшения и увеличения интенсивности. В первом случае из максимального интервала необходимо вычитать величину HadingValue. Во втором, из меняющейся величины (HadingValue) вычитаем нижнюю границу угла.

Для примера, условие для интервала от 0 до 45 градусов. для остальных - аналогично.

void onLED(void)
{

	 if(HeadingValue < 0)
	     		{
	     		HeadingValue=HeadingValue+360;
	     		}
	     		if ((fRollAng <= 40.0f) && (fPitchAng <= 40.0f))//на LED10
	     		{
	     		    		 if (((HeadingValue < 25.0f)&&(HeadingValue >= 0.0f))||((HeadingValue >=340.0f)&&(HeadingValue <= 360.0f)))
	     		    		 {

	     		    			 TIM_SetCompare1(TIM1,0);
	     		    			 TIM_SetCompare2(TIM1,0);
	     		    			 TIM_SetCompare3(TIM1,((int)(45.0f-HeadingValue))*2);//LED10
	     		    			 TIM_SetCompare4(TIM1,((int)(HeadingValue-0))*2);//led8
	     		    			 //----------------------
	     		    			 TIM_SetCompare1(TIM3,0);
	     		    			 TIM_SetCompare2(TIM3,0);
	     		    			 TIM_SetCompare3(TIM3,0);
	     		    			 TIM_SetCompare4(TIM3,0);
 }
/*.....
*/
}
 
Чтобы компас обновлял данные по автоматическому прерыванию (раз в 1 мс) было проинициализировано таймерное прерывание, в которое входит вычисление угла и зажигание соответсвующих светодиодов. Для полного счастья ещё приведена инициализация таймера прерывания через TIM2.

<source>//инициализация таймера
void timer_init(void)
{
	RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM2, ENABLE);
	TIM_TimeBaseStructInit(&timer);
	timer.TIM_Prescaler=720;
	timer.TIM_Period=100;//выставлено значение в 2 секунды
	TIM_TimeBaseInit(TIM2,&timer);
	TIM_ITConfig(TIM2,TIM_IT_Update,ENABLE);
	TIM_Cmd(TIM2,ENABLE);
	NVIC_EnableIRQ(TIM2_IRQn);
}

//таймерное прерывание
void TIM2_IRQHandler(void)
{
		calcangle();
		onLED();
		TIM_ClearFlag(TIM2, TIM_FLAG_Update);
}<source>

Ну и под конец объявление глобальных переменных и функция main:

<source>uint16_t intensity=0;
TIM_OCInitTypeDef timer_oc;
RCC_ClocksTypeDef RCC_Clocks;
TIM_TimeBaseInitTypeDef timer;
TIM_TimeBaseInitTypeDef timer3;
TIM_OCInitTypeDef timer3_oc;
GPIO_InitTypeDef gpio;
GPIO_InitTypeDef gpio2;
float fNormAcc=0.0f;
float HeadingValue=0.0f;
float fSinRoll=0.0f,fCosRoll=0.0f;
float fSinPitch=0.0f, fCosPitch=0.0f;
float fRollAng=0.0f,fPitchAng=0.0f;
float fTitledX=0.0f, fTitledY=0.0f;
uint32_t TimingDelay = 0;

const float PI=3.14;

float MagBuffer[3]={0.0f};
float AccBuffer[3]={0.0f};


int main(void)
{
    //включение тактирования
	RCC_GetClocksFreq(&RCC_Clocks);
	SysTick_Config(RCC_Clocks.HCLK_Frequency / 100);
	//инициализация светодоиодов
	timer3_init();//инициализация 1 и 3 таймера
	timer1_init();
	//устанавливаем акселерометр и магнетометр
	Demo_CompassConfig();
	//calcangle();
	//onLED();
	//запуск таймера
	timer_init();

    while(1)
    {

    }
}<source>

Вот и всё!

Ссылка на проект: https://yadi.sk/d/s7vAizVKh3jFx

Datasheet: https://yadi.sk/i/kmUrRQzeh3jJ9

User Manual: https://yadi.sk/i/IxbW4Gn-h3jKa
Tags:
Hubs:
-4
Comments 5
Comments Comments 5

Articles