Друга версія рукавички для визначення положення руки

Минула стаття була невдалою і не змістовною. Спочатку я планував прикріпити плати і код для мікроконтролера, щоб зібрати її міг кожен охочий. Але там було стільки милиць, що стало соромно це прикріплювати. Тепер же я опишу другу рукавичку, яку збирав два тижні тому, і яка містить більш просунуті датчики і видає більш точні дані. Хоч і виглядає куди гірше:




Відмінності від попередньої версії

1)У першу чергу це датчики. Кути нахилу звичайно можна визначити за допомогою акселерометрів, але крім шумів акселерометра нам будуть заважати ще і будь-які рухи людини, т. к. при русі руки на неї діють різні прискорення, а не тільки прискорення вільного падіння Землі. Тоді ми вирішили взяти гіроскоп. Але MEMS гіроскоп — це зовсім не той гіроскоп, який нам показували на уроках фізики. Цей гіроскоп видає швидкість зміни кута. Так що для отримання повного кута відхилення від початкового положення нам потрібно ці свідчення інтегрувати. Так мало того, що сигнал виходить дискретним, так ще є так званий «дрейф нуля». У стані спокою гіроскоп видає не нуль, а дуже маленьке число, відмінне від нуля. Так що при інтегруванні буде накопичуватися помилка. Це називається низькочастотним шумом гіроскопа. Що ж робити? Тут нам на допомогу приходить альфа-бета фільтр. Виглядає він ось як:

image

Картинка нахабно скопійована з статті bibigone. Сама стаття просто шикарна. Всім, хто хоче розібратися з датчиками детальніше раджу прочитати. А тут наведу копію опису фільтра з самої статті:

Є набагато більш простий фільтр, який називається саме «альфа-бета», який полягає в тому, що припустимо, добре, давайте продовжувати інтегрувати, тільки давайте з невеликою поправочкой, ще додавати кут, який отримуємо з акселерометра. Ну дельта тут якесь невелике число, єдиний параметр фільтра, який треба підібрати. Чому він такий, альфа-бета, іноді його ще називають якимось «композитним фільтром». Тому що цей коефіцієнт, він досить великий, він вбиває низькі частоти, а цей коефіцієнт маленький, він вбиває високочастотний шум. Виходить, що ви вбиваєте низьку частоту шуму від інтегрування, а ось тут вбиваєте високу частоту, той самий шум, який йде з самого датчика.


Все звичайно добре, але в цій же статті у продовженні йдеться про те, що у нас нікуди не дінеться дрейф навколо вектора прискорення вільного падіння Землі. Так що треба звідкись взяти ще один вектор, який не буде збігатися з вектором прискорення. Для цього й потрібний магнітометр, або як його ще називають — компас.
З усього цього випливає, що нам знадобиться три датчика: трехосевой акселерометр, трьохосьовий компас і трехосевой магнітометр. Три мікросхеми на кожен палець ставити не хочеться — багато місця займе. Необхідність визначати орієнтацію різних пристроїв в просторі виникає настільки часто, що випускають суміщені датчики. Наприклад акселерометр і гіроскоп в одному корпусі, або, як у випадку MPU-9250, всі три датчика в одному корпусі. З-за того, що у них три трехосевых датчика їх називають девятиосевыми. Цими датчиками і ми затарились.

2) Мікроконтролер
Хоч у минулій статті про це не було ні слова, але все таки це відмінність. У старій рукавичці стояв вподобаний мною за ціну і корпус STM32F050F4. Але в новій рукавичці його можливостей не вистачало. Ні потужності, ні ніг. Так що було вирішено купити що-небудь в корпусі LQFP64. Цим чимось виявився STM32F103. Так як ніг тепер було з надлишком, то можна було відмовитися від використання сдвигового регістра.

3)Розташування датчиків. А вірніше їх кількість. Тепер датчики були встановлені не тільки на пальці, але й на кисть, і на зап'ясті, і на передпліччі. Це дозволило визначати положення всієї руки в просторі, а не тільки пальців.

На цьому відмінності закінчуються, бо відрізнятися більше нічому. Хіба що рукавичка тепер на праву руку. Ліва рукавичка з цієї пари була використана в минулій версії:



Плата мікроконтролера



На платі розведені: два SPI, два USART, 15GPIO і SWD. Зліва знизу три поруч розташованих майданчики — місце для стабілізатора LP2950. Все інше за даташіту: конденсатори з харчування (тантал типорозміру D, все інше 0603, хоч при бажанні можна і 0805 запаяти на ті ж майданчики), конденсатор на ніжці Reset і конденсатори для кварцу. Перша нога мікроконтролера — ліва гребінка, верхній пін. Після запаювання всіх компонентів і проводів від датчиків все виглядає жахливо:



Мені нема чого сказати у виправдання, крім слова «час».

Плата датчика





Тут, як і в минулий раз, не обійшлося без перемички. Вона відзначена червоною лінією. На відміну від минулого рукавички, тут є невелика обв'язка в особі конденсатора 10 пФ зліва знизу і двох 0.1 мкФ на інших місцях. Схема підключення є у даташіте:



Думаю тут якісь пояснення зайві. Запаюють корпус QFN24, припаюємо перемичку і рассыпуху і запаюють шлейф. До речі, корпус QFN24 хоч і має менший крок ніжок, ніж LGA16, але зате ці ніжки виходять з боків мікросхеми і можна легко проконтролювати якість пайки, а в разі чого і паяльником запаяти невдалі ніжки:



Ініціалізація датчика



Власне дісталися до програмування всього цього неподобства. Тут відразу розповім про мої косяки на попередніх етапах: особисто я використовував мікроконтролер STM32F103RB. МК з префіксами R8 і RB мають не тільки спрощену пам'ять, але і спрощену периферію. Зокрема відсутня USART4, який є в їх більш «об'ємистих» побратимів. І по своїй неуважності я дуже довго намагався його включити. Адже він у мене розлучений. Рятує нас те, що USART1 був розлучений «на всякий випадок». Він ремапится на ніжки PB6 і PB7 мікроконтролера. Власне це ми і зробимо, коли будемо його ініціалізувати. Але повернемося до датчика.

Наведений тут код підходить і для MPU-6050. Це той же датчик, але без магнітометра.

Для початку потрібно ініціалізувати SPI. Для цього використовуємо ось таку функцію:
Ініціалізація SPI
void HeInitSpi(void)
{
RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA|RCC_APB2Periph_AFIO, ENABLE); //Включаємо тактування порту А й альтернативної функції GPIO
RCC_APB2PeriphClockCmd(RCC_APB2Periph_SPI1, ENABLE); //Включаємо тактування SPI

GPIO_InitTypeDef SpiPort;
SPI_InitTypeDef SPI_InitStructure;

SpiPort.GPIO_Mode = GPIO_Mode_AF_PP; //Альтернативна функція Push-Pull
SpiPort.GPIO_Speed = GPIO_Speed_10MHz; //Максимальна частота перемикання 10MHz
SpiPort.GPIO_Pin = GPIO_Pin_5 | GPIO_Pin_7 | GPIO_Pin_4; //Настроювані ноги - 5,6,7
GPIO_Init(GPIOA, &SpiPort); //Ініціалізувати порт А.
SpiPort.GPIO_Mode = GPIO_Mode_IPU; //Вхід з підтяжкою до плюса
SpiPort.GPIO_Pin = GPIO_Pin_6; //Настроюється нога - 6
GPIO_Init(GPIOA, &SpiPort); //Ініціалізувати порт А

SPI_InitStructure.SPI_Direction = SPI_Direction_2Lines_FullDuplex; //Двонаправлений обмін даними
SPI_InitStructure.SPI_DataSize = SPI_DataSize_8b; //Довжина пакету 8 біт
SPI_InitStructure.SPI_CPOL = SPI_CPOL_Low; //Активний рівень - низький
SPI_InitStructure.SPI_CPHA = SPI_CPHA_1Edge; //Вирівнювання по передньому фронту 
SPI_InitStructure.SPI_NSS = SPI_NSS_Soft; //Софтварное управління NSS
SPI_InitStructure.SPI_BaudRatePrescaler = SPI_BaudRatePrescaler_16; //Переддільник частоти тактирования SPI
SPI_InitStructure.SPI_FirstBit = SPI_FirstBit_MSB; 
SPI_InitStructure.SPI_Mode = SPI_Mode_Master; //Ми - майстер
SPI_Init(SPI1, &SPI_InitStructure); //Ініціалізація SPI
SPI_Cmd(SPI1, ENABLE); //Включення SPI

SPI_NSSInternalSoftwareConfig(SPI1, SPI_NSSInternalSoft_Set);
}


Тепер потрібно ініціалізувати ноги мікроконтролера, які підключені до пинам CS датчиків. Заведемо дефайны для всіх датчиків і напишемо декілька функцій, які спрощують нам життя (поки що не звертайте уваги, що у мене тільки 6 датчиків):
Код для датчиків
#define SENSOR_0_PORT GPIOA
#define SENSOR_1_PORT GPIOA
#define SENSOR_2_PORT GPIOA
#define SENSOR_3_PORT GPIOC
#define SENSOR_4_PORT GPIOC
#define SENSOR_5_PORT GPIOC

#define SENSOR_0_PIN GPIO_Pin_11
#define SENSOR_1_PIN GPIO_Pin_9
#define SENSOR_2_PIN GPIO_Pin_8
#define SENSOR_3_PIN GPIO_Pin_9
#define SENSOR_4_PIN GPIO_Pin_8
#define SENSOR_5_PIN GPIO_Pin_7

void HeInitSensorsPort(void)
{
RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA|RCC_APB2Periph_GPIOC|RCC_APB2Periph_AFIO, ENABLE);
GPIO_InitTypeDef SensorsGpioPin;
SensorsGpioPin.GPIO_Mode = GPIO_Mode_Out_PP;
SensorsGpioPin.GPIO_Speed = GPIO_Speed_10MHz;
SensorsGpioPin.GPIO_Pin = SENSOR_0_PIN|SENSOR_1_PIN|SENSOR_2_PIN;
GPIO_Init(SENSOR_0_PORT, &SensorsGpioPin);
SensorsGpioPin.GPIO_Pin = SENSOR_3_PIN|SENSOR_4_PIN|SENSOR_5_PIN;
GPIO_Init(SENSOR_5_PORT, &SensorsGpioPin);
}

void HeReselectSensors(void)
{
GPIO_SetBits(SENSOR_0_PORT, SENSOR_0_PIN);
GPIO_SetBits(SENSOR_1_PORT, SENSOR_1_PIN);
GPIO_SetBits(SENSOR_2_PORT, SENSOR_2_PIN);
GPIO_SetBits(SENSOR_3_PORT, SENSOR_3_PIN);
GPIO_SetBits(SENSOR_4_PORT, SENSOR_4_PIN);
GPIO_SetBits(SENSOR_5_PORT, SENSOR_5_PIN);
}

void HeSelectSensor (uint16_t data)
{
switch (data) {
case 0: HeReselectSensors(); GPIO_ResetBits(SENSOR_0_PORT, SENSOR_0_PIN); break;
case 1: HeReselectSensors(); GPIO_ResetBits(SENSOR_1_PORT, SENSOR_1_PIN); break;
case 2: HeReselectSensors(); GPIO_ResetBits(SENSOR_2_PORT, SENSOR_2_PIN); break;
case 3: HeReselectSensors(); GPIO_ResetBits(SENSOR_3_PORT, SENSOR_3_PIN); break;
case 4: HeReselectSensors(); GPIO_ResetBits(SENSOR_4_PORT, SENSOR_4_PIN); break;
case 5: HeReselectSensors(); GPIO_ResetBits(SENSOR_5_PORT, SENSOR_5_PIN); break;
}
}


Трохи кострубато виглядає. Працює це так: для вибору сенсора номер n я викликаю функцію HeSelectSensor(n). Після цього на ніжці, за якою закріплений у дефайнах сенсор буде виставлений логічний 0, що означає початок роботи з сенсором.
Також напишемо функції для спілкування з датчиками:
Функції для спілкування з датчиками
uint8_t HeSendToSpi(uint16_t nsensor, uint8_t addres, uint8_t data)
{
HeSelectSensor(nsensor);
SPI_I2S_SendData(SPI1, addres);
while(SPI_I2S_GetFlagStatus(SPI1, SPI_I2S_FLAG_BSY) == SET);
SPI_I2S_ReceiveData(SPI1);
SPI_I2S_ReceiveData(SPI1);
SPI_I2S_SendData(SPI1, data);
while(SPI_I2S_GetFlagStatus(SPI1, SPI_I2S_FLAG_BSY) == SET);
HeReselectSensors();
return SPI_I2S_ReceiveData(SPI1);
}

void writeByte(uint16_t sensor, uint8_t address, uint8_t subAddress, uint8_t data)
{
/*char data_write[2];
data_write[0] = subAddress;
data_write[1] = data;
i2c.write(address, data_write, 2, 0);*/
if (address==MPU9250_ADDRESS){
HeSelectSensor(sensor);
SPI_I2S_SendData(SPI1, subAddress);
while(SPI_I2S_GetFlagStatus(SPI1, SPI_I2S_FLAG_BSY) == SET);
SPI_I2S_ReceiveData(SPI1);
SPI_I2S_ReceiveData(SPI1);
SPI_I2S_SendData(SPI1, data);
while(SPI_I2S_GetFlagStatus(SPI1, SPI_I2S_FLAG_BSY) == SET);
SPI_I2S_ReceiveData(SPI1);
HeReselectSensors();
} else
if (address==AK8963_ADDRESS){
HeSelectSensor(sensor);
writeByte(sensor, MPU9250_ADDRESS, I2C_SLV4_ADDR, I2C_SLV4_ADDR&0b01111111);
writeByte(sensor, MPU9250_ADDRESS, I2C_SLV4_REG, subAddress);
writeByte(sensor, MPU9250_ADDRESS, I2C_SLV4_DO, data);
writeByte(sensor, MPU9250_ADDRESS, I2C_SLV4_CTRL, I2C_SLV4_CTRL|0b10000000);
int i;
for (i=0; i<0xFF; i++);
HeReselectSensors();
}
}

uint8_t readByte(uint16_t sensor, uint8_t address, uint8_t subAddress)
{
/*char data[1]; // `data` will store the data register
char data_write[1];
data_write[0] = subAddress;
i2c.write(address, data_write, 1, 1); // no stop
i2c.read(address, data, 1, 0); 
return data[0];*/
int d;
if (address==MPU9250_ADDRESS){
HeSelectSensor(sensor);
SPI_I2S_SendData(SPI1, subAddress|0b10000000);
while(SPI_I2S_GetFlagStatus(SPI1, SPI_I2S_FLAG_BSY) == SET);
SPI_I2S_ReceiveData(SPI1);
SPI_I2S_ReceiveData(SPI1);
SPI_I2S_SendData(SPI1, 0xAA);
while(SPI_I2S_GetFlagStatus(SPI1, SPI_I2S_FLAG_BSY) == SET);
d=SPI_I2S_ReceiveData(SPI1);
HeReselectSensors();
} else
if (address==AK8963_ADDRESS){
HeSelectSensor(sensor);
writeByte(sensor, MPU9250_ADDRESS, I2C_SLV4_ADDR, AK8963_ADDRESS|0b10000000);
writeByte(sensor, MPU9250_ADDRESS, I2C_SLV4_REG, subAddress);
writeByte(sensor, MPU9250_ADDRESS, I2C_SLV4_CTRL, I2C_SLV4_CTRL|0b10000000);
int i;
for (i=0; i<0xFF; i++);
d = readByte(sensor, MPU9250_ADDRESS, I2C_SLV4_DI);
HeReselectSensors();
}
return (d);
}

void readBytes(uint16_t sensor, uint8_t address, uint8_t subAddress, uint8_t count, uint8_t * dest)
{
/*char data[14];
char data_write[1];
data_write[0] = subAddress;
i2c.write(address, data_write, 1, 1); // no stop
i2c.read(address, data, count, 0);*/
int ii;
uint8_t d;
if (address==MPU9250_ADDRESS){
for(ii = 0; ii < count; ii++) {
HeSelectSensor(sensor);
SPI_I2S_SendData(SPI1, ((subAddress|0b10000000)+ii));
while(SPI_I2S_GetFlagStatus(SPI1, SPI_I2S_FLAG_BSY) == SET);
SPI_I2S_ReceiveData(SPI1);
SPI_I2S_ReceiveData(SPI1);
SPI_I2S_SendData(SPI1, 0xAA);
while(SPI_I2S_GetFlagStatus(SPI1, SPI_I2S_FLAG_BSY) == SET);
d=SPI_I2S_ReceiveData(SPI1);
dest[ii] = d;
HeReselectSensors();
}
} else
for(ii = 0; ii < count; ii++) {
if (address==AK8963_ADDRESS){
HeSelectSensor(sensor);
writeByte(sensor, MPU9250_ADDRESS, I2C_SLV4_ADDR, I2C_SLV4_ADDR|0b10000000);
writeByte(sensor, MPU9250_ADDRESS, I2C_SLV4_REG, subAddress+ii);
writeByte(sensor, MPU9250_ADDRESS, I2C_SLV4_CTRL, I2C_SLV4_CTRL|0b10000000);
int i;
for (i=0; i<0xFF; i++);
d = readByte(sensor, MPU9250_ADDRESS, I2C_SLV4_DI);
dest[ii] = d;
HeReselectSensors();
}
}
} 


Тепер можна спробувати перевірити працездатність датчиків. Для цього опитаємо регістр WHO_AM_I і подивимося відповідь. Якщо все добре, то значення цього регістра співпаде з дефолтними значенням з даташита:



Після ось такого пінгу я і з'ясував, що два датчика у мене не працюють. До літака залишалося 4 години, все обладнання було вже зібрано у валізу, так що я вирішив забити на два датчика, бо вони розташовувалися на пальцях, а не на зап'ясті або кисті. Фух. Тепер здається все приготування закінчилися і можна приступити до ініціалізації датчика. Спочатку я написав свою ініціалізацію, а потім у процесі роботи над рукавичкою знайшов бібліотеки, в якій все було добре написано і з коментарями. Але весь код був для плати F401-Nucleo та інтерфейсу SPI. Довелося переписувати. А результат переписування ініціалізації виглядає так:
Ініціалізація датчиків
void initMPU9250(uint16_t sensor)
{ 
// Initialize MPU9250 device
// wake up device
writeByte(sensor, MPU9250_ADDRESS, PWR_MGMT_1, 0x00); // Clear sleep mode bit (6), enable all sensors
int i;
for (i=0; i<0x3FF; i++); // Delay 100 ms for PLL to get established on x-axis gyro; should check for PLL ready interrupt
readByte(sensor, MPU9250_ADDRESS, WHO_AM_I_MPU9250);
// get stable time source
writeByte(sensor, MPU9250_ADDRESS, PWR_MGMT_1, 0x01); // Set clock source to be PLL with x-axis gyroscope reference, bits 2:0 = 001

// Configure Gyro and Accelerometer
// Disable FSYNC and set accelerometer and gyro bandwidth to 44 and 42 Hz, respectively; 
// DLPF_CFG = bits 2:0 = 010; this sets the sample rate at 1 kHz for both
// Maximum delay is 4.9 ms which is just over a 200 Hz maximum rate
writeByte(sensor, MPU9250_ADDRESS, CONFIG, 0x03);

// Set sample rate = gyroscope output rate/(1 + SMPLRT_DIV)
writeByte(sensor, MPU9250_ADDRESS, SMPLRT_DIV, 0x04); // Use a 200 Hz rate; the same rate set in CONFIG above

// Set gyroscope full scale range
// Range selects FS_SEL and AFS_SEL are 0 - 3, so 2-bit values are left-shifted into positions 4:3
uint8_t c = readByte(sensor, MPU9250_ADDRESS, GYRO_CONFIG);
writeByte(sensor, MPU9250_ADDRESS, GYRO_CONFIG, c & ~0xE0); // Clear self-test bits [7:5]
writeByte(sensor, MPU9250_ADDRESS, GYRO_CONFIG, c & ~0x18); // Clear AFS bits [4:3]
writeByte(sensor, MPU9250_ADDRESS, GYRO_CONFIG, c | Gscale << 3); // Set full scale range for the gyro

// Set configuration accelerometer
c = readByte(sensor, MPU9250_ADDRESS, ACCEL_CONFIG);
writeByte(sensor, MPU9250_ADDRESS, ACCEL_CONFIG, c & ~0xE0); // Clear self-test bits [7:5]
writeByte(sensor, MPU9250_ADDRESS, ACCEL_CONFIG, c & ~0x18); // Clear AFS bits [4:3]
writeByte(sensor, MPU9250_ADDRESS, ACCEL_CONFIG, c | Ascale << 3); // Set full scale range for the accelerometer

// Set accelerometer sample rate configuration
// It is possible to get a 4 kHz sample rate from the accelerometer by choosing 1 for
// accel_fchoice_b bit [3]; in this case the bandwidth is 1.13 kHz
c = readByte(sensor, MPU9250_ADDRESS, ACCEL_CONFIG2);
writeByte(sensor, MPU9250_ADDRESS, ACCEL_CONFIG2, c & ~0x0F); // Clear accel_fchoice_b (bit 3) and A_DLPFG (bits [2:0])
writeByte(sensor, MPU9250_ADDRESS, ACCEL_CONFIG2, c | 0x03); // Set accelerometer rate to 1 kHz and bandwidth to 41 Hz

// The accelerometer, gyro, and thermometer are set to 1 kHz sample rates, 
// but all these rates are further reduced by a factor of 5 to 200 Hz because of the setting SMPLRT_DIV

// Configure Interrupts and Enable Bypass
// Set interrupt pin active high, push-pull, and clear on read of INT_STATUS, enable I2C_BYPASS_EN so additional chips 
// can join the I2C bus and all can be controlled by the Arduino as master
writeByte(sensor, MPU9250_ADDRESS, INT_PIN_CFG, 0x22);
writeByte(sensor, MPU9250_ADDRESS, INT_ENABLE, 0x01); // Enable data ready (bit 0) interrupt

writeByte(sensor, MPU9250_ADDRESS, I2C_SLV4_ADDR, AK8963_ADDRESS); // Address of mag on I2C bus
writeByte(sensor, MPU9250_ADDRESS, I2C_MST_CTRL, I2C_MST_CTRL|0x0D); // Speed of I2C 400kHz
}


Подякуємо автора бібліотеки за заработавшую з першого разу ініціалізацію. Взагалі чудова бібліотека, але для I2C і для любителів бібліотеки mbed.

Тепер, коли датчики ініціалізується, ми можемо перейти до отримання даних.

Отримання і обробка даних



Для початку потрібно навчити контролер спілкуватися з комп'ютером. А то нікуди буде наші дані передавати. Для цього ініціалізуємо USART1. При цьому не забуваємо про ремап ніжок.
Ініціалізація USART
void HeInitUsartGpio(void)
{
GPIO_InitTypeDef UsartGPIO;

RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB|RCC_APB2Periph_AFIO, ENABLE);

UsartGPIO.GPIO_Mode = GPIO_Mode_AF_PP;
UsartGPIO.GPIO_Speed = GPIO_Speed_10MHz;
UsartGPIO.GPIO_Pin = GPIO_Pin_6;
GPIO_Init(GPIOB, &UsartGPIO);

UsartGPIO.GPIO_Mode = GPIO_Mode_IN_FLOATING;
UsartGPIO.GPIO_Pin = GPIO_Pin_7;
GPIO_Init(GPIOB, &UsartGPIO);

GPIO_PinRemapConfig(GPIO_Remap_USART1, ENABLE);
}
void HeInitUsart(void)
{
HeInitUsartGpio();

USART_InitTypeDef USART1_InitStructure;

RCC_APB2PeriphClockCmd(RCC_APB2Periph_USART1, ENABLE);

USART1_InitStructure.USART_BaudRate = 115200;
USART1_InitStructure.USART_HardwareFlowControl = USART_HardwareFlowControl_None;
USART1_InitStructure.USART_Mode = USART_Mode_Rx|USART_Mode_Tx;
USART1_InitStructure.USART_Parity = USART_Parity_No;
USART1_InitStructure.USART_StopBits = USART_StopBits_1;
USART1_InitStructure.USART_WordLength = USART_WordLength_8b;
USART_Init(USART1, &USART1_InitStructure);
USART_Cmd(USART1, ENABLE);
}
void HeSendToUsart (uint8_t data)
{
USART_SendData(USART1, data);
while(!(USART_GetFlagStatus(USART1, USART_FLAG_TC)));
USART_ClearFlag(USART1, USART_FLAG_TC);
}


Для відправки даних будемо використовувати функцію HeSendToUsart.

Тепер можна написати функцію main і переривання:
main і Ko
int main(void)
{
HeInitUsart();
HeInitSensorsPort();
HeInitSpi();
initMPU9250(0);

RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM4,ENABLE);

TIM4->PSC = 7200 - 1; 
TIM4->ARR = 100 ; 
TIM4->DIER |= TIM_DIER_UIE; 
TIM4->CR1 |= TIM_CR1_CEN; 

NVIC_InitTypeDef TIMQ;
TIMQ.NVIC_IRQChannel = TIM4_IRQn;
TIMQ.NVIC_IRQChannelCmd = ENABLE;
NVIC_Init(&TIMQ);

int j;

while(1){
int p;
for (p=0; p<18; p++)
{
HeSendToUsart(SensBuf[p]);
HeDelay(0xFF);
}
HeDelay(0x2FFFF);
}
return 0;
}

void TIM4_IRQHandler(void)
{
TIM4->SR &= ~TIM_SR_UIF;
int ax, ay, az, gx, gy, gz;
int j;
for (j=0; j<6; j++){
readAccelData(5-j, accelCount);
readGyroData(5-j, gyroCount);
ax = accelCount[0];
ay = accelCount[1];
az = accelCount[2];
gx = gyroCount[0];
gy = gyroCount[1];
gz = gyroCount[2];
hy[5-j] =((1 - (r/100)) * (hy[5-j] + (gy*0.00762939) * 0.01)) + ((r/100) * (90+(180/3.1415)*atan2(-az,-ax)));
hx[5-j] =((1 - (r/100)) * (hx[5-j] + (gx*0.00762939) * 0.01)) + ((r/100) * (180/3.1415)*atan2(ay,az));
hz[5-j] =((1 - (r/100)) * (hz[5-j] + (gz*0.00762939) * 0.01)) + ((r/100) * (90+(180/3.1415)*atan2(-ay, ax)));
SensBuf[0+j*3] = ((uint8_t)(int)(hx[5-j]*255/360));
SensBuf[1+j*3] = ((uint8_t)(int)(hy[5-j]*255/360));
SensBuf[2+j*3] = ((uint8_t)(int)(hz[5-j]*255/360));

}
}



Тут-то і відбувається фінт хвостом. По перше ми всі ініціалізуємо. Це як зазвичай. А потім навіщось включаємо таймер. Навіщо? А з його допомогою ми будемо через рівні проміжки часу складати свідчення гіроскопа. Таймер налаштований на 100 переривань в секунду. Так що частота опитування датчиків буде 100 sps. Потім йде нескінченний цикл, в якому зроблена затримка, після якої відправляється масив з даними датчиків. Масив виглядає як xyz першого датчика, xyz другого датчика і т. д. Всього 18 значень (3 осі на 6 датчиків).
У перериванні ми спочатку зчитуємо показання акселерометра і гіроскопа, а потім підставляємо в альфа-бета фільтр. Тут є два магічні числа. 0.00762939 — це дозвіл гіроскопа. Тобто з допомогою цього коефіцієнта ми переводимо число, прийняте від гіроскопа, в кут. Друге магічне число — 0.01. Це власне час між вимірами в секундах. Саме для цього і використовується таймер — щоб ми точно знали, що минув між вимірами час. Незрозуміла змінна r — коефіцієнт альфа-бета фільтра помножений на 100. Оголошений в самому початку програми і дорівнює 10.
Уф. Ну ось ніби і все! Можна отримувати дані. Щоправда ці дані не зовсім прості. Так як в USART ми відправляємо восьмибитное число, то довелося викрутитися з кутом: відправляється кількість 360/256 часток. Тобто куту нахилу в 90 градусів відповідає значення 63.

Ось і все!



Ура-ура. І ось на це я вбив тиждень і одну ніч. Ось вихідні коди проекту для середовища CooCox:
Проект
Якщо хтось працював з цими датчиками і у нього є робочий код роботи з магнітометрів через SPI, то я буду дуже вдячний за допомогу. Сподіваюся ця стаття виявилася цікавіше і корисніше попередньої.

P.s. Забув написати про найбільшу свою лажу! У мене магнітометр так і не запрацював( Тому я і прошу поділитися робочим кодом.

Джерело: Хабрахабр

0 коментарів

Тільки зареєстровані та авторизовані користувачі можуть залишати коментарі.