Учебное пособие по акселерометру и гироскопу MPU6050. Подключение к Ардуино |

Введение:

1. Чтобы узнать принцип работы MPU6050, вы должны сначала понять следующие два датчика:

①Гироскопический датчик:

Принцип гироскопа заключается в том, что вращающийся объектОсь вращенияУказанное направление не изменится, если на него не действуют внешние силы. Люди используют его, чтобы поддерживать направление, основанное на этом принципе.

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

Современные гироскопы позволяют точно определять положение движущихся объектов. Они используются в современной авиации, навигации, авиакосмической и космической промышленности.Оборонная промышленностьШироко используемыйИнерциальная навигацияинструмент.

Основная часть традиционного инерциального гироскопа – это механический гироскоп, а механический гироскоп предъявляет высокие требования к структуре процесса. СовременныйВолоконно-оптический гироскопОсновное предположение 1980-х годов,Волоконно-оптический гироскопПолучил очень быстрое развитие, лазеррезонансГироскоп также получил большое развитие.

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

②Акселерометр:

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

На самом деле все проще, используем mpu6050Гироскопический датчик для измерения угла, используйтеДатчик ускорения для измерения ускорения

MPU-60X0 :

MPU-60X0 – первый в мире 9-осевой датчик обработки движения.Он объединяет 3-осевой гироскоп MEMS, 3-осевой акселерометр MEMS и расширяемый цифровой процессор движения DMP (Digital Motion Processor)., Интерфейс I2C можно использовать для подключения сторонних данныхДатчики слов, например магнитометры.

После расширения 9-осевой сигнал может выводиться через его интерфейс I2C или SPI (интерфейс SPI доступен только в MPU-6000). MPU-60X0 также может быть подключен к неинерциальным цифровым датчикам, таким как датчики давления, через интерфейс I2C. ЧтобыMPU-60X0 использует три 16-битных АЦП для гироскопа и акселерометра, соответственно, для преобразования измеренной аналоговой величины в цифровую величину, которая может быть выведена.

Чтобы точно отслеживать быстрое и медленное движение, диапазон измерения датчика регулируется пользователем. Гироскоп может измерять диапазон ± 250, ± 500, ± 1000, ± 2000 ° / сек (dps), а акселерометр может измерять диапазон Это ± 2, ± 4, ± 8, ± 16 г.

1024-байтовый FIFO на кристалле помогает снизить энергопотребление системы. Для связи со всеми регистрами устройства используется интерфейс I2C с частотой 400 кГц или интерфейс SPI с частотой 1 МГц (SPI доступен только для MPU-6000). Для приложений, которым требуется высокоскоростная передача, можно использовать SPI 20 МГц для чтения и прерывания регистров.

Кроме того, в микросхему встроены датчик температуры и генератор с отклонением ± 1% рабочей среды. Размер микросхемы составляет 4 × 4 × 0,9 мм, используется корпус QFN (квадратный безвыводный корпус), он выдерживает максимальное ударное воздействие 10000 г и имеет программируемый фильтр нижних частот.

Что касается источника питания, MPU-60X0 может поддерживать диапазон VDD 2,5 В ± 5%, 3,0 В ± 5% или 3,3 В ± 5%. Кроме того, MPU-6050 также имеет вывод VLOGIC, который используется для обеспечения логического уровня для выхода I2C. Напряжение VLOGIC может составлять 1,8 ± 5% или VDD.

Цифровой процессор движения (DMP):

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

Подключение оборудования

В эксперименте используется шестиосевой сенсорный модуль AN1507 ATK-MPU6050 от Punctual Atoms.

MPU6050STM32VCC         <-GND         <-SDA         <-SCL         <-INT         <-AD0         <-

1 Регистр управления питанием 1

Бит DEVICE_RESET используется для управления сбросом. Установите его в 1, чтобы сбросить MPU6050. После сброса MPUОборудование автоматически очищает этот бит

Бит SLEEEP используется для управления рабочим режимом MPU6050. После сброса бит равен 1, то есть.В спящий режим (низкое энергопотребление), поэтому мы должны сбросить этот бит, чтобы войти в нормальный рабочий режим

TEMP_DIS используется, чтобы установить, включать ли датчик температуры, установите его на 0, чтобы включить

CLKSEL [2: 0] используется для выбора источника системных часов, соотношение выбора показано в таблице.

** По умолчанию используется внутренний кварцевый генератор 8M RC, точность невысока, поэтому обычно мы выбираем гироскоп оси X / Y / Z в качестве эталона.ФАПЧ в качестве источника тактовой частоты, обычно устанавливают CLKSEL = 001. **

11 Регистр разрешения прерывания

 
OT_EN Этот бит установлен в 1, что позволяет детектору движения (Motiondetection) генерировать прерывания.

FIFO_OFLOW_EN Этот бит установлен в 1, что разрешает переполнение буфера FIFO для генерации прерываний.

I2C_MST_INT_EN установлен в 1, что позволяет всем источникам прерываний ведущего устройства I2C генерировать прерывания.

DATA_RDY_EN Этот бит установлен в 1, этот бит включает прерывание готовности данных, которое будет сгенерировано, когда все операции записи в регистр датчика будут завершены.

Отключите все прерывания и присвойте этому регистру 0X00

2 Регистр конфигурации гироскопа

 
FS_SEL [1: 0] Эти два бита используются для установки полного диапазона шкалы гироскопа: 0, ± 250 °
/ S; 1, ± 500 ° / S; 2, ± 1000 ° / S; 3, ± 2000 ° / S; обычно мы устанавливаем его на 3, то есть ± 2000 ° / S, потому что
АЦП для гироскопа имеет разрешение 16 бит, поэтому чувствительность составляет: 65536/4000 = 16,4 LSB / (° / S).

3 Регистр конфигурации датчика ускорения

 
AFS_SEL [1: 0] Эти два бита используются для установки полного диапазона датчика ускорения: 0,
± 2g; 1, ± 4g; 2, ± 8g; 3, ± 16g; обычно мы устанавливаем его на 0, то есть ± 2g, потому что датчик ускорения
АЦП также 16-битный, поэтому чувствительность составляет: 65536/4 = 16384LSB / g.

4 Регистр включения FIFO

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

Все 3 оси датчика ускорения контролируются 1Управление одним битом (ACCEL_FIFO_EN), пока этот бит равен 1, все три канала акселерометра будут открывать FIFO

5 Регистр делителя частоты дискретизации гироскопа

 
Этот регистр используется для установки частоты дискретизации гироскопа MPU6050. Формула расчета:

Частота дискретизации = выходная частота гироскопа / (1 SMPLRT_DIV)

Выходная частота гироскопа здесь составляет 1 кГц или 8 кГц, что связано с настройкой цифрового фильтра нижних частот (DLPF).Когда DLPF_CFG = 0/7, частота составляет 8 кГц, в противном случае – 1 кГц. И частота фильтра DLPF обычно устанавливаетсяЭто половина частоты дискретизации. Частота дискретизации, мы предполагаем, что она установлена ​​на 50 Гц, тогда SMPLRT_DIV = 1000 / 50-1 = 19

6 Регистр конфигурации

 
Бит установки цифрового фильтра нижних частот (DLPF), а именно: DLPF_CFG [2: 0], ускорение
И измеритель, и гироскоп фильтруются в соответствии с конфигурацией этих трех битов. DLPF_CFG Условия фильтрации, соответствующие различным конфигурациям
Ситуация следующая:
 
Выходная скорость (Fs) датчика ускорения здесь фиксирована на 1 кГц, а выходная скорость (Fs) датчика угловой скорости,
Он отличается в зависимости от конфигурации DLPF_CFG. Обычно мы устанавливаем полосу пропускания датчика угловой скорости равной половине его частоты дискретизации.
Как упоминалось ранее, если частота дискретизации установлена ​​на 50 Гц, тогда ширина полосы пропускания должна быть установлена ​​на 25 Гц, что составляет примерно 20 Гц.
Следует установить DLPF_CFG = 100

7 Регистр управления питанием 2

 
LP_WAKE_CTRL используется для управления частотой пробуждения при низком энергопотреблении

Остальные 6 битов соответственно управляют ускорением и тем, входит ли ось x / y / z гироскопа в режим ожидания.Здесь мы не входим в режим ожидания, поэтому установите их все в 0.

1 Чтение и запись регистров MPU6050 через IIC

u8 IIC_Write_Byte(u8 reg,u8 data)
{
    IIC_Start();
    IIC_Send_Byte((MPU_ADDR<<1)|0);if(IIC_Wait_Ack())  
    {
        IIC_Stop(); 
        return1;       
    }
    IIC_Send_Byte(reg); 
    IIC_Wait_Ack();     
        IIC_Send_Byte(data);if(IIC_Wait_Ack())  
    {
        IIC_Stop();  
        return1;        
    }        
    IIC_Stop();  
    return0;
}

u8 IIC_Read_Byte(u8 reg)
{
    u8 res;
    IIC_Start();
    IIC_Send_Byte((MPU_ADDR<<1)|0);
    IIC_Wait_Ack();
    IIC_Send_Byte(reg);
    IIC_Wait_Ack();
    IIC_Start();
    IIC_Send_Byte((MPU_ADDR<<1)|1);
    IIC_Wait_Ack();
    res=IIC_Read_Byte(0);
    IIC_Stop();return res;
}

u8 IIC_Write_Len(u8 addr,u8 reg,u8 len,u8 *buf)
{
    u8 i;
    IIC_Start();
    IIC_Send_Byte((addr<<1)|0);if(IIC_Wait_Ack())
    {
        IIC_Stop();
        return1;
    }
    IIC_Send_Byte(reg);
    IIC_Wait_Ack();for(i=0;i<len;i  )
    {
        IIC_Send_Byte(buf[i]);if(IIC_Wait_Ack())
        {
            IIC_Stop();
            return1;
        }
    }
    IIC_Stop();
    return0;
}
u8 IIC_Read_Len(u8 addr,u8 reg,u8 len,u8 *buf)
{
    IIC_Start();
    IIC_Send_Byte((addr<<1)|0);if(IIC_Wait_Ack())
    {
        IIC_Stop();
        return1;
    }
    IIC_Send_Byte(reg);
    IIC_Wait_Ack();
    IIC_Start();
    IIC_Send_Byte((addr<<1)|1);
    IIC_Wait_Ack();while(len)
    {
        if(len==1) *buf=IIC_Read_Byte(0);else *buf=IIC_Read_Byte(1);
        len--;
        buf  ;
    }
    IIC_Stop();return0;
}

3 Считайте измеренные необработанные данные, относящиеся к MPU6050

short MPU_Get_Temperature(void)
{
    u8 buf[2]; 
    short raw;
        float temp;
        IIC_Read_Len(MPU_ADDR,MPU_TEMP_OUTH_REG,2,buf); 
    raw=((u16)buf[0]<<8)|buf[1];  
    temp=36.53 ((double)raw)/340;  
    return temp*100;;
}
u8 MPU_Get_Gyroscope(short *gx,short *gy,short *gz)
{
    u8 buf[6],res;
    res=IIC_Read_Len(MPU_ADDR,MPU_GYRO_XOUTH_REG,6,buf);
    if(res==0)
    {
        *gx=((u16)buf[0]<<8)|buf[1];  
        *gy=((u16)buf[2]<<8)|buf[3];  
        *gz=((u16)buf[4]<<8)|buf[5];
    }   
    return res;
}

u8 MPU_Get_Accelerometer(short *ax,short *ay,short *az)
{
    u8 buf[6],res;  
    res=IIC_Read_Len(MPU_ADDR,MPU_ACCEL_XOUTH_REG,6,buf);
    if(res==0)
    {
        *ax=((u16)buf[0]<<8)|buf[1];  
        *ay=((u16)buf[2]<<8)|buf[3];  
        *az=((u16)buf[4]<<8)|buf[5];
    }   
    return res;;
}

Mpu6050 — как это работает

MPU6050 имеет 3-осевой акселерометр и 3-осевой гироскоп, интегрированные в один чип. Гироскоп измеряет скорость вращения или скорость изменения углового положения во времени по осям X, Y и Z. Для измерения используется технология MEMS и эффект Кориолиса.

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

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

MPU6050 также называют устройством слежения за движением по шести осям или устройством с 6 степенями свободы (шесть степеней свободы) из-за его 6 выходов, или 3 выхода акселерометра и 3 выхода гироскопа.

Mpu6500 привод отладки отладки (stm32f407 spi) – русские блоги

/*
******************************************************************
**  Filename :  mpu6500.C
** Аннотация: драйвер SPI для MPU6500
**  Device   :  stm32f4xx
**  Compiler :  keil 5
**  By       :  yulong <[email protected]>
**  Date     :  2023-09-21 17:25:39
 ** Changelog: 1. Созданный в первый раз
*******************************************************************
*/
#include "mpu6500.h"
#include "stm32f4xx_exti.h"
#include "stdio.h"
#include "exti.h"
#include "Show_Scope.h"
#include "sys.h"
#include "delay.h"
#include "usart.h"   
#include "led.h"
 


 / ** Инициализация MPU6500 порт ** /
void mpu6500_Init(void) 
{
    GPIO_InitTypeDef    GPIO_InitStructure;
	NVIC_InitTypeDef	NVIC_InitStructure;
	EXTI_InitTypeDef 	EXTI_InitStructure;
    
         // mpu6500 CS лапка G11
    GPIO_InitStructure.GPIO_Pin = mpu6500_CS;
 	  GPIO_INITSTRUCTURE.GPIO_MODE = GPIO_MODE_OUT; // Режим вывода Обычный
         GPIO_INITSTRUCTURE.GPIO_OTYPE = GPIO_OTYPE_PP; // Двухтактный выходной
    GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;//100MHz
         GPIO_INITSTRUCTURE.GPIO_PUPD = GPIO_PUPD_UP; / / тяга
 	GPIO_Init(mpu6500_CS_G, &GPIO_InitStructure);
         не GPIO_SETBITS (MPU6500_CS_G, MPU6500_CS); // высокий CS, ни
         / / Другие устройства общественного SPI, тянуть ломтик
    GPIO_InitStructure.GPIO_Pin = GPIO_Pin_15;//PG15 //flash_cs
         GPIO_INIT (GPIOG, & GPIO_INITSTRUCTURE); // инициализация
         GPIO_SETBITS (GPIOG, GPIO_PIN_15; // PG15 выход 1, чтобы предотвратить СИФ от вмешательства SPI флэш-связи 
    
         // MPU6500 DRDY ноги
    GPIO_InitStructure.GPIO_Pin = mpu6500_DRDY;
         GPIO_INITSTRUCTURE.GPIO_MODE = GPIO_MODE_IN; // Нормальный режим ввода
    GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;//100M
         GPIO_INITSTRUCTURE.GPIO_PUPD = GPIO_PUPD_UP; / / тяга
 	GPIO_Init(mpu6500_DRDY_G, &GPIO_InitStructure);   
    
         SPI3_init (); // Инициализация режима SPI-3
         SPI3_SETSPEED (SPI_BAUDRATEPRESCAL_256); // Набор для 42Х часов, режим высокой скорости 

         // DRDY прерывания инициализации приема
    //EXTIX_Init();
}


 // Инициализация MPU6500
 // Возвращаемое значение: 0, успех
 / / Другое, код ошибки
u8 MPU6500_Init(void)
{ 
	u8 res, t=5;
	
	 MPU6500_init (); // Инициализация SPI шины .Exit внешнее прерывание
	
	 MPU6500_WRITE_BYTE (MPU_PWR_MGMT1_REG, 0x80); // Сброс MPU6500
    delay_ms(100); //see 《register map》page 42 - delay 100ms
	mpu6500_Write_Byte(MPU_SIGPATH_RST_REG,0X07);	//reset GYR ACC TEMP
	delay_ms(100); //page 42 - delay 100ms
	mpu6500_Write_Byte(MPU_USER_CTRL_REG, 0x11); //SET spi mode Reset all gyro digital signal path, accel digital signal path, and temp
	delay_ms(1000);
	
	res=mpu6500_Read_Byte(MPU_DEVICE_ID_REG);
	 ЕСЛИ (разреш == 0x70) // ИД устройства правильна
	{
		printf("mpu6500_ADDR INIT OK!n");
		
		 MPU_SET_GYRO_FSR (0); // гироскоп датчики, ± 250DPS
		 Delay_ms (10); // Каждый раз, когда вы пишете внимание регистр получки задержки! В противном случае я не буду думать о неправильном лол --yulong
		 MPU_SET_ACCEL_FSR (0); // Датчик ускорения, ± 2g
		delay_ms(10);
		mpu6500_Write_Byte(MPU_CFG_REG,0X03);	//gyr Fs=1khz,bandwidth=41hz
		delay_ms(10); 
		mpu6500_Write_Byte(MPU_ACCEL_CFG2_REG,0X03);	//Acc Fs=1khz, bandtidth=41hz
		delay_ms(10);
		 // mpu6500_write_byte (MPU_INTBP_CFG_REG, 0xa0); // INT контактный эффективен, после выхода
		delay_ms(10);
		//mpu6500_Write_Byte(MPU_INT_EN_REG,0X01);	//raw data inter open
		delay_ms(10);
		 // MPU6500_WRITE_BYTE (MPU_PWR_MGMT1_REG, 0x01); // Задает CLKSEL, ФАПЧ оси Х ссылки
		delay_ms(10);
		 MPU6500_WRITE_BYTE (MPU_PWR_MGMT2_REG, 0x00); // Ускорение и гироскоп работы
		delay_ms(10);
		 MPU_set_rate (200); // Установка частоты дискретизации составляет 200 Гц
		delay_ms(10);
 	}
	else 
	{
		printf("ERROR! mpu6500_ADDR IS %xn", mpu6500_Read_Byte(MPU_DEVICE_ID_REG));
		 Возврат 1; // Выход ошибки
	}

	//just for test --yulong 2023/9/20
	//loop all the time(send data to com)
	while(1)
	{
		short accx_original=0, accy_original=0, accz_original=0;
		u16 ACC_DATA[7];
		u8 raw_datas[14]={0}; //acc*6 temp*2 gyr*6
		u8 res;
		
		 Res = MPU6500_read_byte (MPU_INT_STA_REG);. / / По умолчанию Чтение этот регистр может очистить этот флаг Поскольку запрос петли этого регистра
		//printf("int status:%xn", res);
		delay_us(10);
		 IF (Рез == 0x01) // данные готовы
		{
			mpu6500_Read_Len(MPU_ACCEL_XOUTH_REG, 8, &raw_datas[0]);
			delay_us(10);
			mpu6500_Read_Len(MPU_GYRO_XOUTH_REG, 6, &raw_datas[8]);
			delay_us(10);

			 ACC_DATA [0] = ((U16) Raw_Data [0] << 8) | Raw_Data [1]; // трехосного ускорение
			ACC_DATA[1]=((u16)raw_datas[2]<<8)|raw_datas[3];
			ACC_DATA[2]=((u16)raw_datas[4]<<8)|raw_datas[5];
			 ACC_DATA [3] = ((U16) Raw_DataS [8] << 8) | Raw_Data [9]; // трехосевой угловая скорость
			ACC_DATA[4]=((u16)raw_datas[10]<<8)|raw_datas[11];
			ACC_DATA[5]=((u16)raw_datas[12]<<8)|raw_datas[13];
			 ACC_DATA [6] = ((U16) Raw_DataS [6] << 8) | Raw_Data [7]; // Температура данных
			 Data_send_status (ACC_DATA, 0xF1, 7); // 7 данные канала, отправленные на анонимный хост-машина дисплея формы волны
			
			LED3_ON(); //open red led
		}
	}
	return 0;
}
 / / Установить гироскоп датчик MPU6050 полный диапазон Диапазон
//fsr:0,±250dps;1,±500dps;2,±1000dps;3,±2000dps
 // Возвращаемое значение: 0, набор успеха
 / / Другое, набор не удалось 
u8 MPU_Set_Gyro_Fsr(u8 fsr)
{
	 ВОЗВРАТ MPU6500_WRITE_BYTE (MPU_GYRO_CFG_REG, FSR << 3); // Установить гироскоп полный диапазон  
}
 / / Установить MPU6050 датчик ускорения полный диапазон Диапазон
//fsr:0,±2g;1,±4g;2,±8g;3,±16g
 // Возвращаемое значение: 0, набор успеха
 / / Другое, набор не удалось 
u8 MPU_Set_Accel_Fsr(u8 fsr)
{
	 ВОЗВРАТ MPU6500_WRITE_BYTE (MPU_Accel_cfg_reg, FSR << 3); // Установить датчик ускорения полномасштабной диапазон  
}
 / / Установить цифровой фильтр нижних частот из MPU 6050
 // ФНЧ: Цифровой низкочастотный фильтр (Гц)
 // Возвращаемое значение: 0, набор успеха
 / / Другое, набор не удалось 
u8 MPU_Set_LPF(u16 lpf)
{
	u8 data=0;
	if(lpf>=188)data=1;
	else if(lpf>=98)data=2;
	else if(lpf>=42)data=3;
	else if(lpf>=20)data=4;
	else if(lpf>=10)data=5;
	else data=6; 
	Возврат mpu6500_write_byte (mpu_cfg_reg, data); // установить цифровой фильтр низкого прохода  
}
 // Установите скорость дискретизации MPU 6050 (при условии fs = 1 кГц)
//rate:4~1000(Hz)
 // возвращаемое значение: 0, установить успех
 / / Другое, установить не удалось 
u8 MPU_Set_Rate(u16 rate)
{
	u8 data;
	 Если (скорость> 1000) скорость = 1000; // максимальная скорость образца 1 кГц
	if(rate<4)rate=4;
	 Data = 1000 / rate-1; // рассчитаны в соответствии с формулами
	 Data = mpu6500_write_byte (mpu_sample_rate_reg, data); // Настройки частоты дискретизации
 	  // return mpu_set_lpf (скорость / 2); // автоматически настроить LPF для половины частоты дискретизации
}


 / / Читать несколько регистров одновременно
 // Рег: начать регистрационный адрес
 // Лен: Общее количество реестров чтения
 // * Buf: хранение задний указатель
u8 mpu6500_Read_Len(u8 reg, u8 len,u8 *buf)
{ 
 	u8 tmp=0;
	
	while(len)
	{
		mpu6500_CS_L;
		 SPI3_READWRITEBYTE (REG | 0x80); // R высший бит 1
		*buf=SPI3_ReadWriteByte(0x00);
		len--;
		buf  ;
		reg  ;
		mpu6500_CS_H;
		 Delay_us (5); // Каждый раз, когда вы читаете реестр, должен быть задержан на некоторое время. Не могу прочитать следующий реестр немедленно - yulong
	}	
	return tmp;
}


 
//spi write a byte
u8 mpu6500_Write_Byte(u8 reg,u8 data) 				 
{ 
	mpu6500_CS_L;
	 SPI3_READWRITEBYTE (REG); // W самый высокий бит 0
	SPI3_ReadWriteByte(data);
	mpu6500_CS_H;
}

//spi read a byte
u8 mpu6500_Read_Byte(u8 reg)
{
	u8 tmp=0;
	mpu6500_CS_L;
	 SPI3_READWRITEBYTE (REG | 0x80); // R высший бит 1
	tmp=SPI3_ReadWriteByte(0xff);
	mpu6500_CS_H;
	return tmp;
}

Ардуино и mpu6050

Давайте посмотрим, как мы можем подключить и прочитать данные с датчика MPU6050 с помощью Ардуино. Для связи с Ардуино мы используем протокол I2C, поэтому нам нужны всего два провода для подключения по линии данных и два провода для питания.

Два·передача данных:

1. Принцип I2C подробно объяснялся в предыдущем блоге, поэтому я не буду повторять его здесь.

Если вы хотите записать в регистр MPU-60X0, помимо отправки флага запуска (S) и бита адреса, ведущее устройство также должно добавить бит чтения / записи, 0 для записи и 1 для чтения. В 9-м тактовом цикле (высокий уровень) MPU-60X0 генерирует ответный сигнал.

Последовательность записи одного байта:

Последовательность многобайтовой записи:

Если вы хотите прочитать значение регистра MPU-60X0, сначала ведущее устройство генерирует стартовый сигнал (S), затем отправляет бит адреса ведомого устройства и бит данных записи, а затем отправляет адрес регистра для начала чтения регистра. Сразу после получения ответного сигнала ведущее устройство отправляет стартовый сигнал, а затем отправляет бит адреса ведомого устройства и бит считанных данных.

Затем MPU-60X0 как ведомое устройство генерирует ответный сигнал и начинает отправлять данные регистра. Связь заканчивается сигналом отказа (NACK) и флагом окончания (P), генерируемым ведущим устройством. NACK определяется как данные SDA всегда на высоком уровне в 9-м тактовом цикле. Чтобы

Проект под кодовым названием: «бульболёт». часть 1. погружение в mpu6050 (или нет)

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

С начала я изучил основы, а именно – посмотрел что там придумали китайцы, а придумали они “полётные контроллеры”, в основу которых входит микроконтроллер (в основном STM32), гироскоп, барометр и т.д. В принципе, подумал я, всё выглядит довольно просто, значит, можно повторить.

Итак, мой путь начался с выбора начинки нашего “полётника”. Я взял за основу микроконтроллер STM32F103C8T6, расположенный на распаянной плате (blue pil). В периферию: микросхему MPU6050 (3 осевой гироскоп и акселерометр) разведенную на плате под кодовым названием (GY-521), BMP280 (датчик давления), HMC5883L (3-осевой цифровой компас) распаянный на плате (модуль GY-273). Для передачи и приёма я использую MRF49XA (трансивер). В последствии всё будет выпаяно и припаяно по месту назначению, а пока ограничимся макетной платой.

И так начнём, для работы с камнем я буду использовать STM32CubeMX (библиотека HAL), а для редактирования прослойки будем юзать STM32CubeIDE. Почему именно эти проги, во-первых, они официальные с поддержкой STM, во-вторых, имеют привлекательный и понятный интерфейс, а как же большое обилие примеров для изучения. Для дебагинга я использую USART, но в иделае надо бы юзать ST LINK (поэтому не экономим и берём вместе с blue pil-ом).

Смотрите про коптеры:  Как летать не управляя или RX3S OrangeRX V2

Приступим-с! Открываем STM32CubeMX и выбираем наш МК. Начинаем настраивать его, а именно, для начала включим внешнее тактирование (ведь чем больше частота – тем быстрее работает МК).

Переходи во вкладку Clock Configuration, вписываем значение нашего кварца, в моём случае это 8. После ищем клеточку с надписью HCLK и вписываем 72 (72 MHz – максимальная частота при кварце на 8 MHz). Программа сама подстроит оставшиеся настройки.

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

Идём дальше и забегая чуть – чуть вперёд сразу скажу, что стандартное общение по I2C довольно сильно замедляет основную программу. Поэтому для решения этой проблемы у нас может быть два пути решения либо исправлять это с помощью прерываний:

Либо с помощью DMA(Direct Memory Access) – прямой доступ к памяти. Прикол этой штуки в том, чтобы распараллелить программу и облегчить жизнь CPU, то есть DMA общается по I2C, а основной процесс обрабатывает полученные данные.

И как вы поняли, я выбрал 2 способ. И так приступим к настройке. Так как я буду только считывать данные, то будем юзать RX. Все настройке на картинке.

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

Включаю USART для дебага.

И так пока остановимся, ведь мне надо пояснить некоторые детали, о которых я должен был ещё сказать в начале статьи, а именно то, что сейчас мы подготавливаем наш МК для работы с MPU6050. Поэтому мы и используем I2С, однако это не все плюшки микросхемы, прошу вас обратить внимание на вывод INT. Он нужен для того, чтобы подавать сигналы мастеру, к примеру, о готовности данных. Удобно! Я тоже так думаю, поэтому нам надо настроить наш МК, чтобы он захватывал эти сигналы, для этого настроим таймер.

Активируем TIM1, ибо он самый мясистый.

Теперь пробежимся по пунктам, и первым идёт режим управления таймером Master/Slave.

Суть заключается в том, чтобы при возникновении тех или иных событий таймер мог посылать различные триггеры (сигналы) другим таймерам — режим Master.

А таймер, получающий сигнал от другого таймера, является подчинённым — режим Slave.

Следовательно, Slave Mode – этот пункт, указывающий, что должен делать таймер, находясь в подчинённом режиме, в нашем случае, таймер будет подчинён сам себе, то есть, совершая захват сигнала, он будет генерировать тригер, и на основании этого сигнала, обнуляет счётчик (Reset Mode — означает, что при поступлении сигнала таймер должен обнулить счётчик) – это нам и нужно. Есть также и другие функции, если кому нужно:

Trigger Source — а этот пункт указывает что будет служить триггерным сигналом для таймера в моём случае TI1FP1, а так смотрим на картинку.

Идём дальше, и тут стоит заметить, что у каждого таймера есть четыре независимых канала, которые могут подключаться к физическим пинам микроконтроллера, а могут и не подключаться, работая как внутренние входы/выходы. Поэтому при настройке двух каналов (direct и indirect) активировался только один вход (РА8). Зачем мы это сделали? Чтобы первый канал ловил передний фронт, а второй — задний, тем самым мы и измерим длину импульса.

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

Осталось дело за малым, смотрим на картинку. Тут особо менять ничего не надо, но вы проверьте чтобы всё сходилось, а я лишь остановлюсь на одном пункте: Prescaler – предделитель частоты таймера (частоты поступающей с шины APB2). И тут важно отметить! Поскольку счётчик начинает отсчёт с нуля, то предделитель должен быть на 1 меньше. В нашем случае 72мГц / 71 = 1000000 тиков в секунду.

И так с настройкой камня покончено, перейдем к самой микросхеме (MPU6050). И первым делом почитаем даташит и карту регистров. Пойдём по порядку, сначала у нас идёт SELF_TEST, мы его оперативно скипаем ибо мы и сами сможем вычислить среднее значение погрешности. Далее у нас идёт несколько пунктов (SMPLRT_DIV, CONFIG, GYRO_CONFIG, ACCEL_CONFIG), которые нам понадобится для правильной работы датчика. Реализуем это в программе для этого создадим функцию инициализации. В ней мы с помощью стандартной функции общения по I2C (HAL_I2C_Mem_Write()) будем устанавливать начальные параметры работы.

void InitMPU6050(void) {     

        uint8_t data;
        data = 0;
        HAL_I2C_Mem_Write(&hi2c1, MPU6050_Address, PWR_MGMT_1_REG, 1, &data, 1, time);

        data = 0x07;
        HAL_I2C_Mem_Write(&hi2c1, MPU6050_Address, SMPLRT_DIV_REG, 1, &data, 1, time);
 
        data = 0x18;
        HAL_I2C_Mem_Write(&hi2c1, MPU6050_Address, ACCEL_CONFIG_REG, 1, &data, 1, time);

        data = 0x18;
        HAL_I2C_Mem_Write(&hi2c1, MPU6050_Address, GYRO_CONFIG_REG, 1, &data, 1, time);

        data = 0x1;
        HAL_I2C_Mem_Write(&hi2c1, MPU6050_Address, INT_ENABLE_REG, 1, &data, 1, time);

}

Теперь научимся захватывать наши импульсы, с помощью колбека (он срабатывает при прерывании от DMA) HAL_TIM_IC_CaptureCallback(TIM_HandleTypeDef *htim){} после мы проверяем тот ли таймер захватил сигнал, если тот то действуем.

uint32_t zntime;

/* USER CODE BEGIN 2 */
  HAL_TIM_IC_Start_DMA(&htim1, TIM_CHANNEL_2, (uint32_t*)&zntime, 1);//включаем таймер     
/* USER CODE END 2 */

/* USER CODE BEGIN 4 */
void HAL_TIM_IC_CaptureCallback(TIM_HandleTypeDef *htim) 
{
        if(htim->Instance == TIM1)
        {
           HAL_I2C_Mem_Read_DMA(&hi2c1, MPU6050_Address, ACCEL_XOUT_H_REG, 1, data, 14);
        }
}
/* USER CODE END 4 */

Далее будем считывать готовые данные с помощью DMA,для этого нам понадобятся регистры с 3B (ACCEL_XOUT_H_REG) по 48, то есть за раз нам надо считать 14 регистров. Чтение будет выполняться с помощью функцииHAL_I2C_Mem_Read_DMA()и с помощью прерывания по окончанию передачи по I2C void HAL_I2C_MemRxCpltCallback(I2C_HandleTypeDef *hi2c)

/* USER CODE BEGIN 4 */
void HAL_I2C_MemRxCpltCallback(I2C_HandleTypeDef *hi2c)
{
	  MPU6050Read();
}
/* USER CODE END 4 */

Также стоит отметить, что данные будут состоять из двух 8 битных частей, которые надо будет склеить в 16 битное число, а так же для наглядности высчитаем pitch.

typedef struct
{

    int16_t AccelX;
    int16_t AccelY;
    int16_t AccelZ;
    double aX;
    double aY;
    double aZ;

    int16_t GyroX;
    int16_t GyroY;
    int16_t GyroZ;
    double gX;
    double gY;
    double gZ;

    int16_t temp;
    int Temperature;

} MPU6050znach;// можно с помощью переменных или как тут


MPU6050znach z;


void MPU6050Read(void)
{
  ///////////////////////////склейка/////////////////
        z.AccelX = (int16_t)(data[0] << 8 | data[1]);
        z.AccelY = (int16_t)(data[2] << 8 | data[3]);
        z.AccelZ = (int16_t)(data[4] << 8 | data[5]);
        z.temp = (int16_t)(data[6] << 8 | data[7]);
        z.GyroX = (int16_t)(data[8] << 8 | data[9]);
        z.GyroY = (int16_t)(data[10] << 8 | data[11]);
        z.GyroZ = (int16_t)(data[12] << 8 | data[13]);

/////////////////////////////обработка////////////////////
        z.aX = z.AccelX / 2048.0;
        z.aY = z.AccelY / 2048.0;
        z.aZ = z.AccelZ / 2048.0;
        z.Temperature = (int)((int16_t)z.temp / (float)340.0   (float)36.53);
        z.gX = z.GyroX / 131.0;
        z.gY = z.GyroY / 131.0;
        z.gZ = z.GyroZ / 131.0;

 /////////////////////////вычисление////////////////////
        int pitch;
        int pitch_sqrt = sqrt(z.aY * z.aY    z.aZ * z.aZ);
        pitch = atan2(-z.aX, pitch_sqrt) * RAD_TO_DEG;

  /////////////////////////вывод/////////////////
         snprintf(msg, sizeof(msg), "%d", pitch);
           HAL_UART_Transmit(&huart1, (uint8_t*)msg, strlen(msg), HAL_MAX_DELAY);
           HAL_UART_Transmit(&huart1, (uint8_t*)"rn", strlen("rn"), HAL_MAX_DELAY);
           HAL_UART_Transmit(&huart1, (uint8_t*)"rn", strlen("rn"), HAL_MAX_DELAY);
}

И тадам получаем угол отклонения, однако значения достаточно сырые, ибо должно было получится где-то -90 градусов.

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

Для борьбы с этим недугом есть 2 пути решения использовать фильтр (к примеру Кальмана) или DMP (это небольшая програмка вшитая в MPU6050, которая сглаживает значения). И об этом мы поговорим в следующей статье, а так же подключим HMC5883L (3-осевой цифровой компас). На этом всё, до скорого!

Скетч mpu6050 ардуино

Ниже приведем полный код, а после разъясним его поподробнее:

#include <Wire.h>

const int MPU = 0x68; // MPU6050 I2C адрес
float AccX, AccY, AccZ;
float GyroX, GyroY, GyroZ;
float accAngleX, accAngleY, gyroAngleX, gyroAngleY, gyroAngleZ;
float roll, pitch, yaw;
float AccErrorX, AccErrorY, GyroErrorX, GyroErrorY, GyroErrorZ;
float elapsedTime, currentTime, previousTime;
int c = 0;

void setup() {
  Serial.begin(19200);
  Wire.begin();                      // Инициализировать связь
  Wire.beginTransmission(MPU);       // Начать связь с MPU6050 // MPU = 0x68
  Wire.write(0x6B);                  // обращаемся к регистру 6B
  Wire.write(0x00);                  //  сброс - поместите 0 в регистр 6B
  Wire.endTransmission(true);        //закончить передачу
  /*
  // Настройка чувствительности акселерометра - полный диапазон шкалы (по умолчанию  /- 2g)
  Wire.beginTransmission(MPU);
  Wire.write(0x1C);                  //обращаемся к регистру ACCEL_CONFIG (1C hex)
  Wire.write(0x10);                  //Установить биты регистра как 00010000 (диапазон полной шкалы  /- 8g)
  Wire.endTransmission(true);
  // Настроить чувствительность гироскопа - полный диапазон шкалы (по умолчанию  /- 250 град / с)
  Wire.beginTransmission(MPU);
  Wire.write(0x1B);                   // Обращаемся к регистру GYRO_CONFIG (1B hex)
  Wire.write(0x10);                   // Установить биты регистра как 00010000 (полная шкала 1000 град / с)
  Wire.endTransmission(true);
  delay(20);
  */
  // Вызов этой функции, покажет значения ошибок IMU для вашего модуля
  calculate_IMU_error();
  delay(20);

}

void loop() {
 // === Считать данные акселерометра === //
  Wire.beginTransmission(MPU);
  Wire.write(0x3B); // Начинаем с регистра 0x3B (ACCEL_XOUT_H)
  Wire.endTransmission(false);
  Wire.requestFrom(MPU, 6, true); // Чтение всех 6 регистров, значение каждой оси сохраняется в 2 регистрах
// Для диапазона   -2g нам нужно разделить необработанные значения на 16384 в соответствии с datasheet
  AccX = (Wire.read() << 8 | Wire.read()) / 16384.0; // Значение по оси X
  AccY = (Wire.read() << 8 | Wire.read()) / 16384.0; // Значение по оси Y
  AccZ = (Wire.read() << 8 | Wire.read()) / 16384.0; // Значение по оси Z
 // Расчет крена и тангажа по данным акселерометра
  accAngleX = (atan(AccY / sqrt(pow(AccX, 2)   pow(AccZ, 2))) * 180 / PI) - 0.58;
  accAngleY = (atan(-1 * AccX / sqrt(pow(AccY, 2)   pow(AccZ, 2))) * 180 / PI)   1.58;

  // === Считываем данные гироскопа === //
  previousTime = currentTime;        // Предыдущее время сохраняется до фактического чтения
  currentTime = millis();           // Текущее время Фактическое время чтения
  elapsedTime = (currentTime - previousTime) / 1000; // Разделим на 1000, чтобы получить секунды
  Wire.beginTransmission(MPU);
  Wire.write(0x43); // Адрес первого регистра данных гироскопа 0x43
  Wire.endTransmission(false);
  Wire.requestFrom(MPU, 6, true); // Чтение всех 4 регистров, значение каждой оси сохраняется в 2 регистрах
  GyroX = (Wire.read() << 8 | Wire.read()) / 131.0; // Для диапазона 250 градусов / с мы должны сначала разделить необработанное значение на 131.0, согласно datasheet
  GyroY = (Wire.read() << 8 | Wire.read()) / 131.0;
  GyroZ = (Wire.read() << 8 | Wire.read()) / 131.0;
 // Корректируем выходы с вычисленными значениями ошибок
  GyroX = GyroX   0.56; // GyroErrorX ~(-0.56)
  GyroY = GyroY - 2; // GyroErrorY ~(2)
  GyroZ = GyroZ   0.79; // GyroErrorZ ~ (-0.8)

 // В настоящее время необработанные значения выражаются в градусах в секунду, град / с, поэтому нам нужно умножить на sendonds (s), чтобы получить угол в градусах
  gyroAngleX = gyroAngleX   GyroX * elapsedTime; // deg/s * s = deg
  gyroAngleY = gyroAngleY   GyroY * elapsedTime;
  yaw =  yaw   GyroZ * elapsedTime;

// Дополнительный фильтр - объединить значения угла акселерометра и гироскопа
  roll = 0.96 * gyroAngleX   0.04 * accAngleX;
  pitch = 0.96 * gyroAngleY   0.04 * accAngleY;
  
// Выводим значения на серийный монитор
  Serial.print(roll);
  Serial.print("/");
  Serial.print(pitch);
  Serial.print("/");
  Serial.println(yaw);
}


void calculate_IMU_error() {
// Мы можем вызвать эту функцию в разделе setup(), чтобы вычислить ошибку данных акселерометра и гироскопа. Отсюда мы получим значения ошибок, используемые в приведенных выше уравнениях, напечатанные на последовательном мониторе.
   // Обратите внимание, что мы должны разместить датчик плоско, чтобы получить правильные значения, чтобы затем мы могли считать правильные значения
   // Считываем значения акселерометра 200 раз
  while (c < 200) {
    Wire.beginTransmission(MPU);
    Wire.write(0x3B);
    Wire.endTransmission(false);
    Wire.requestFrom(MPU, 6, true);
    AccX = (Wire.read() << 8 | Wire.read()) / 16384.0 ;
    AccY = (Wire.read() << 8 | Wire.read()) / 16384.0 ;
    AccZ = (Wire.read() << 8 | Wire.read()) / 16384.0 ;
    // Sum all readings
    AccErrorX = AccErrorX   ((atan((AccY) / sqrt(pow((AccX), 2)   pow((AccZ), 2))) * 180 / PI));
    AccErrorY = AccErrorY   ((atan(-1 * (AccX) / sqrt(pow((AccY), 2)   pow((AccZ), 2))) * 180 / PI));
    c  ;
  }
  // Разделим сумму на 200, чтобы получить значение ошибки
  AccErrorX = AccErrorX / 200;
  AccErrorY = AccErrorY / 200;
  c = 0;
  // Считываем значения гироскопа 200 раз
  while (c < 200) {
    Wire.beginTransmission(MPU);
    Wire.write(0x43);
    Wire.endTransmission(false);
    Wire.requestFrom(MPU, 6, true);
    GyroX = Wire.read() << 8 | Wire.read();
    GyroY = Wire.read() << 8 | Wire.read();
    GyroZ = Wire.read() << 8 | Wire.read();
    // Sum all readings
    GyroErrorX = GyroErrorX   (GyroX / 131.0);
    GyroErrorY = GyroErrorY   (GyroY / 131.0);
    GyroErrorZ = GyroErrorZ   (GyroZ / 131.0);
    c  ;
  }
// Разделим сумму на 200, чтобы получить значение ошибки
  GyroErrorX = GyroErrorX / 200;
  GyroErrorY = GyroErrorY / 200;
  GyroErrorZ = GyroErrorZ / 200;
 // Выводим значения ошибок на Serial Monitor
  Serial.print("AccErrorX: ");
  Serial.println(AccErrorX);
  Serial.print("AccErrorY: ");
  Serial.println(AccErrorY);
  Serial.print("GyroErrorX: ");
  Serial.println(GyroErrorX);
  Serial.print("GyroErrorY: ");
  Serial.println(GyroErrorY);
  Serial.print("GyroErrorZ: ");
  Serial.println(GyroErrorZ);
}

Описание кода: Итак, сначала нам нужно подключить библиотеку Wire.h, которая используется для I2C связи, и определить некоторые переменные, необходимые для хранения данных.

В разделе setup() нам нужно инициализировать библиотеку Wire.h и сбросить датчик через регистр управления . Для этого нам нужно взглянуть в datasheet, где мы можем увидеть адрес регистра:

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

// Настройка чувствительности акселерометра - полный диапазон шкалы (по умолчанию  /- 2g)
  Wire.beginTransmission(MPU);
  Wire.write(0x1C);                  //обращаемся к регистру ACCEL_CONFIG (1C hex)
  Wire.write(0x10);                  //Установить биты регистра как 00010000 (диапазон полной шкалы  /- 8g)
  Wire.endTransmission(true);
  // Настроить чувствительность гироскопа - полный диапазон шкалы (по умолчанию  /- 250 град / с)
  Wire.beginTransmission(MPU);
  Wire.write(0x1B);                   // Обращаемся к регистру GYRO_CONFIG (1B hex)
  Wire.write(0x10);                   // Установить биты регистра как 00010000 (полная шкала 1000 град / с)
  Wire.endTransmission(true);
  delay(20);

В разделе loop() мы начинаем с чтения данных акселерометра. Данные для каждой оси хранятся в двух байтах или регистрах, и мы можем видеть адреса этих регистров в datasheet датчика:

Чтобы прочитать их все, мы начинаем с первого регистра и с помощью функции RequiestFrom() запрашиваем чтение всех 6 регистров для осей X, Y и Z. Затем мы читаем данные из каждого регистра, и, поскольку выходные данные состроят из старшего и младшего байта, мы соответствующим образом объединяем их, чтобы получить правильные значения:

 // === Считать данные акселерометра === //
  Wire.beginTransmission(MPU);
  Wire.write(0x3B); // Начинаем с регистра 0x3B (ACCEL_XOUT_H)
  Wire.endTransmission(false);
  Wire.requestFrom(MPU, 6, true); // Чтение всех 6 регистров, значение каждой оси сохраняется в 2 регистрах
// Для диапазона   -2g нам нужно разделить необработанные значения на 16384 в соответствии с datasheet
  AccX = (Wire.read() << 8 | Wire.read()) / 16384.0; // Значение по оси X
  AccY = (Wire.read() << 8 | Wire.read()) / 16384.0; // Значение по оси Y
  AccZ = (Wire.read() << 8 | Wire.read()) / 16384.0; // Значение по оси Z

Чтобы получить выходные значения от -1g до 1g, подходящие для расчета углов, мы делим выходной сигнал с предварительно выбранной чувствительностью.

Наконец, используя две формулы, мы вычисляем углы крена и тангажа на основе данных акселерометра:

// Расчет крена и тангажа по данным акселерометра
 accAngleX = (atan(AccY / sqrt(pow(AccX, 2)   pow(AccZ, 2))) * 180 / PI) - 0.58; 
 accAngleY = (atan(-1 * AccX / sqrt(pow(AccY, 2)   pow(AccZ, 2))) * 180 / PI)   1.58;

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

Мы считываем шесть регистров гироскопа, соответствующим образом объединяем их данные и делим их на предварительно выбранную чувствительность, чтобы получить результат в градусах в секунду:

// === Считываем данные гироскопа === //
previousTime = currentTime;        // Предыдущее время сохраняется до фактического чтения
currentTime = millis();           // Текущее время Фактическое время чтения
elapsedTime = (currentTime - previousTime) / 1000; // Разделим на 1000, чтобы получить секунды
Wire.beginTransmission(MPU);
Wire.write(0x43); // Адрес первого регистра данных гироскопа 0x43
Wire.endTransmission(false);
Wire.requestFrom(MPU, 6, true); // Чтение всех 4 регистров, значение каждой оси сохраняется в 2 регистрах
GyroX = (Wire.read() << 8 | Wire.read()) / 131.0; // Для диапазона 250 градусов / с мы должны сначала разделить необработанное значение на 131.0, согласно datasheet
GyroY = (Wire.read() << 8 | Wire.read()) / 131.0;
GyroZ = (Wire.read() << 8 | Wire.read()) / 131.0;

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

// Корректируем выходы с вычисленными значениями ошибок
  GyroX = GyroX   0.56; // GyroErrorX ~(-0.56)
  GyroY = GyroY - 2; // GyroErrorY ~(2)
  GyroZ = GyroZ   0.79; // GyroErrorZ ~ (-0.8)

 // В настоящее время необработанные значения выражаются в градусах в секунду, град / с, поэтому нам нужно умножить на sendonds (s), чтобы получить угол в градусах
  gyroAngleX = gyroAngleX   GyroX * elapsedTime; // deg/s * s = deg
  gyroAngleY = gyroAngleY   GyroY * elapsedTime;
  yaw =  yaw   GyroZ * elapsedTime;

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

Обратной стороной гироскопа является то, что он дрейфует или вносит ошибку в выходной сигнал с течением времени. Поэтому в долгосрочной перспективе мы используем данные акселерометра, в данном случае 4%, что достаточно для устранения ошибки дрейфа гироскопа.

// Дополнительный фильтр - объединить значения угла акселерометра и гироскопа
  roll = 0.96 * gyroAngleX   0.04 * accAngleX;
  pitch = 0.96 * gyroAngleY   0.04 * accAngleY;

Однако, поскольку мы не можем рассчитать рыскание по данным акселерометра, мы не можем реализовать для него дополнительный фильтр.

Прежде чем мы взглянем на результаты, объясним, как получить значения коррекции ошибок. Для вычисления этих ошибок мы можем вызвать пользовательскую функцию calculate_IMU_error(), когда датчик находится в неподвижном положении.

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

void calculate_IMU_error() {
// Мы можем вызвать эту функцию в разделе setup(), чтобы вычислить ошибку данных акселерометра и гироскопа. Отсюда мы получим значения ошибок, используемые в приведенных выше уравнениях, напечатанные на последовательном мониторе.
   // Обратите внимание, что мы должны разместить датчик плоско, чтобы получить правильные значения, чтобы затем мы могли считать правильные значения
   // Считываем значения акселерометра 200 раз
  while (c < 200) {
    Wire.beginTransmission(MPU);
    Wire.write(0x3B);
    Wire.endTransmission(false);
    Wire.requestFrom(MPU, 6, true);
    AccX = (Wire.read() << 8 | Wire.read()) / 16384.0 ;
    AccY = (Wire.read() << 8 | Wire.read()) / 16384.0 ;
    AccZ = (Wire.read() << 8 | Wire.read()) / 16384.0 ;
    // Sum all readings
    AccErrorX = AccErrorX   ((atan((AccY) / sqrt(pow((AccX), 2)   pow((AccZ), 2))) * 180 / PI));
    AccErrorY = AccErrorY   ((atan(-1 * (AccX) / sqrt(pow((AccY), 2)   pow((AccZ), 2))) * 180 / PI));
    c  ;
  }
  // Разделим сумму на 200, чтобы получить значение ошибки
  AccErrorX = AccErrorX / 200;
  AccErrorY = AccErrorY / 200;
  c = 0;
  // Считываем значения гироскопа 200 раз
  while (c < 200) {
    Wire.beginTransmission(MPU);
    Wire.write(0x43);
    Wire.endTransmission(false);
    Wire.requestFrom(MPU, 6, true);
    GyroX = Wire.read() << 8 | Wire.read();
    GyroY = Wire.read() << 8 | Wire.read();
    GyroZ = Wire.read() << 8 | Wire.read();
    // Sum all readings
    GyroErrorX = GyroErrorX   (GyroX / 131.0);
    GyroErrorY = GyroErrorY   (GyroY / 131.0);
    GyroErrorZ = GyroErrorZ   (GyroZ / 131.0);
    c  ;
  }
// Разделим сумму на 200, чтобы получить значение ошибки
  GyroErrorX = GyroErrorX / 200;
  GyroErrorY = GyroErrorY / 200;
  GyroErrorZ = GyroErrorZ / 200;
 // Выводим значения ошибок на Serial Monitor
  Serial.print("AccErrorX: ");
  Serial.println(AccErrorX);
  Serial.print("AccErrorY: ");
  Serial.println(AccErrorY);
  Serial.print("GyroErrorX: ");
  Serial.println(GyroErrorX);
  Serial.print("GyroErrorY: ");
  Serial.println(GyroErrorY);
  Serial.print("GyroErrorZ: ");
  Serial.println(GyroErrorZ);
}

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

Наконец, с помощью функции Serial.print мы можем распечатать значения Roll, Pitch и Yaw на последовательном мониторе и посмотреть, правильно ли работает датчик.

Источник

8 Регистр вывода данных гироскопа

 
Считывая эти 6 регистров, вы можете прочитать значение оси x / y / z гироскопа, например, данные оси x, которые могут быть прочитаны
Получены регистры 0X43 (старшие 8 бит) и 0X44 (младшие 8 бит), другие оси могут быть выведены по аналогии

10 Регистр вывода данных датчика температуры

Значение датчика температуры может быть получено путем чтения регистров 0X41 (старшие 8 бит) и 0X42 (младшие 8 бит).Формула преобразования температуры:

Temperature = 36.53 regval/340

Среди них температура – это расчетное значение температуры, единица измерения – ℃, а regval – значение, считываемое из 0X41 и 0X42.Значение датчика температуры

9 Регистр вывода данных датчика ускорения

 
Считывая эти 6 регистров, вы можете прочитать значение оси x / y / z датчика ускорения. Например, считывание данных оси x может быть получено путем чтения регистров 0X3B (старшие 8 бит) и 0X3C (младшие 8 бит). Другие топоры и так далее