STM Урок 49. HAL. Магнитометр LSM303DLHC. Часть 2



 Урок 49

 

Часть 2

 

HAL. Магнитометр LSM303DLHC

 

В прошлой части урока мы кратко изучили документацию на магнитометр, создали для него проект, добавили некоторые макросы и написали функцию инициализации датчика.

Добавим функцию считывания данных с осей магнитометра, скопировав и исправив подобную для осей акселерометра

 

//———————————————

void Mag_GetXYZ(int16_t* pData)

{

        uint8_t buffer[6];

        uint8_t i=0;

        

        buffer[0] = Accel_IO_Read(MAG_I2C_ADDRESS,LSM303DLHC_OUT_X_H_M);

        buffer[1] = Accel_IO_Read(MAG_I2C_ADDRESS,LSM303DLHC_OUT_X_L_M);

        buffer[2] = Accel_IO_Read(MAG_I2C_ADDRESS,LSM303DLHC_OUT_Y_H_M);

        buffer[3] = Accel_IO_Read(MAG_I2C_ADDRESS,LSM303DLHC_OUT_Y_L_M);

        buffer[4] = Accel_IO_Read(MAG_I2C_ADDRESS,LSM303DLHC_OUT_Z_H_M);

        buffer[5] = Accel_IO_Read(MAG_I2C_ADDRESS,LSM303DLHC_OUT_Z_L_M);

                for(i=0;i<3;i++)

                {

                        if(pData[i]!=-4096)         pData[i]=((int16_t)((uint16_t)buffer[2*i]<<8)+buffer[2*i+1]);

                }

}

//———————————————

 

У функции Accel_ReadAcc поменяем имя на AccelMag_Read, также изменив её и раскомментировав в файле main.c в бесконечном цикле

 

//———————————————

void AccelMag_Read(void)

{

  /* USER CODE BEGIN 3 */

                AccelMag_Read();

  }

 

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

 

void AccelMag_Read(void)

{

        int16_t buffer[3] = {0};

        static int16_t val[3], tmp16;

        Mag_GetXYZ(buffer);

 

Пишем данную функцию дальше

 

        Mag_GetXYZ(buffer);

        //уберем переполнение из показаний осей

        tmp16=buffer[0];        if(tmp16!=-4096) val[0]=tmp16;

        tmp16=buffer[1];        if(tmp16!=-4096) val[1]=tmp16;

        tmp16=buffer[2];        if(tmp16!=-4096) val[2]=tmp16;

 

Из комментария здесь в принципе всё ясно.

Дальше немного подправим код

 

        tmp16=buffer[2];        if(tmp16!=-4096) val[2]=tmp16;

        sprintf(str1,»X:%06d Y:%06d Z:%06drn», val[0], val[1], val[2]);

        HAL_UART_Transmit(&huart2, (uint8_t*)str1,strlen(str1),0x1000);

//        buf2[0]=0x11;

//        buf2[1]=0x55;

//        buf2[2]=(uint8_t)(val[0]>>8);

//        buf2[3]=(uint8_t)val[0];

//        buf2[4]=(uint8_t)(val[1]>>8);

//        buf2[5]=(uint8_t)val[1];

//        buf2[6]=(uint8_t)(val[2]>>8);

//        buf2[7]=(uint8_t)val[2];

//        HAL_UART_Transmit(&huart2,buf2,8,0×1000);

 

Исправляем код дальше, поменяв в нём также местами светодиоды в связи с определённым положением осей магнитометра

 

//        HAL_UART_Transmit(&huart2,buf2,8,0×1000);

        if(val[0]>60)

        {

                LD10_ON;

                if(val[1]>60)

                {

                        LD10_OFF;

                        LD9_ON;

                }

                if(val[1]<-60)

                {

                        LD10_OFF;

                        LD8_ON;

                }

        }

        else if(val[0]<-60)

        {

                LD3_ON;

                if(val[1]>60)

                {

                        LD3_OFF;

                        LD5_ON;

                }

                if(val[1]<-60)

                {

                        LD3_OFF;

                        LD4_ON;

                }        

        }

        else

        {

                if(val[1]>60)

                {

                        LD7_ON;

                }

                if(val[1]<-60)

                {

                        LD6_ON;

                }        

        }

        HAL_Delay(10);

 

 

Соберем код, прошьем контроллер и посмотрим в терминале показания осей

 

image10

 

Покрутим плату относительно всех трёх и сверим показания. На основании данной сверки внесем корректировку нулей в код. То есть мы сверяем максимальное положительное показание по каждой оси с максимальным отрицательным показанием по той же оси, складываем их и результат делим пополам. Лучше всего ориентировать каждую ось вертикально в разных направлениях. Вообщем, если показания будут отличаться только знаком, то корректировать не нужно, а если будет отличие по модулю то корректируем по знаку в сторону меньшего.

Например, показания по X у нас в среднем -165 и 165, значит корректировка по данной оси нам не требуется.

Проделаем то же самое для остальных трёх осей. У меня получилось вот так

 

        tmp16=buffer[0];        if(tmp16!=-4096) val[0]=tmp16;

        tmp16=buffer[1];        if(tmp16!=-4096) val[1]=tmp16+40;

        tmp16=buffer[2];        if(tmp16!=-4096) val[2]=tmp16-50;

 

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

Теперь мы раскомментируем код для визуализации данных, а для текстового вывода наоборот закомментируем

 

        tmp16=buffer[2];        if(tmp16!=-4096) val[2]=tmp16-50;

//        sprintf(str1,»X:%06d Y:%06d Z:%06drn», val[0], val[1], val[2]);

//        HAL_UART_Transmit(&huart2, (uint8_t*)str1,strlen(str1),0x1000);

        buf2[0]=0x11;

        buf2[1]=0x55;

        buf2[2]=(uint8_t)(val[0]>>8);

        buf2[3]=(uint8_t)val[0];

        buf2[4]=(uint8_t)(val[1]>>8);

        buf2[5]=(uint8_t)val[1];

        buf2[6]=(uint8_t)(val[2]>>8);

        buf2[7]=(uint8_t)val[2];

        HAL_UART_Transmit(&huart2,buf2,8,0×1000);

 

Я немного подправил свои программки визуализации данных под собственно магнитометр. Запустим первую из них. Покрутим немного плату

 

image12

 

Из данной картины мы видим, что наш магнитометр работает нормально, только единственное, присутствует некоторый шум. Давайте всё-таки сочиним некоторый низкочастотный фильтр. Самым простейшим фильтром, но и в то же время вполне работающим мне показалось скользящее среднее. Я исследовал некоторое количество передового опыта по данной теме, и всё-таки решил сочинить свою реализацию. Вообще скользящее среднее основано на том, что мы собираем определённое количество показаний подряд каждого измеренного значения, вычисляем их среднее арифметическое и используем его уже на выходе. Затем после следующего измеренного значения мы берем столько же последних показаний и также вычисляем их среднее арифметическое. Но на деле данная операция проводится немного не так. Мы сначала набираем буфер из определенного количества измеренных подряд показаний, находим среднее арифметическое, но также запоминаем их сумму, не разделенную ещё на количество показаний. Затем мы к данной сумме прибавляем следующее измеренное показание и вычитаем самое старое показание, а затем делим на количество показаний. И так далее. То есть при больших количествах значений нам не приходится каждый раз складывать их, что вносит оптимизацию в код и экономит процессорное время.

 

 

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

Вот, собственно и код функции вычисления скользящего среднего. Сначала добавим глобальные переменные

 

char str1[30]={0};

//буферы для скользящего среднего

volatile int16_t xbuf_avg[8]={0},ybuf_avg[8]={0},zbuf_avg[8]={0};

//счётчик наполнения буферов скользящего среднего

volatile int8_t avg_cnt;

//сумма для среднего арифметического

volatile int64_t tmp64[3];

 

Теперь сама функция

 

//———————————————

void MovingAverage(int16_t* dt)

{

        if(avg_cnt<8)

        {

                xbuf_avg[avg_cnt]=dt[0];

                ybuf_avg[avg_cnt]=dt[1];

                zbuf_avg[avg_cnt]=dt[2];

                if(avg_cnt==7)

                {

                        tmp64[0]=xbuf_avg[7]+xbuf_avg[6]+xbuf_avg[5]+xbuf_avg[4]+xbuf_avg[3]+xbuf_avg[2]+xbuf_avg[1]+xbuf_avg[0];

                        tmp64[1]=ybuf_avg[7]+ybuf_avg[6]+ybuf_avg[5]+ybuf_avg[4]+ybuf_avg[3]+ybuf_avg[2]+ybuf_avg[1]+ybuf_avg[0];

                        tmp64[2]=zbuf_avg[7]+zbuf_avg[6]+zbuf_avg[5]+zbuf_avg[4]+zbuf_avg[3]+zbuf_avg[2]+zbuf_avg[1]+zbuf_avg[0];

                        dt[0]=tmp64[0]/8;

                        dt[1]=tmp64[1]/8;

                        dt[2]=tmp64[2]/8;

                }

                avg_cnt++;

        }

        else

        {

                //вычтем из общих сумм последние элементы

                tmp64[0]-=xbuf_avg[0];

                tmp64[1]-=ybuf_avg[0];

                tmp64[2]-=zbuf_avg[0];

                //сдвинем буферы на 1 элемент

                memcpy((void*)xbuf_avg,(void*)(xbuf_avg+1),sizeof(int16_t)*7);

                memcpy((void*)ybuf_avg,(void*)(ybuf_avg+1),sizeof(int16_t)*7);

                memcpy((void*)zbuf_avg,(void*)(zbuf_avg+1),sizeof(int16_t)*7);

                //заменим на 7 элементы на новые

                xbuf_avg[7]=dt[0];

                ybuf_avg[7]=dt[1];

                zbuf_avg[7]=dt[2];

                //прибавим новые элементы

                tmp64[0]+=dt[0];

                tmp64[1]+=dt[1];

                tmp64[2]+=dt[2];

                //обновим средние значения

                dt[0]=tmp64[0]/8;

                dt[1]=tmp64[1]/8;

                dt[2]=tmp64[2]/8;                

        }

}

//———————————————

 

В функции инициализации AccelMag_Ini добавим инициализацию счётчика

 

        uint32_t ctrl32 = 0x00000000;

        //инициализируем буфер скользящего среднего

        avg_cnt=0;//счетчик заполнения

 

В функции AccelMag_Read вызовем нашу функцию фильтрации

 

        tmp16=buffer[2];        if(tmp16!=-4096) val[2]=tmp16-50;

        MovingAverage(val);

 

Скомпилируем код, прошьём контроллер и посмотрим, что у нас получится

 

image13

 

Я думаю, изменения налицо.

Теперь уберём все корректировки

 

        tmp16=buffer[0];        if(tmp16!=-4096) val[0]=tmp16;

        tmp16=buffer[1];        if(tmp16!=-4096) val[1]=tmp16;

        tmp16=buffer[2];        if(tmp16!=-4096) val[2]=tmp16;

 

и запустим другую программу визуализации

 

image14

 

Но шарик у нас быстро укатился, так как постоянно действуют магнитные поля. Откорректируем теперь показания по осям в коде, чтобы они вообще обнулились

 

//уберем переполнение из показаний осей

        tmp16=buffer[0];        if(tmp16!=-4096) val[0]=tmp16+6;

        tmp16=buffer[1];        if(tmp16!=-4096) val[1]=tmp16-64;

        tmp16=buffer[2];        if(tmp16!=-4096) val[2]=tmp16+90;

 

Запустим заново программу визуализации. Шарик теперь будет укатываться тогда, когда мы будем подносить магнит, а также вращать его

 

image15

 

Ну вот собственно и всё. Я думаю, мы неплохо познакомились и научились работать с данным магнитометром

 

 

Предыдущая часть Программирование МК STM32 Следующий урок

 

Исходный код

 

Техническая документация на датчик

Программа Hyper Terminal

Программа NS Port Monitor

Программа NS Mag Visual

 

 

Купить отладочную плату можно здесь STM32F3-DISCOVERY

 

 

Смотреть ВИДЕОУРОК

 

STM32 HAL. Магнитометр LSM303DLHC

Добавить комментарий

Ваш e-mail не будет опубликован. Обязательные поля помечены *

*