Отличия от прошлой версии
1)В первую очередь это датчики. Углы наклона конечно можно определить с помощью акселерометров, но помимо шумов акселерометра нам будут мешать еще и любые движения человека, т.к. при движении руки на нее действуют различные ускорения, а не только ускорение свободного падения Земли. Тогда мы решили взять гироскоп. Но MEMS гироскоп — это вовсе не тот гироскоп, который нам показывали на уроках физики. Этот гироскоп выдает скорость изменения угла. Так что для получения полного угла отклонения от начального положения нам нужно эти показания интегрировать. Так мало того, что сигнал получается дискретным, так еще имеется так называемый «дрейф нуля». В состоянии покоя гироскоп выдает не ноль, а очень маленькое число, отличное от нуля. Так что при интегрировании будет накапливаться ошибка. Это называется низкочастотным шумом гироскопа. Что же делать? Тут нам на помощь приходит альфа-бета фильтр. Выглядит он вот как:
Картинка нагло скопирована из статьи 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. Для этого используем вот такую функцию:
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 register data
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 accelerometer configuration
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 SMPLRT_DIV setting
// Configure Interrupts and Bypass Enable
// 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. При этом не забываем про ремап ножек.
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 и прерывания:
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. Забыл написать про самую большую свою лажу! У меня магнитометр так и не заработал( Поэтому я и прошу поделиться рабочим кодом.
Recommended article: Chomsky: We Are All – Fill in the Blank.
This entry passed through the Full-Text RSS service - if this is your content and you're reading it on someone else's site, please read the FAQ at http://ift.tt/jcXqJW.
Комментариев нет:
Отправить комментарий