Идея реализации компаса на 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ссылка на оригинал статьи http://habrahabr.ru/post/259473/
Добавить комментарий