SPL: комплементарный фильтр

Краткое описание: В примере рассматривается совместное использование гироскопа и акселерометра для фильтрации данных от датчика MPU9250.

Задача: Требуется написать с использованием библиотеки StdPeriph код. Программа должна производить настойку аппаратного i2c.  Используя стандартные функции библиотеки StdPeriph для работы с i2c прочитать данные с гироскопа и акселерометра. Используя формулу комплементарного фильтра фильтровать значение угла поворота относительно оси x. При повороте платы относительно оси x больше чем на 10 градусов, зажигать индикационный светодиод. 

Направление осей гироскопа и акселерометра 

#include "stm32f30x.h"
#include "stm32f30x_gpio.h"
#include "stm32f30x_rcc.h"
#include "stm32f30x_i2c.h"
#include "stm32f30x_syscfg.h"
#include "stm32f30x_tim.h"
#include <math.h>

#define ADR_MPU9250_W 0xD0 //(hex 0xD0 = bin 1101000 (адрес MPU9250)) + (bin 0 (write / бит записи))
#define ADR_MPU9250_R 0xD1 //(hex 0xD1 = bin 1101000 (адрес MPU9250)) + (bin 1 (read / бит чтения))

#define GYRO_CONFIG 0x1B //Адрес регистра выбора шкалы чувствительности гироскопа

//Переменная для выбора шкалы чувствительности гироскопа
//Возможные варианты 250 DPS (hex 0x00), 500 DPS (hex 0x08), 1000 DPS (hex 0x10), and 2000 DPS (hex 0x18)

#define GYRO_FULL_SCALE 0x00 //bin 0x00000000 (GYRO_FULL_SCALE = hex 0x00)

//Разрешение для шкалы гироскопа DPS / (16 bit ADC -> 2^16=65536/2)=32768

#define GYRO_RESOLUTION_SCALE_250_DPS 250.0 / 32768.0
#define GYRO_RESOLUTION_SCALE_500_DPS 500.0 / 32768.0
#define GYRO_RESOLUTION_SCALE_1000_DPS 1000.0 / 32768.0
#define GYRO_RESOLUTION_SCALE_2000_DPS 2000.0 / 32768.0

#define ACCEL_CONFIG 0x1C //Адрес регистра выбора шкалы чувствительности акселерометра

//Переменная для выбора шкалы чувствительности акселерометра
//Возможные варианты 2 Gs (hex 0x00), 4 Gs (hex 0x08), 8 Gs (hex 0x10), and 16 Gs (hex 0x18)

#define ACCEL_FULL_SCALE 0x00 //bin 0x00000000 (ACCEL_FULL_SCALE = hex 0x00)

//Разрешение для шкалы акселерометра DPS / (16 bit ADC -> 2^16=65536/2)=32768

#define ACCEL_RESOLUTION_SCALE_2_DPS 2.0 / 32768.0
#define ACCEL_RESOLUTION_SCALE_4_DPS 4.0 / 32768.0
#define ACCEL_RESOLUTION_SCALE_8_DPS 8.0 / 32768.0
#define ACCEL_RESOLUTION_SCALE_16_DPS 16.0 / 32768.0

#define GYRO_XOUT_H 0x43 //Адрес регистра гироскопа с измеренными значениями
#define GYRO_XOUT_L 0x44 //Адрес регистра гироскопа с измеренными значениями

#define ACCEL_XOUT_H 0x3D //Адрес регистра акселерометра с измеренными значениями
#define ACCEL_XOUT_L 0x3C //Адрес регистра акселерометра с измеренными значениями

#define OWN_ADR 0x15 //Собственный адрес устройства i2c1
#define TIMING_I2C1 200000 //Частота тактового сигнала i2c1

#define PRESCALER_TIM2 720 //Пределитель чистоты для таймера 2
#define PERIOD_TIM2 1000 //Период таймера 10 миллисекунд (1 единица значения равна 0,01 миллисекунде (10 микросекунд))

GPIO_InitTypeDef gpio; //Объявляем переменную gpio типа GPIO_InitTypeDef
I2C_InitTypeDef i2c; //Объявляем переменную i2c типа I2C_InitTypeDef
TIM_TimeBaseInitTypeDef timer; //Объявляем переменную timer типа TIM_TimeBaseInitTypeDef

unsigned char G_X_DATA_FROM_MPU9250 = 0; //Переменная для работы c данными гироскопа MPU9250
uint16_t G_X_RESULT = 0; //Переменная для работы c данными гироскопа MPU9250
float G_X_AXIS_POSITION = 0; //Переменная для работы c данными гироскопа MPU9250
int G_X_SIGN; //Переменная для работы c данными гироскопа MPU9250

unsigned char A_X_DATA_FROM_MPU9250 = 0; //Переменная для работы c данными акселерометром MPU9250
uint16_t A_X_RESULT = 0; //Переменная для работы c данными акселерометром MPU9250
float A_X_AXIS_POSITION = 0; //Переменная для работы c данными акселерометром MPU9250
int A_X_SIGN; //Переменная для работы c данными акселерометром MPU9250

float FACTOR_FILTER = 0.01; //Переменная для работы c комплиментарным фильтром
float ANGLE_FROM_AXEL_AXIS_X = 0; //Переменная для работы c комплиментарным фильтром
float ANGLE_AXIS_X_AFTER_FILTER = 0; //Переменная для работы c комплиментарным фильтром
float pi = 3.1415926535; //Значение для работы c комплиментарным фильтром

void initAll()
{
    RCC_DeInit(); //Сбрасываем настройки тактирования
    RCC_HSICmd(DISABLE); //Отключаем внутренний источник тактирования
    RCC_HSEConfig(RCC_HSE_ON); //Включаем внешний источник тактирования
    RCC_SYSCLKConfig(RCC_SYSCLKSource_HSE); //Подаем тактовый сигнал на системную шину

    //RCC_HSICmd(ENABLE); //Включаем тактирование от внутреннего источника
    //RCC_SYSCLKConfig(RCC_SYSCLKSource_HSI); //Подаем тактовый сигнал на системную шину

    RCC_APB1PeriphClockCmd(RCC_APB1Periph_I2C1, ENABLE); //Тактирование i2c1
    RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM2, ENABLE); //Тактирование таймера 2

    RCC_AHBPeriphClockCmd(RCC_AHBPeriph_GPIOC, ENABLE); //Тактирование порта C
    gpio.GPIO_Mode = GPIO_Mode_OUT; //Выбираем режим работы пинов
    gpio.GPIO_Pin = GPIO_Pin_6; //Выбираем нужны пин к которому подключен светодиод
    GPIO_Init(GPIOC, &gpio); //Инициализируем структуру с настройками

    RCC_AHBPeriphClockCmd(RCC_AHBPeriph_GPIOB, ENABLE); //Тактирование порта B
    GPIO_PinAFConfig(GPIOB, GPIO_PinSource8, GPIO_AF_4); //(SCL) Настраиваем ножку для работы в режиме альтернативной функции
    GPIO_PinAFConfig(GPIOB, GPIO_PinSource9, GPIO_AF_4); //(SDA) Настраиваем ножку для работы в режиме альтернативной функции

    //Настройки модуля i2c1
    i2c.I2C_Timing = TIMING_I2C1; //Частота тактового сигнала i2c1
    i2c.I2C_Mode = I2C_Mode_I2C; //Выбираем режим работы
    i2c.I2C_OwnAddress1 = OWN_ADR; //Собственный адрес устройства
    i2c.I2C_Ack = I2C_Ack_Enable; //Включение/отключения бита подтверждения ACK
    i2c.I2C_AcknowledgedAddress = I2C_AcknowledgedAddress_7bit; //Выбор формата адреса: 7 бит / 10 бит.
    I2C_Init(I2C1, &i2c); //Инициализация настроек

    //Настройки ножек i2c1
    gpio.GPIO_Pin = GPIO_Pin_8 | GPIO_Pin_9; //Выбираем нужные пины SCL/SDA
    gpio.GPIO_Mode = GPIO_Mode_AF; //Выбираем режим работы пинов
    gpio.GPIO_Speed = GPIO_Speed_50MHz; //Выбираем максимальную скорость
    gpio.GPIO_OType = GPIO_OType_OD; //Выход с открытым стоком
    gpio.GPIO_PuPd = GPIO_PuPd_UP; //Конфигурируем подтяжку
    GPIO_Init(GPIOB, &gpio); //Инициализируем структуру с настройками

    I2C_Cmd(I2C1, ENABLE); //Включение модуля i2c1

    TIM_TimeBaseStructInit(&timer); //Инициализация структуры

    timer.TIM_Prescaler = PRESCALER_TIM2; //Выбор пределителя
    timer.TIM_Period = PERIOD_TIM2; //Период досчитав до которого произойдет прерывание
    TIM_TimeBaseInit(TIM2, &timer); //Инициализация выбранных значений

    TIM_ITConfig(TIM2, TIM_IT_Update, ENABLE); //Настраивает таймер на генерирования прерывания по переполнению
    NVIC_EnableIRQ(TIM2_IRQn); //Разрешаем соответствующие прерывания
    TIM_Cmd(TIM2, ENABLE); //Запускаем таймер
}

int main()
{

    initAll(); //Вызов функции инициализации i2c1
    ok_enable_irq(); //Разрешаются прерывания с линии IRQ на контроллере " disable_irq и enable_irq управляeт битом в регистре PRIMASK (запрет/разрешение)"

    while (1) {
    }
}

void TIM2_IRQHandler() //Функция прерывания по переполнению таймера 2
{
    TIM_ClearITPendingBit(TIM2, TIM_IT_Update); //Функция очищает бит прерывания

    /*!!!!!!!!!!!!!Для работы с I2C1!!!!!!!!!!!!!*/
    //*******************************************************************************************************
    /*!!!!!!!!!!!!!Настраиваем чувствительность осей гироскопа через регистр GYRO_CONFIG!!!!!!!!!!!!!*/
    //*******************************************************************************************************
    while (I2C_GetFlagStatus(I2C1, I2C_FLAG_BUSY)) {}; //Ждем пока освободится шина i2c1

    /*Формируем первую посылку с адресом ведомого устройства и битом записи (ADR_MPU9250_W = hex 0xD0)/передаем два байта /выбираем режим работы модуля /генерируем старт записи*/

    I2C_TransferHandling(I2C1, ADR_MPU9250_W, 2, I2C_AutoEnd_Mode, I2C_Generate_Start_Write);

    while (I2C_GetFlagStatus(I2C1, I2C_ISR_TXE) == RESET) {}; //Ждем, установки флага TXE

    I2C_SendData(I2C1, GYRO_CONFIG); //Отправляем адрес регистра в который хотим записать данные (GYRO_CONFIG = hex 0x1B)

    while (I2C_GetFlagStatus(I2C1, I2C_ISR_TXE) == RESET) {}; //Ждем, установки флага TXE

    I2C_SendData(I2C1, GYRO_FULL_SCALE); //Отправляем данные в регистр GYRO_FULL_SCALE -> bin 0x00000000 (GYRO_FULL_SCALE = hex 0x00)

    while (I2C_GetFlagStatus(I2C1, I2C_ISR_TXE) == RESET) {}; //Ждем, установки флага TXE

    while (I2C_GetFlagStatus(I2C1, I2C_ISR_STOPF) == RESET); //Ждем установки флага STOPF

    I2C_ClearFlag(I2C1, I2C_ICR_STOPCF); //Очистить флаг STOPF

    //*************************************************************************************************************
    /*!!!!!!!!!!!!!Настраиваем чувствительность осей акселерометра через регистр ACCEL_CONFIG!!!!!!!!!!!!!*/
    //*************************************************************************************************************
    while (I2C_GetFlagStatus(I2C1, I2C_FLAG_BUSY)) {}; //Ждем пока освободится шина i2c1

    /*Формируем первую посылку с адресом ведомого устройства и битом записи (ADR_MPU9250_W = hex 0xD0)/передаем два байта /выбираем режим работы модуля /генерируем старт записи*/

    I2C_TransferHandling(I2C1, ADR_MPU9250_W, 2, I2C_AutoEnd_Mode, I2C_Generate_Start_Write);

    while (I2C_GetFlagStatus(I2C1, I2C_ISR_TXE) == RESET) {}; //Ждем, установки флага TXE

    I2C_SendData(I2C1, ACCEL_CONFIG); //Отправляем адрес регистра в который хотим записать данные (ACCEL_CONFIG = hex 0x1B)

    while (I2C_GetFlagStatus(I2C1, I2C_ISR_TXE) == RESET) {}; //Ждем, установки флага TXE

    I2C_SendData(I2C1, ACCEL_FULL_SCALE); //Отправляем данные в регистр ACCEL_FULL_SCALE -> bin 0x00000000 (ACCEL_FULL_SCALE = hex 0x00)

    while (I2C_GetFlagStatus(I2C1, I2C_ISR_TXE) == RESET) {}; //Ждем, установки флага TXE

    while (I2C_GetFlagStatus(I2C1, I2C_ISR_STOPF) == RESET); //Ждем установки флага STOPF

    I2C_ClearFlag(I2C1, I2C_ICR_STOPCF); //Очистить флаг STOPF

    //*****************************************************************************
    /*!!!!!!!!!!!!!Читаем данные из регистра GYRO_XOUT_H = hex 0x43!!!!!!!!!!!!!*/
    //*****************************************************************************

    while (I2C_GetFlagStatus(I2C1, I2C_FLAG_BUSY)) {}; //Ждем пока освободится шина i2c1

    /*Формируем первую посылку с адресом ведомого устройства и битом записи (ADR_MPU9250_W = hex 0xD0) /выбираем режим работы модуля /генерируем старт записи*/

    I2C_TransferHandling(I2C1, ADR_MPU9250_W, 1, I2C_AutoEnd_Mode, I2C_Generate_Start_Write);

    while (I2C_GetFlagStatus(I2C1, I2C_ISR_TXE) == RESET) {}; //Ждем, установки флага TXE

    I2C_SendData(I2C1, GYRO_XOUT_H); //Отправляем адрес регистра из которого хотим прочитать данные (GYRO_XOUT_H = hex 0x43)

    while (I2C_GetFlagStatus(I2C1, I2C_ISR_TXE) == RESET) {}; //Ждем, установки флага TXE

    /*Формируем посылку с адресом ведомого устройства и битом чтения (ADR_MPU9250_R = hex 0xD1) /выбираем режим работы модуля /генерируем старт чтения*/

    I2C_TransferHandling(I2C1, ADR_MPU9250_R, 1, I2C_AutoEnd_Mode, I2C_Generate_Start_Read);

    while (I2C_GetFlagStatus(I2C1, I2C_FLAG_RXNE) == RESET) {}; //Ждем, установки флага RXEN

    G_X_DATA_FROM_MPU9250 = I2C_ReceiveData(I2C1); //Запись данных в переменную Data

    while (I2C_GetFlagStatus(I2C1, I2C_FLAG_RXNE) != RESET) {}; //Ждем, установки флага RXEN

    /*!!!!!!!!!!!!!Читаем данные из регистра ACCEL_XOUT_H = hex 0x3B!!!!!!!!!!!!!*/

    while (I2C_GetFlagStatus(I2C1, I2C_FLAG_BUSY)) {}; //Ждем пока освободится шина i2c1

    /*Формируем первую посылку с адресом ведомого устройства и битом записи (ADR_MPU9250_W = hex 0xD0) /выбираем режим работы модуля /генерируем старт записи*/

    I2C_TransferHandling(I2C1, ADR_MPU9250_W, 1, I2C_AutoEnd_Mode, I2C_Generate_Start_Write);

    while (I2C_GetFlagStatus(I2C1, I2C_ISR_TXE) == RESET) {}; //Ждем, установки флага TXE

    I2C_SendData(I2C1, ACCEL_XOUT_H); //Отправляем адрес регистра из которого хотим прочитать данные (ACCEL_XOUT_H = hex 0x3B)

    while (I2C_GetFlagStatus(I2C1, I2C_ISR_TXE) == RESET) {}; //Ждем, установки флага TXE

    /*Формируем посылку с адресом ведомого устройства и битом чтения (ADR_MPU9250_R = hex 0xD1) /выбираем режим работы модуля /генерируем старт чтения*/

    I2C_TransferHandling(I2C1, ADR_MPU9250_R, 1, I2C_AutoEnd_Mode, I2C_Generate_Start_Read);

    while (I2C_GetFlagStatus(I2C1, I2C_FLAG_RXNE) == RESET) {}; //Ждем, установки флага RXEN

    A_X_DATA_FROM_MPU9250 = I2C_ReceiveData(I2C1); //Запись данных в переменную Data

    while (I2C_GetFlagStatus(I2C1, I2C_FLAG_RXNE) != RESET) {}; //Ждем, установки флага RXEN

    /*!!!!!!!!!!!!!По окончанию работы с I2С вызываем следующие функции!!!!!!!!!!!!!*/

    while (I2C_GetFlagStatus(I2C1, I2C_ISR_STOPF) == RESET); //Ждем установки флага STOPF

    I2C_ClearFlag(I2C1, I2C_ICR_STOPCF); //Очистить флаг STOPF

    G_X_RESULT = (G_X_DATA_FROM_MPU9250 << 8) | G_X_DATA_FROM_MPU9250; //Записываем полученные данные в переменную G_X_RESULT

    if (G_X_RESULT < 0x0A)
        G_X_RESULT = 0; //Игнорируем небольшой дрейф нуля

    if ((G_X_RESULT & 0x8000) == 0)
        G_X_SIGN = 0; //Проверяем направление вращения

    else {
        G_X_SIGN = 1;
        G_X_RESULT &= 0x7FFF;
        G_X_RESULT = 0x7FFF - G_X_RESULT;
    }

    if (G_X_SIGN == 0) {

        //Интегрируем полученное значение по времени: вращение в положительном направлении значит увеличиваем угол
        G_X_AXIS_POSITION += (GYRO_RESOLUTION_SCALE_250_DPS)*G_X_RESULT * 0.01;
    }

    else {

        //Интегрируем полученное значение по времени: вращение в отрицательном направлении значит уменьшаем угол
        G_X_AXIS_POSITION -= (GYRO_RESOLUTION_SCALE_250_DPS)*G_X_RESULT * 0.01;
    }

    A_X_RESULT = (A_X_DATA_FROM_MPU9250 << 8) | A_X_DATA_FROM_MPU9250; //Записываем полученные данные в переменную A_X_RESULT

    if ((A_X_RESULT & 0x8000) == 0)
        A_X_SIGN = 0; //Проверяем направление ускорения

    else {
        A_X_SIGN = 1;
        A_X_RESULT &= 0x7FFF;
        A_X_RESULT = 0x7FFF - A_X_RESULT;
    }

    if (A_X_SIGN == 0) {
        A_X_AXIS_POSITION = A_X_RESULT * ACCEL_RESOLUTION_SCALE_2_DPS; //Записываем значение ускорения в переменную   A_X_AXIS_POSITION
    }

    else {
        A_X_AXIS_POSITION = ((A_X_RESULT * ACCEL_RESOLUTION_SCALE_2_DPS) * (-1)); //Записываем значение ускорения в переменную A_X_AXIS_POSITION
    }

    /*Поправка значения проекции вектора G на ось X для использования при расчете asin(x) для вычисления угла наклона по акселерометру*/

    if (A_X_AXIS_POSITION > 1) {
        float xx;
        xx = A_X_AXIS_POSITION - 1;
        A_X_AXIS_POSITION = A_X_AXIS_POSITION - xx;
    }

    if (A_X_AXIS_POSITION < (-1)) {
        float xy;
        xy = (A_X_AXIS_POSITION + 1) * (-1);
        A_X_AXIS_POSITION = A_X_AXIS_POSITION + xy;
    }

    /*Реализация комплементарного фильтра для оси X*/
    /*Рассчитываем угол наклона по оси X с помощью данных от акселерометра функция asin(A_X_AXIS_POSITION) возвращает значение в радианах, для перевода в градусы используем умножение на (180/pi)*/
    ANGLE_FROM_AXEL_AXIS_X = asin(A_X_AXIS_POSITION) * (180 / pi);

    /*Фильтруем значение угла по оси X с использованием значений улов от гироскопа и акселерометра*/
    G_X_AXIS_POSITION = ((1 - FACTOR_FILTER) * G_X_AXIS_POSITION + FACTOR_FILTER * ANGLE_FROM_AXEL_AXIS_X);

    /*Присваиваем отфильтрованные данные переменной в которой хранится значение отфильтрованных данных угол X*/
    ANGLE_AXIS_X_AFTER_FILTER = G_X_AXIS_POSITION;

    if (G_X_AXIS_POSITION > 10) {
        GPIO_SetBits(GPIOC, GPIO_Pin_6); //Зажигаем индикационный светодиод если повернули плату на угол больше 10 градусов
    }

    if (G_X_AXIS_POSITION < 10) {
        GPIO_ResetBits(GPIOC, GPIO_Pin_6); //Гасим индикационный светодиод если угол поворота меньше 10 градусов
    }
}

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

     ВНИМАНИЕ: Приведенный листинг кода не является единственной и оптимальной реализацией поставленной задачи. Пример опубликован с демонстрационной целью. Также автор во избежание переписывания чужих статей, пропускает теоретические основы необходимые для понимания примера, связывая это с тем что всю необходимую информацию можно найти в интернете.

Комментариев (0)

Написать комментарий

Имя *
E-mail
Введите комментарий *
Капча
43 + ? = 44