Вторая версия перчатки для определения положения руки

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);
}

#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;
	}
}

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();
			}
		}
} 

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
}

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);
}

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));

	}
}