Миландр

void updateAngleCode0(){
	uint16_t count = 0;
	uint16_t data = 0;
	uint16_t dataArr[2];
	while (not spi::isInputEmpty(spi::Spi2)){
		spi::pop(spi::Spi2, data);
		dataArr[count] = data;
		if (++count > 1)
			break;
	}
        angleCode0.setData(dataArr, count);
        core::disableInterrupts();
       for (int i = 0; i < 2; i ++)
	{
		dataArr [i] = *angleCode0.TxBuffer;
		spi::push(spi::Spi2, dataArr [i]);
		angleCode0.TxBuffer++;

	}
	core::enableInterrupts();
}

bool ReadWriteRegistr ( uint16_t * Data )
{
	uint16_t data_rx = 0;
	if(!flag_rw)
	{

		uint16_t address =  *Data;
		uint16_t registr =  *++Data;

		core::disableInterrupts();

		if (!flag_address_rw)
		{
			spi::push(spi::Spi0, address );

			core::waitTimer(2, ( 5 * core::TIME_1mSec ) );

			flag_address_rw = true;
			flag_registr_rw = false;
		}
		else
		if (!flag_registr_rw)
		{
			spi::push(spi::Spi0, registr );

			core::waitTimer(2, ( 5 * core::TIME_1mSec ) );

			flag_address_rw = false;
			flag_registr_rw = true;

		}

		core::enableInterrupts();

		while (not spi::isInputEmpty(spi::Spi0))
		{
			spi::pop(spi::Spi0, data_rx);

			if (data_rx == registr)
			{
				flag_address_rw = false;
				flag_registr_rw = false;
				flag_rw = true;
				//spi::pop(spi::Spi0, data_rx);
				//if (data_rx == registr)
				//	return true;
			}

		}
		return false;
	}
	else if (flag_rw)
	{
		spi::pop(spi::Spi0, data_rx);
		flag_address_rw = false;
		flag_registr_rw = false;
		flag_rw = false;
		return true;
	}
}

void ReadRegistrReg2 ( uint16_t * Data )
{
	uint16_t address =  *Data;

	uint16_t registr =  0;

	core::disableInterrupts();

	spi::push(spi::Spi0, address);
	core::waitTimer(2, ( 1 * core::TIME_1mSec ) );

	core::enableInterrupts();

	while (not spi::isInputEmpty(spi::Spi0))
	{
		spi::pop(spi::Spi0, registr);
	}
}

void Get_Registrs_R2()
{

	//ReadRegistrReg2 ( (uint16_t*) &angleCode.Config_R2.Reg.pLL_config_0x00);
	//ReadRegistrReg2 ( (uint16_t*) &angleCode.Config_R2.Reg.aFE_config_0x02);
	//ReadRegistrReg2 ( (uint16_t*) &angleCode.Config_R2.Reg.mode_stat_0x04);
	//ReadRegistrReg2 ( (uint16_t*) &angleCode.Config_R2.Reg.c1Cntrl_0x18);
	//ReadRegistrReg2 ( (uint16_t*) &angleCode.Config_R2.Reg.c2Cntrl_0x58);
	//ReadRegistrReg2 ( (uint16_t*) &angleCode.Config_R2.Reg.c1ResCntrl_0x1A);
	//ReadRegistrReg2 ( (uint16_t*) &angleCode.Config_R2.Reg.c2ResCntrl_0x5A);
	//ReadRegistrReg2 ( (uint16_t*) &angleCode.Config_R2.Reg.c1EXInc_0x1E);
	ReadRegistrReg2 ( (uint16_t*) &angleCode.Config_R2.Reg.c1Coord_0x10);
	//ReadRegistrReg2 ( (uint16_t*) &angleCode.Config_R2.Reg.c1Stat_0x14);
}

void Set_Registrs_RW()
{
	count = 0;

	while	(!ReadWriteRegistr ( (uint16_t*) &angleCode.pLL_config_0x00))	count++;
	while	(!ReadWriteRegistr ( (uint16_t*) &angleCode.aFE_config_0x02))	count++;
	while	(!ReadWriteRegistr ( (uint16_t*) &angleCode.c1Cntrl_0x18))		count++;
	//while	(!ReadWriteRegistr ( (uint16_t*) &angleCode.c2Cntrl_0x58))		count++;
	while	(!ReadWriteRegistr ( (uint16_t*) &angleCode.c1ResCntrl_0x1A))	count++;
	//while	(!ReadWriteRegistr ( (uint16_t*) &angleCode.c2ResCntrl_0x5A))	count++;
	while	(!ReadWriteRegistr ( (uint16_t*) &angleCode.c1EXInc_0x1E))		count++;
	//while	(!ReadWriteRegistr ( (uint16_t*) &angleCode.c2EXInc_0x5E))		count++;
	while	(!ReadWriteRegistr ( (uint16_t*) &angleCode.c1Mask_0x16))		count++;
	//while	(!ReadWriteRegistr ( (uint16_t*) &angleCode.c2Mask_0x56))		count++;
	while	(!ReadWriteRegistr ( (uint16_t*) &angleCode.mask_0x06))		count++;
	for (int i = 0; i < 4; i ++)
	{
		ReadWriteRegistr ( (uint16_t*) &angleCode.mode_stat_0x04);
		count++;
	}

}