void ANO_DT::Send_Senser2(s32 alt_bar,u16 alt_csb)
{
u8 _cnt=0;
vs32 _temp;
data_to_send[_cnt++]=0xAA;
data_to_send[_cnt++]=0xAA;
data_to_send[_cnt++]=0x07;
data_to_send[_cnt++]=0;
_temp = alt_bar;
data_to_send[_cnt++]=BYTE3(_temp);
data_to_send[_cnt++]=BYTE2(_temp);
data_to_send[_cnt++]=BYTE1(_temp);
data_to_send[_cnt++]=BYTE0(_temp);
_temp = alt_csb;
data_to_send[_cnt++]=BYTE1(_temp);
data_to_send[_cnt++]=BYTE0(_temp);
data_to_send[3] = _cnt-4;
u8 sum = 0;
for(u8 i=0;i<_cnt;i++)
sum += data_to_send[i];
data_to_send[_cnt++] = sum;
Send_Data(data_to_send, _cnt);
}
static void write_multibyte(long i)
{
if (BYTE1(i) != 0) buffer[last++] = BYTE1(i);
if (BYTE2(i) != 0) buffer[last++] = BYTE2(i);
/* always */ buffer[last++] = BYTE3(i);
/* always */ buffer[last++] = BYTE4(i);
}
void Data_Send_VOTAGE(void)
{
u8 _cnt=0;
float adc;
adc = (ADC_ConvertedValue / 4096.0) * 3.3 * 11.0;
ADC_ConvertedValue = (u32)(adc * 100);
data_to_send[_cnt++]=0xAA;
data_to_send[_cnt++]=0xAA;
data_to_send[_cnt++]=0x05;
data_to_send[_cnt++]=0;
data_to_send[_cnt++]=BYTE1(ADC_ConvertedValue);
data_to_send[_cnt++]=BYTE0(ADC_ConvertedValue);
data_to_send[_cnt++]=BYTE1(ADC_ConvertedValue);
data_to_send[_cnt++]=BYTE0(ADC_ConvertedValue);
data_to_send[_cnt++]=BYTE1(ADC_ConvertedValue);
data_to_send[_cnt++]=BYTE0(ADC_ConvertedValue);
data_to_send[_cnt++]=BYTE1(ADC_ConvertedValue);
data_to_send[_cnt++]=BYTE0(ADC_ConvertedValue);
data_to_send[3] = _cnt-4;
u8 sum = 0;
for(u8 i=0;i<_cnt;i++)
sum += data_to_send[i];
data_to_send[_cnt++]=sum;
#ifdef DATA_TRANSFER_USE_USART
Uart2_Put_Buf(data_to_send,_cnt);
#else
NRF_TxPacket(data_to_send,_cnt);
#endif
}
//■4：发送PWM值
void Data_Send_MotoPWM(void)
{
u8 _cnt=0;
u8 i;
data_to_send[_cnt++]=0xAA;
data_to_send[_cnt++]=0xAA;
data_to_send[_cnt++]=0x06;
data_to_send[_cnt++]=0;
data_to_send[_cnt++]=BYTE1(Pwm_Para.motor_front_pwm);
data_to_send[_cnt++]=BYTE0(Pwm_Para.motor_front_pwm);
data_to_send[_cnt++]=BYTE1(Pwm_Para.motor_back_pwm);
data_to_send[_cnt++]=BYTE0(Pwm_Para.motor_back_pwm);
data_to_send[_cnt++]=BYTE1(Pwm_Para.motor_left_pwm);
data_to_send[_cnt++]=BYTE0(Pwm_Para.motor_left_pwm);
data_to_send[_cnt++]=BYTE1(Pwm_Para.motor_right_pwm);
data_to_send[_cnt++]=BYTE0(Pwm_Para.motor_right_pwm); //四个电机油门
data_to_send[3] = _cnt-4;
u8 sum = 0;
for(i=0;i<_cnt;i++)
sum += data_to_send[i];
data_to_send[_cnt++]=sum;
Uart0_Put_Buf(data_to_send,_cnt);
}
void ANO_DT::Send_Status(void)
{
u8 _cnt=0;
data_to_send[_cnt++]=0xAA;
data_to_send[_cnt++]=0xAA;
data_to_send[_cnt++]=0x01;
data_to_send[_cnt++]=0;
vs16 _temp;
_temp = (int)(imu.angle.x*100);
data_to_send[_cnt++]=BYTE1(_temp);
data_to_send[_cnt++]=BYTE0(_temp);
_temp = (int)(imu.angle.y*100);
data_to_send[_cnt++]=BYTE1(_temp);
data_to_send[_cnt++]=BYTE0(_temp);
_temp = (int)(imu.angle.z*100);
data_to_send[_cnt++]=BYTE1(_temp);
data_to_send[_cnt++]=BYTE0(_temp);
vs32 _temp2 = 0;//UltraAlt * 100;
data_to_send[_cnt++]=BYTE3(_temp2);
data_to_send[_cnt++]=BYTE2(_temp2);
data_to_send[_cnt++]=BYTE1(_temp2);
data_to_send[_cnt++]=BYTE0(_temp2);
data_to_send[3] = _cnt-4;
u8 sum = 0;
for(u8 i=0;i<_cnt;i++)
sum += data_to_send[i];
data_to_send[_cnt++]=sum;
Send_Data(data_to_send, _cnt);
}
/* -------------发送PWM值---------------------------- */
void Data_Send_MotoPWM(void)
{
uint8_t _cnt=0;
data_to_send[_cnt++]=0xAA;
data_to_send[_cnt++]=0xAA;
data_to_send[_cnt++]=0x02;
// data_to_send[_cnt++]=0x06;
data_to_send[_cnt++]=0;
data_to_send[_cnt++]=BYTE1(MOTOR4); //X轴正向电机
data_to_send[_cnt++]=BYTE0(MOTOR4);
data_to_send[_cnt++]=BYTE1(MOTOR3); //X轴负向电机
data_to_send[_cnt++]=BYTE0(MOTOR3);
data_to_send[_cnt++]=BYTE1(MOTOR2); //Y轴正向电机
data_to_send[_cnt++]=BYTE0(MOTOR2);
data_to_send[_cnt++]=BYTE1(MOTOR1); //Y轴负向电机
data_to_send[_cnt++]=BYTE0(MOTOR1);
// data_to_send[_cnt++]=BYTE1(Moto_PWM_5);
// data_to_send[_cnt++]=BYTE0(Moto_PWM_5);
// data_to_send[_cnt++]=BYTE1(Moto_PWM_6);
// data_to_send[_cnt++]=BYTE0(Moto_PWM_6);
// data_to_send[_cnt++]=BYTE1(Moto_PWM_7);
// data_to_send[_cnt++]=BYTE0(Moto_PWM_7);
// data_to_send[_cnt++]=BYTE1(Moto_PWM_8);
// data_to_send[_cnt++]=BYTE0(Moto_PWM_8);
data_to_send[3] = _cnt-4;
uint8_t sum = 0;
for(uint8_t i=0;i<_cnt;i++)
sum += data_to_send[i];
data_to_send[_cnt++]=sum;
HAL_UART_Transmit_DMA(&huart2, data_to_send, _cnt);;
}
void Data_Send_Voltage(T_RC_Voltage *data)
{
rt_uint8_t _cnt=0;
data_to_send[_cnt++]=0XAA;
data_to_send[_cnt++]=0xAA;
data_to_send[_cnt++]=0x05;//µçѹ¹¦ÄÜ×Ö
data_to_send[_cnt++]=0;
rt_uint16_t _temp;//µçѹ¿Ï¶¨ÎªÕý
_temp = data->Voltage1*100;
data_to_send[_cnt++]=BYTE1(_temp);
data_to_send[_cnt++]=BYTE0(_temp);
_temp = data->Voltage2*100;
data_to_send[_cnt++]=BYTE1(_temp);
data_to_send[_cnt++]=BYTE0(_temp);
_temp = data->Voltage3*100;
data_to_send[_cnt++]=BYTE1(_temp);
data_to_send[_cnt++]=BYTE0(_temp);
data_to_send[3] = _cnt-4;
rt_uint8_t sum = 0;
for(rt_uint8_t i=0;i<_cnt;i++)
sum += data_to_send[i];
data_to_send[_cnt++]=sum;
NRF24L01_TxPacket(data_to_send);
}
void ANO_DT_Send_Power(u16 votage, u16 current)
{
u8 _cnt=0;
u16 temp;
data_to_send[_cnt++]=0xAA;
data_to_send[_cnt++]=0xAA;
data_to_send[_cnt++]=0x05;
data_to_send[_cnt++]=0;
temp = votage;
data_to_send[_cnt++]=BYTE1(temp);
data_to_send[_cnt++]=BYTE0(temp);
temp = current;
data_to_send[_cnt++]=BYTE1(temp);
data_to_send[_cnt++]=BYTE0(temp);
data_to_send[3] = _cnt-4;
u8 sum = 0;
for(u8 i=0;i<_cnt;i++)
sum += data_to_send[i];
data_to_send[_cnt++]=sum;
ANO_DT_Send_Data(data_to_send, _cnt);
}
/*
* The IXP4xx expansion bus only allows 16-bit wide acceses
* when attached to a 16-bit wide device (such as the 28F128J3A),
* so we can't just memcpy_fromio().
*/
static void ixp4xx_copy_from(struct map_info *map, void *to,
unsigned long from, ssize_t len)
{
u8 *dest = (u8 *) to;
void __iomem *src = map->virt + from;
if (len <= 0)
return;
if (from & 1) {
*dest++ = BYTE1(flash_read16(src-1));
src++;
--len;
}
while (len >= 2) {
u16 data = flash_read16(src);
*dest++ = BYTE0(data);
*dest++ = BYTE1(data);
src += 2;
len -= 2;
}
if (len > 0)
*dest++ = BYTE0(flash_read16(src));
}
void ANO_DT_Send_Version(u8 hardware_type, u16 hardware_ver,u16 software_ver,u16 protocol_ver,u16 bootloader_ver)
{
u8 _cnt=0;
data_to_send[_cnt++]=0xAA;
data_to_send[_cnt++]=0xAA;
data_to_send[_cnt++]=0x00;
data_to_send[_cnt++]=0;
data_to_send[_cnt++]=hardware_type;
data_to_send[_cnt++]=BYTE1(hardware_ver);
data_to_send[_cnt++]=BYTE0(hardware_ver);
data_to_send[_cnt++]=BYTE1(software_ver);
data_to_send[_cnt++]=BYTE0(software_ver);
data_to_send[_cnt++]=BYTE1(protocol_ver);
data_to_send[_cnt++]=BYTE0(protocol_ver);
data_to_send[_cnt++]=BYTE1(bootloader_ver);
data_to_send[_cnt++]=BYTE0(bootloader_ver);
data_to_send[3] = _cnt-4;
u8 sum = 0;
for(u8 i=0;i<_cnt;i++)
sum += data_to_send[i];
data_to_send[_cnt++]=sum;
ANO_DT_Send_Data(data_to_send, _cnt);
}
void Data_Send_RCData(void)
{
u8 _cnt=0;
data_to_send[_cnt++]=0xAA;
data_to_send[_cnt++]=0xAA;
data_to_send[_cnt++]=0x03;
data_to_send[_cnt++]=0;
data_to_send[_cnt++]=BYTE1(Rc_D.THROTTLE);
data_to_send[_cnt++]=BYTE0(Rc_D.THROTTLE);
data_to_send[_cnt++]=BYTE1(Rc_D.YAW);
data_to_send[_cnt++]=BYTE0(Rc_D.YAW);
data_to_send[_cnt++]=BYTE1(Rc_D.ROLL);
data_to_send[_cnt++]=BYTE0(Rc_D.ROLL);
data_to_send[_cnt++]=BYTE1(Rc_D.PITCH);
data_to_send[_cnt++]=BYTE0(Rc_D.PITCH);
data_to_send[3] = _cnt-4;
u8 sum = 0;
for(u8 i=0;i<_cnt;i++)
sum += data_to_send[i];
data_to_send[_cnt++]=sum;
#ifdef DATA_TRANSFER_USE_USART
Uart2_Put_Buf(data_to_send,_cnt);
#else
NRF_TxPacket(data_to_send,_cnt);
#endif
}
void Data_Send_OFFSET(void)
{
u8 _cnt=0;
data_to_send[_cnt++]=0xAA;
data_to_send[_cnt++]=0xAA;
data_to_send[_cnt++]=0x16;
data_to_send[_cnt++]=0;
vs16 _temp = AngleOffset_Rol*1000;
data_to_send[_cnt++]=BYTE1(_temp);
data_to_send[_cnt++]=BYTE0(_temp);
_temp = AngleOffset_Pit*1000;
data_to_send[_cnt++]=BYTE1(_temp);
data_to_send[_cnt++]=BYTE0(_temp);
data_to_send[3] = _cnt-4;
u8 sum = 0;
for(u8 i=0;i<_cnt;i++)
sum += data_to_send[i];
data_to_send[_cnt++]=sum;
#ifdef DATA_TRANSFER_USE_USART
Uart1_Put_Buf(data_to_send,_cnt);
#else
NRF_TxPacket(data_to_send,_cnt);
#endif
}
void Data_Send_MotoPWM(void)
{
vs16 a,b,c,d;
a=Moto_PWM_1-1000;
b=Moto_PWM_2-1000;
c=Moto_PWM_3-1000;
d=Moto_PWM_4-1000;
u8 _cnt=0;
data_to_send[_cnt++]=0xAA;
data_to_send[_cnt++]=0xAA;
data_to_send[_cnt++]=0x06;
data_to_send[_cnt++]=0;
data_to_send[_cnt++]=BYTE1(a);//((Moto_PWM_1-1000));
data_to_send[_cnt++]=BYTE0(a);//((Moto_PWM_1-1000));
data_to_send[_cnt++]=BYTE1(b);//((Moto_PWM_2-1000));
data_to_send[_cnt++]=BYTE0(b);//((Moto_PWM_2-1000));
data_to_send[_cnt++]=BYTE1(c);//((Moto_PWM_3-1000));
data_to_send[_cnt++]=BYTE0(c);//((Moto_PWM_3-1000));
data_to_send[_cnt++]=BYTE1(d);//((Moto_PWM_4-1000));
data_to_send[_cnt++]=BYTE0(d);//((Moto_PWM_4-1000));
data_to_send[3] = _cnt-4;
u8 sum = 0;
for(u8 i=0;i<_cnt;i++)
sum += data_to_send[i];
data_to_send[_cnt++]=sum;
#ifdef DATA_TRANSFER_USE_USART
Uart2_Put_Buf(data_to_send,_cnt);
#else
NRF_TxPacket(data_to_send,_cnt);
#endif
}
uint32_t * vtuDownsampleImageRGBA(const uint32_t *tex)
{
uint32_t *smallTex = (uint32_t *)malloc(c.pageDimension * c.pageDimension);
assert(smallTex);
for (uint16_t x = 0; x < c.pageDimension / 2; x++)
{
for (uint16_t y = 0; y < c.pageDimension / 2; y++)
{
#ifdef COLOR_CODE_MIPPED_PHYSTEX
smallTex[y * (c.pageDimension / 2) + x] = (255 << 24) + (0 << 16) + (0 << 8) + 255;
#else
uint32_t pix1 = tex[(y*2) * c.pageDimension + (x*2)];
uint32_t pix2 = tex[(y*2+1) * c.pageDimension + (x*2)];
uint32_t pix3 = tex[(y*2) * c.pageDimension + (x*2+1)];
uint32_t pix4 = tex[(y*2+1) * c.pageDimension + (x*2+1)];
uint32_t b1 = BYTE1(pix1) + BYTE1(pix2) + BYTE1(pix3) + BYTE1(pix4);
uint32_t b2 = BYTE2(pix1) + BYTE2(pix2) + BYTE2(pix3) + BYTE2(pix4);
uint32_t b3 = BYTE3(pix1) + BYTE3(pix2) + BYTE3(pix3) + BYTE3(pix4);
uint32_t b4 = BYTE4(pix1) + BYTE4(pix2) + BYTE4(pix3) + BYTE4(pix4);
smallTex[y * (c.pageDimension / 2) + x] = ((b4 / 4) << 24) + ((b3 / 4) << 16) + ((b2 / 4) << 8) + (b1 / 4); // ARGB
#endif
}
}
return smallTex;
}
bool Communication::SendRcvControlQuantity(float aux1,float aux2,float aux3,float aux4,float aux5,float aux6)
{
u8 _cnt=0;
u8 data_to_send[30];
vs16 _temp;
data_to_send[_cnt++]=0xAA;
data_to_send[_cnt++]=0xAA;
data_to_send[_cnt++]=0x03;
data_to_send[_cnt++]=0;
data_to_send[_cnt++]=BYTE1(mRcvTargetThr);
data_to_send[_cnt++]=BYTE0(mRcvTargetThr);
data_to_send[_cnt++]=BYTE1(mRcvTargetYaw);
data_to_send[_cnt++]=BYTE0(mRcvTargetYaw);
data_to_send[_cnt++]=BYTE1(mRcvTargetRoll);
data_to_send[_cnt++]=BYTE0(mRcvTargetRoll);
data_to_send[_cnt++]=BYTE1(mRcvTargetPitch);
data_to_send[_cnt++]=BYTE0(mRcvTargetPitch);
_temp = (int)(aux1*100);
data_to_send[_cnt++]=BYTE1(_temp);
data_to_send[_cnt++]=BYTE0(_temp);
_temp = (int)(aux2*100);
data_to_send[_cnt++]=BYTE1(_temp);
data_to_send[_cnt++]=BYTE0(_temp);
_temp = (int)(aux3*100);
data_to_send[_cnt++]=BYTE1(_temp);
data_to_send[_cnt++]=BYTE0(_temp);
_temp = (int)(aux4*100);
data_to_send[_cnt++]=BYTE1(_temp);
data_to_send[_cnt++]=BYTE0(_temp);
_temp = (int)(aux5*100);
data_to_send[_cnt++]=BYTE1(_temp);
data_to_send[_cnt++]=BYTE0(_temp);
_temp = (int)(aux6*100);
data_to_send[_cnt++]=BYTE1(_temp);
data_to_send[_cnt++]=BYTE0(_temp);
data_to_send[3] = _cnt-4;
u8 sum = 0;
for(u8 i=0;i<_cnt;i++)
sum += data_to_send[i];
data_to_send[_cnt++] = sum;
usart.SendData(data_to_send, _cnt);
return true;
}
//¡ö8£º·¢ËÍPID4
void Data_Send_PID4(void)
{
u8 _cnt = 0;
data_to_send[_cnt++] = 0xAA;
data_to_send[_cnt++] = 0xAA;
data_to_send[_cnt++] = 0x13;
data_to_send[_cnt++] = 0;
vs16 _temp;
_temp = (vs16)(PID_PID_5.P * PID_PID_5_P_MULTIPLYING); data_to_send[_cnt++] = BYTE1(_temp); data_to_send[_cnt++] = BYTE0(_temp);
_temp = (vs16)(PID_PID_5.I * PID_PID_5_I_MULTIPLYING); data_to_send[_cnt++] = BYTE1(_temp); data_to_send[_cnt++] = BYTE0(_temp);
_temp = (vs16)(PID_PID_5.D * PID_PID_5_D_MULTIPLYING); data_to_send[_cnt++] = BYTE1(_temp); data_to_send[_cnt++] = BYTE0(_temp);
_temp = (vs16)(PID_PID_6.P * PID_PID_6_P_MULTIPLYING); data_to_send[_cnt++] = BYTE1(_temp); data_to_send[_cnt++] = BYTE0(_temp);
_temp = (vs16)(PID_PID_6.I * PID_PID_6_I_MULTIPLYING); data_to_send[_cnt++] = BYTE1(_temp); data_to_send[_cnt++] = BYTE0(_temp);
_temp = (vs16)(PID_PID_6.D * PID_PID_6_D_MULTIPLYING); data_to_send[_cnt++] = BYTE1(_temp); data_to_send[_cnt++] = BYTE0(_temp);
_temp = (vs16)(PID_PID_7.P * PID_PID_7_P_MULTIPLYING); data_to_send[_cnt++] = BYTE1(_temp); data_to_send[_cnt++] = BYTE0(_temp);
_temp = (vs16)(PID_PID_7.I * PID_PID_7_I_MULTIPLYING); data_to_send[_cnt++] = BYTE1(_temp); data_to_send[_cnt++] = BYTE0(_temp);
_temp = (vs16)(PID_PID_7.D * PID_PID_7_D_MULTIPLYING); data_to_send[_cnt++] = BYTE1(_temp); data_to_send[_cnt++] = BYTE0(_temp);
data_to_send[3] = _cnt - 4;
u8 sum = 0;
for (u8 i = 0; i < _cnt; i++)
sum += data_to_send[i];
data_to_send[_cnt++] = sum;
#ifdef DATA_TRANSFER_USE_USART
Sys_sPrintf(Printf_USART, data_to_send, _cnt);
#else
NRF_TxPacket(data_to_send, _cnt);
#endif
}
//¡ö8£º·¢ËÍPID3
void Data_Send_PID3(void)
{
u8 _cnt = 0;
data_to_send[_cnt++] = 0xAA;
data_to_send[_cnt++] = 0xAA;
data_to_send[_cnt++] = 0x12;
data_to_send[_cnt++] = 0;
vs16 _temp;
_temp = (vs16)(PID_PID_2.P * PID_PID_2_P_MULTIPLYING); data_to_send[_cnt++] = BYTE1(_temp); data_to_send[_cnt++] = BYTE0(_temp);
_temp = (vs16)(PID_PID_2.I * PID_PID_2_I_MULTIPLYING); data_to_send[_cnt++] = BYTE1(_temp); data_to_send[_cnt++] = BYTE0(_temp);
_temp = (vs16)(PID_PID_2.D * PID_PID_2_D_MULTIPLYING); data_to_send[_cnt++] = BYTE1(_temp); data_to_send[_cnt++] = BYTE0(_temp);
_temp = (vs16)(PID_PID_3.P * PID_PID_3_P_MULTIPLYING); data_to_send[_cnt++] = BYTE1(_temp); data_to_send[_cnt++] = BYTE0(_temp);
_temp = (vs16)(PID_PID_3.I * PID_PID_3_I_MULTIPLYING); data_to_send[_cnt++] = BYTE1(_temp); data_to_send[_cnt++] = BYTE0(_temp);
_temp = (vs16)(PID_PID_3.D * PID_PID_3_D_MULTIPLYING); data_to_send[_cnt++] = BYTE1(_temp); data_to_send[_cnt++] = BYTE0(_temp);
_temp = (vs16)(PID_PID_4.P * PID_PID_4_P_MULTIPLYING); data_to_send[_cnt++] = BYTE1(_temp); data_to_send[_cnt++] = BYTE0(_temp);
_temp = (vs16)(PID_PID_4.I * PID_PID_4_I_MULTIPLYING); data_to_send[_cnt++] = BYTE1(_temp); data_to_send[_cnt++] = BYTE0(_temp);
_temp = (vs16)(PID_PID_4.D * PID_PID_4_D_MULTIPLYING); data_to_send[_cnt++] = BYTE1(_temp); data_to_send[_cnt++] = BYTE0(_temp);
data_to_send[3] = _cnt - 4;
u8 sum = 0;
for (u8 i = 0; i < _cnt; i++)
sum += data_to_send[i];
data_to_send[_cnt++] = sum;
#ifdef DATA_TRANSFER_USE_USART
Sys_sPrintf(Printf_USART, data_to_send, _cnt);
#else
NRF_TxPacket(data_to_send, _cnt);
#endif
}
void Data_Send_PID2(void)
{
u8 _cnt = 0;
data_to_send[_cnt++] = 0xAA;
data_to_send[_cnt++] = 0xAA;
data_to_send[_cnt++] = 0x11;
data_to_send[_cnt++] = 0;
vs16 _temp;
_temp = (vs16)(PID_ALT.P * PID_ALT_P_MULTIPLYING ); data_to_send[_cnt++] = BYTE1(_temp); data_to_send[_cnt++] = BYTE0(_temp);
_temp = (vs16)(PID_ALT.I * PID_ALT_I_MULTIPLYING ); data_to_send[_cnt++] = BYTE1(_temp); data_to_send[_cnt++] = BYTE0(_temp);
_temp = (vs16)(PID_ALT.D * PID_ALT_D_MULTIPLYING ); data_to_send[_cnt++] = BYTE1(_temp); data_to_send[_cnt++] = BYTE0(_temp);
_temp = (vs16)(PID_POS.P * PID_POS_P_MULTIPLYING ); data_to_send[_cnt++] = BYTE1(_temp); data_to_send[_cnt++] = BYTE0(_temp);
_temp = (vs16)(PID_POS.I * PID_POS_I_MULTIPLYING ); data_to_send[_cnt++] = BYTE1(_temp); data_to_send[_cnt++] = BYTE0(_temp);
_temp = (vs16)(PID_POS.D * PID_POS_D_MULTIPLYING ); data_to_send[_cnt++] = BYTE1(_temp); data_to_send[_cnt++] = BYTE0(_temp);
_temp = (vs16)(PID_PID_1.P * PID_PID_1_P_MULTIPLYING); data_to_send[_cnt++] = BYTE1(_temp); data_to_send[_cnt++] = BYTE0(_temp);
_temp = (vs16)(PID_PID_1.I * PID_PID_1_I_MULTIPLYING); data_to_send[_cnt++] = BYTE1(_temp); data_to_send[_cnt++] = BYTE0(_temp);
_temp = (vs16)(PID_PID_1.D * PID_PID_1_D_MULTIPLYING); data_to_send[_cnt++] = BYTE1(_temp); data_to_send[_cnt++] = BYTE0(_temp);
data_to_send[3] = _cnt - 4;
u8 sum = 0;
for (u8 i = 0; i < _cnt; i++)
sum += data_to_send[i];
data_to_send[_cnt++] = sum;
#ifdef DATA_TRANSFER_USE_USART
Sys_sPrintf(Printf_USART, data_to_send, _cnt);
#else
NRF_TxPacket(data_to_send, _cnt);
#endif
}
void Data_Send_PID1(void)
{
u8 _cnt = 0;
data_to_send[_cnt++] = 0xAA;
data_to_send[_cnt++] = 0xAA;
data_to_send[_cnt++] = 0x10;
data_to_send[_cnt++] = 0;
vs16 _temp;
_temp = PID_ROL.P * PID_ROL_P_MULTIPLYING; data_to_send[_cnt++] = BYTE1(_temp); data_to_send[_cnt++] = BYTE0(_temp);
_temp = PID_ROL.I * PID_ROL_I_MULTIPLYING; data_to_send[_cnt++] = BYTE1(_temp); data_to_send[_cnt++] = BYTE0(_temp);
_temp = PID_ROL.D * PID_ROL_D_MULTIPLYING; data_to_send[_cnt++] = BYTE1(_temp); data_to_send[_cnt++] = BYTE0(_temp);
_temp = PID_PIT.P * PID_PIT_P_MULTIPLYING; data_to_send[_cnt++] = BYTE1(_temp); data_to_send[_cnt++] = BYTE0(_temp);
_temp = PID_PIT.I * PID_PIT_I_MULTIPLYING; data_to_send[_cnt++] = BYTE1(_temp); data_to_send[_cnt++] = BYTE0(_temp);
_temp = PID_PIT.D * PID_PIT_D_MULTIPLYING; data_to_send[_cnt++] = BYTE1(_temp); data_to_send[_cnt++] = BYTE0(_temp);
_temp = PID_YAW.P * PID_YAW_P_MULTIPLYING; data_to_send[_cnt++] = BYTE1(_temp); data_to_send[_cnt++] = BYTE0(_temp);
_temp = PID_YAW.I * PID_YAW_I_MULTIPLYING; data_to_send[_cnt++] = BYTE1(_temp); data_to_send[_cnt++] = BYTE0(_temp);
_temp = PID_YAW.D * PID_YAW_D_MULTIPLYING; data_to_send[_cnt++] = BYTE1(_temp); data_to_send[_cnt++] = BYTE0(_temp);
data_to_send[3] = _cnt - 4;
u8 sum = 0;
for (u8 i = 0; i < _cnt; i++)
sum += data_to_send[i];
data_to_send[_cnt++] = sum;
#ifdef DATA_TRANSFER_USE_USART
Sys_sPrintf(Printf_USART, data_to_send, _cnt);
#else
NRF_TxPacket(data_to_send, _cnt);
#endif
}
void Data_Send_PID5(void)
{
u8 _cnt = 0;
data_to_send[_cnt++] = 0xAA;
data_to_send[_cnt++] = 0xAA;
data_to_send[_cnt++] = 0x14;
data_to_send[_cnt++] = 0;
vs16 _temp;
_temp = (vs16)(PID_PID_8.P * PID_PID_8_P_MULTIPLYING ); data_to_send[_cnt++] = BYTE1(_temp); data_to_send[_cnt++] = BYTE0(_temp);
_temp = (vs16)(PID_PID_8.I * PID_PID_8_I_MULTIPLYING ); data_to_send[_cnt++] = BYTE1(_temp); data_to_send[_cnt++] = BYTE0(_temp);
_temp = (vs16)(PID_PID_8.D * PID_PID_8_D_MULTIPLYING ); data_to_send[_cnt++] = BYTE1(_temp); data_to_send[_cnt++] = BYTE0(_temp);
_temp = (vs16)(PID_PID_9.P * PID_PID_9_P_MULTIPLYING ); data_to_send[_cnt++] = BYTE1(_temp); data_to_send[_cnt++] = BYTE0(_temp);
_temp = (vs16)(PID_PID_9.I * PID_PID_9_I_MULTIPLYING ); data_to_send[_cnt++] = BYTE1(_temp); data_to_send[_cnt++] = BYTE0(_temp);
_temp = (vs16)(PID_PID_9.D * PID_PID_9_D_MULTIPLYING ); data_to_send[_cnt++] = BYTE1(_temp); data_to_send[_cnt++] = BYTE0(_temp);
_temp = (vs16)(PID_PID_10.P * PID_PID_10_P_MULTIPLYING); data_to_send[_cnt++] = BYTE1(_temp); data_to_send[_cnt++] = BYTE0(_temp);
_temp = (vs16)(PID_PID_10.I * PID_PID_10_I_MULTIPLYING); data_to_send[_cnt++] = BYTE1(_temp); data_to_send[_cnt++] = BYTE0(_temp);
_temp = (vs16)(PID_PID_10.D * PID_PID_10_D_MULTIPLYING); data_to_send[_cnt++] = BYTE1(_temp); data_to_send[_cnt++] = BYTE0(_temp);
data_to_send[3] = _cnt - 4;
u8 sum = 0;
for (u8 i = 0; i < _cnt; i++)
sum += data_to_send[i];
data_to_send[_cnt++] = sum;
#ifdef DATA_TRANSFER_USE_USART
Sys_sPrintf(Printf_USART, data_to_send, _cnt);
#else
NRF_TxPacket(data_to_send, _cnt);
#endif
}
void NRF_Send_RC(void)
{
NRF24L01_TXDATA_RC[0] = 0x8A; //源程序中为8A 遥控指令
NRF24L01_TXDATA_RC[1] = 0x8A; //源程序中为8A 遥控指令
NRF24L01_TXDATA_RC[2] = 0x1C;
NRF24L01_TXDATA_RC[3] = BYTE1(Rc_Get.THROTTLE);
NRF24L01_TXDATA_RC[4] = BYTE0(Rc_Get.THROTTLE);
NRF24L01_TXDATA_RC[5] = BYTE1(Rc_Get.YAW);
NRF24L01_TXDATA_RC[6] = BYTE0(Rc_Get.YAW);
NRF24L01_TXDATA_RC[7] = BYTE1(Rc_Get.ROLL);
NRF24L01_TXDATA_RC[8] = BYTE0(Rc_Get.ROLL);
NRF24L01_TXDATA_RC[9] = BYTE1(Rc_Get.PITCH);
NRF24L01_TXDATA_RC[10] = BYTE0(Rc_Get.PITCH);
NRF24L01_TXDATA_RC[11] = BYTE1(Rc_Get.AUX1);
NRF24L01_TXDATA_RC[12] = BYTE0(Rc_Get.AUX1);
NRF24L01_TXDATA_RC[13] = BYTE1(Rc_Get.AUX2);
NRF24L01_TXDATA_RC[14] = BYTE0(Rc_Get.AUX2);
NRF24L01_TXDATA_RC[15] = BYTE1(Rc_Get.AUX3);
NRF24L01_TXDATA_RC[16] = BYTE0(Rc_Get.AUX3);
NRF24L01_TXDATA_RC[17] = BYTE1(Rc_Get.AUX4);
NRF24L01_TXDATA_RC[18] = BYTE0(Rc_Get.AUX4);
NRF24L01_TXDATA_RC[19] = BYTE1(Rc_Get.AUX5);
NRF24L01_TXDATA_RC[20] = BYTE0(Rc_Get.AUX5);
u8 sum = 0;
for(u8 i=0;i<31;i++)
sum += NRF24L01_TXDATA_RC[i];
NRF24L01_TXDATA_RC[31] = sum;
NRF_SEND_RC_FLAG = 1;
}
void Communication::test(float a,float b,float c,float d,float e,float f,float g,float h,float i)
{
u8 _cnt=0;
vs16 _temp;
u8 data_to_send[30];
data_to_send[_cnt++]=0xAA;
data_to_send[_cnt++]=0xAA;
data_to_send[_cnt++]=0x02;
data_to_send[_cnt++]=0;
_temp = (int)(a*100);
data_to_send[_cnt++]=BYTE1(_temp);
data_to_send[_cnt++]=BYTE0(_temp);
_temp = (int)(b*100);
data_to_send[_cnt++]=BYTE1(_temp);
data_to_send[_cnt++]=BYTE0(_temp);
_temp = (int)(c*100);
data_to_send[_cnt++]=BYTE1(_temp);
data_to_send[_cnt++]=BYTE0(_temp);
_temp = (int)(d*100);
data_to_send[_cnt++]=BYTE1(_temp);
data_to_send[_cnt++]=BYTE0(_temp);
_temp = (int)(e*100);
data_to_send[_cnt++]=BYTE1(_temp);
data_to_send[_cnt++]=BYTE0(_temp);
_temp = (int)(f*100);
data_to_send[_cnt++]=BYTE1(_temp);
data_to_send[_cnt++]=BYTE0(_temp);
_temp = (int)(g*100);
data_to_send[_cnt++]=BYTE1(_temp);
data_to_send[_cnt++]=BYTE0(_temp);
_temp = (int)(h*100);
data_to_send[_cnt++]=BYTE1(_temp);
data_to_send[_cnt++]=BYTE0(_temp);
_temp = (int)(i*100);
data_to_send[_cnt++]=BYTE1(_temp);
data_to_send[_cnt++]=BYTE0(_temp);
data_to_send[3] = _cnt-4;
u8 sum = 0;
for(u8 i=0;i<_cnt;i++)
sum += data_to_send[i];
data_to_send[_cnt++] = sum;
usart.SendData(data_to_send, _cnt);
}
static void PM_SetReloadingState() {
pmove_t *xm = *(pmove_t**)(int)pm;
int clientNum = *(int*)((int)xm->ps + 172);
xclient_t *xcl = &xclients[clientNum];
int *weaponstate = (int*)((int)xm->ps + 180);
int *weapons = *(int**)((int)xm->ps + 796);
int weapon = *(int*)((int)xm->ps + 176);
int *weaponTime = (int*)((int)xm->ps + 44);
int *weaponDelay = (int*)((int)xm->ps + 48);
int weaponinfo = BG_GetInfoForWeapon(weapon);
int v2 = *(int*)((int)pml + 132);
int event = EV_RELOAD_FROM_EMPTY, v3;
if (*(int *)(4 * *(int *)(weaponinfo + 424) + (int)xm->ps + 524) || *(int *)(v2 + 112)) {
if ( xm->ps->pm_type <= 5 ) {
if (xm->cmd.wbuttons ) {
v3 = *(int *)((int)xm->ps + 980) & 0x200;
BYTE1(v3) ^= 2u;
LOBYTE(v3) = 11;
*(int *)((int)xm->ps + 980) = v3;
}
}
if(!xcl->perks[PERK_QUICK_RELOAD])
*weaponTime = *(int *)(v2 + 488);
else
*weaponTime = (int)(*(int *)(v2 + 488) / QUICK_RELOAD_FRACTION);
event = EV_RELOAD;
} else {
if ( xm->ps->pm_type <= 5 ) {
if (xm->cmd.wbuttons ) {
v3 = *(int *)((int)xm->ps + 980) & 0x200;
BYTE1(v3) ^= 2u;
LOBYTE(v3) = 12;
*(int *)((int)xm->ps + 980) = v3;
}
}
if(!xcl->perks[PERK_QUICK_RELOAD])
*weaponTime = *(int *)(v2 + 492);
else
*weaponTime = (int)(*(int *)(v2 + 492) / QUICK_RELOAD_FRACTION);
}
PM_AddEvent(event);
if ( *weaponstate == 8 )
*weaponstate = 6; //WEAPON_RELOADING_INTERRUPT??
else
*weaponstate = 5; //WEAPON_RELOADING
//if(!reloadtime->integer)
PM_SetWeaponReloadAddAmmoDelay();
if(xcl->perks[PERK_QUICK_RELOAD])
*weaponDelay = (int)(*weaponDelay / QUICK_RELOAD_FRACTION);
}
bool Communication::SendSensorOriginalData(Vector3<int> acc, Vector3<int> gyro,Vector3<int> mag)
{
u8 _cnt=0;
vs16 _temp;
u8 data_to_send[30];
data_to_send[_cnt++]=0xAA;
data_to_send[_cnt++]=0xAA;
data_to_send[_cnt++]=0x02;
data_to_send[_cnt++]=0;
_temp = acc.x;
data_to_send[_cnt++]=BYTE1(_temp);
data_to_send[_cnt++]=BYTE0(_temp);
_temp = acc.y;
data_to_send[_cnt++]=BYTE1(_temp);
data_to_send[_cnt++]=BYTE0(_temp);
_temp = acc.z;
data_to_send[_cnt++]=BYTE1(_temp);
data_to_send[_cnt++]=BYTE0(_temp);
_temp = gyro.x;
data_to_send[_cnt++]=BYTE1(_temp);
data_to_send[_cnt++]=BYTE0(_temp);
_temp = gyro.y;
data_to_send[_cnt++]=BYTE1(_temp);
data_to_send[_cnt++]=BYTE0(_temp);
_temp = gyro.z;
data_to_send[_cnt++]=BYTE1(_temp);
data_to_send[_cnt++]=BYTE0(_temp);
_temp = mag.x;
data_to_send[_cnt++]=BYTE1(_temp);
data_to_send[_cnt++]=BYTE0(_temp);
_temp = mag.y;
data_to_send[_cnt++]=BYTE1(_temp);
data_to_send[_cnt++]=BYTE0(_temp);
_temp = mag.z;
data_to_send[_cnt++]=BYTE1(_temp);
data_to_send[_cnt++]=BYTE0(_temp);
data_to_send[3] = _cnt-4;
u8 sum = 0;
for(u8 i=0;i<_cnt;i++)
sum += data_to_send[i];
data_to_send[_cnt++] = sum;
usart.SendData(data_to_send, _cnt);
return true;
}
bool Communication::SendPID(u8 PIDnumber,float p1_p,float p1_i,float p1_d,float p2_p,float p2_i,float p2_d,float p3_p,float p3_i,float p3_d)
{
u8 data_to_send[30]={0};
//u8 _cnt=0;
vs16 _temp=0;
data_to_send[0]=0xAA;
data_to_send[1]=0xAA;
data_to_send[2]=PIDnumber;
data_to_send[3]=18;
_temp = (int)(p1_p*1000);
data_to_send[4]=BYTE1(_temp);
data_to_send[5]=BYTE0(_temp);
_temp = (int)(p1_i*1000);
data_to_send[6]=BYTE1(_temp);
data_to_send[7]=BYTE0(_temp);
_temp = (int)(p1_d*1000);
data_to_send[8]=BYTE1(_temp);
data_to_send[9]=BYTE0(_temp);
_temp = (int)(p2_p*1000);
data_to_send[10]=BYTE1(_temp);
data_to_send[11]=BYTE0(_temp);
_temp = (int)(p2_i*1000);
data_to_send[12]=BYTE1(_temp);
data_to_send[13]=BYTE0(_temp);
_temp = (int)(p2_d*1000);
data_to_send[14]=BYTE1(_temp);
data_to_send[15]=BYTE0(_temp);
_temp = (int)(p3_p*1000);
data_to_send[16]=BYTE1(_temp);
data_to_send[17]=BYTE0(_temp);
_temp = (int)(p3_i*1000);
data_to_send[18]=BYTE1(_temp);
data_to_send[19]=BYTE0(_temp);
_temp = (int)(p3_d*1000);
data_to_send[20]=BYTE1(_temp);
data_to_send[21]=BYTE0(_temp);
//data_to_send[3] = _cnt-4;
u8 sum = 0;
for(u8 i=0;i<=21;i++)
sum += data_to_send[i];
data_to_send[22]=sum;
usart.SendData(data_to_send, 23);
return true;
}
// Function to play sound from the EEPROM
void play_sound(short rate, unsigned long offset, unsigned long length) {
unsigned int wave_data;
// Wake up EEPROM - with power-up delay
PORTC.F0 = 1;
PORTC.F0 = 0;
spi_write(0xAB);
PORTC.F0 = 1;
delay_us(100);
// Setup the EEPROM
PORTC.F0 = 0;
PORTC.F2 = 1; // Unhold EEPROM
spi_write(0x03); // EEPROM read command
spi_write(BYTE2(offset));
spi_write(BYTE1(offset));
spi_write(BYTE0(offset));
// Process all bytes in sound file
for (wave_scan = 0x000000; wave_scan < length; wave_scan++) {
// Retrieve a byte of audio data
PORTC.F2 = 1; // Unhold EEPROM
wave_data = (SPI_Read(0x00) << 4); // Read EEPROM byte
PORTC.F2 = 0; // Hold EEPROM
// Write audio data to DAC
PORTC.F1 = 0;
spi_write(BYTE1(wave_data) | 0x10);
spi_write(BYTE0(wave_data));
PORTC.F1 = 1;
// Set delays for different sampling rates
switch (rate) {
case FREQ_8000: delay_us(0x65); break;
case FREQ_11025: delay_us(0x41); break;
case FREQ_22050: delay_us(0x12); break;
}
}
// Set DAC voltage output to normalized level
PORTC.F1 = 0;
spi_write(0x18);
spi_write(0x00);
PORTC.F1 = 1;
// Shutdown EEPROM - with power-down delay
PORTC.F0 = 1;
PORTC.F0 = 0;
spi_write(0xB9);
PORTC.F0 = 1;
delay_us(100);
}
void ANO_DT::Send_RCData(void)
{
u8 _cnt=0;
data_to_send[_cnt++]=0xAA;
data_to_send[_cnt++]=0xAA;
data_to_send[_cnt++]=0x03;
data_to_send[_cnt++]=0;
data_to_send[_cnt++]=BYTE1(rc.rawData[THROTTLE]);
data_to_send[_cnt++]=BYTE0(rc.rawData[THROTTLE]);
data_to_send[_cnt++]=BYTE1(rc.rawData[YAW]);
data_to_send[_cnt++]=BYTE0(rc.rawData[YAW]);
data_to_send[_cnt++]=BYTE1(rc.rawData[ROLL]);
data_to_send[_cnt++]=BYTE0(rc.rawData[ROLL]);
data_to_send[_cnt++]=BYTE1(rc.rawData[PITCH]);
data_to_send[_cnt++]=BYTE0(rc.rawData[PITCH]);
data_to_send[_cnt++]=BYTE1(rc.rawData[AUX1]);
data_to_send[_cnt++]=BYTE0(rc.rawData[AUX1]);
data_to_send[_cnt++]=BYTE1(rc.rawData[AUX2]);
data_to_send[_cnt++]=BYTE0(rc.rawData[AUX2]);
data_to_send[_cnt++]=BYTE1(rc.rawData[AUX3]);
data_to_send[_cnt++]=BYTE0(rc.rawData[AUX3]);
data_to_send[_cnt++]=BYTE1(rc.rawData[AUX4]);
data_to_send[_cnt++]=BYTE0(rc.rawData[AUX4]);
data_to_send[3] = _cnt-4;
u8 sum = 0;
for(u8 i=0;i<_cnt;i++)
sum += data_to_send[i];
data_to_send[_cnt++]=sum;
Send_Data(data_to_send, _cnt);
}
void Data_Send_MotoPWM(void)
{
u8 _cnt = 0;
data_to_send[_cnt++] = 0xAA;
data_to_send[_cnt++] = 0xAA;
data_to_send[_cnt++] = 0x06;
data_to_send[_cnt++] = 0;
data_to_send[_cnt++] = BYTE1(MOTO1_PWM);
data_to_send[_cnt++] = BYTE0(MOTO1_PWM);
data_to_send[_cnt++] = BYTE1(MOTO2_PWM);
data_to_send[_cnt++] = BYTE0(MOTO2_PWM);
data_to_send[_cnt++] = BYTE1(MOTO3_PWM);
data_to_send[_cnt++] = BYTE0(MOTO3_PWM);
data_to_send[_cnt++] = BYTE1(MOTO4_PWM);
data_to_send[_cnt++] = BYTE0(MOTO4_PWM);
data_to_send[_cnt++] = BYTE1(MOTO1_PWM);
data_to_send[_cnt++] = BYTE0(MOTO1_PWM);
data_to_send[_cnt++] = BYTE1(MOTO1_PWM);
data_to_send[_cnt++] = BYTE0(MOTO1_PWM);
data_to_send[_cnt++] = BYTE1(MOTO1_PWM);
data_to_send[_cnt++] = BYTE0(MOTO1_PWM);
data_to_send[_cnt++] = BYTE1(MOTO1_PWM);
data_to_send[_cnt++] = BYTE0(MOTO1_PWM);
data_to_send[3] = _cnt - 4;
u8 sum = 0;
for (u8 i = 0; i < _cnt; i++)
sum += data_to_send[i];
data_to_send[_cnt++] = sum;
#ifdef DATA_TRANSFER_USE_USART
Sys_sPrintf(Printf_USART, data_to_send, _cnt);
#else
NRF_TxPacket(data_to_send, _cnt);
#endif
}
static int put_multibyte(long c, FILE *fp) {
#ifdef WIN32
if (sjisterminal) {
const int fd = fileno(fp);
if ((fd == fileno(stdout) || fd == fileno(stderr)) && _isatty(fd)) {
HANDLE hStdout;
DWORD ret, wclen;
UINT cp;
wchar_t buff[2];
char str[4];
int mblen;
if (fd == fileno(stdout))
hStdout = GetStdHandle(STD_OUTPUT_HANDLE);
else
hStdout = GetStdHandle(STD_ERROR_HANDLE);
mblen=0;
if (BYTE1(c) != 0) str[mblen++]=BYTE1(c);
if (BYTE2(c) != 0) str[mblen++]=BYTE2(c);
if (BYTE3(c) != 0) str[mblen++]=BYTE3(c);
/* always */ str[mblen++]=BYTE4(c);
#define CP_932 932
#define CP_UTF8 65001
if (is_internalUPTEX())
cp = CP_UTF8;
else
cp = CP_932;
if (MultiByteToWideChar(cp, 0, str, mblen, buff, 2) == 0)
return EOF;
wclen = mblen > 3 ? 2 : 1;
if (WriteConsoleW(hStdout, buff, wclen, &ret, NULL) == 0)
return EOF;
return BYTE4(c);
}
}
#endif
if (BYTE1(c) != 0 && putc(BYTE1(c), fp) == EOF) return EOF;
if (BYTE2(c) != 0 && putc(BYTE2(c), fp) == EOF) return EOF;
if (BYTE3(c) != 0 && putc(BYTE3(c), fp) == EOF) return EOF;
/* always */ return putc(BYTE4(c), fp);
}
void Data_Send_Status(void)
{
uint8_t _cnt=0;
data_to_send[_cnt++]=0xAA;
data_to_send[_cnt++]=0xAA;
data_to_send[_cnt++]=0x01;
data_to_send[_cnt++]=0;
__IO int16_t _temp;
_temp = (int)(Roll*100);
data_to_send[_cnt++]=BYTE1(_temp);
data_to_send[_cnt++]=BYTE0(_temp);
_temp = (int)(Pitch*100);
data_to_send[_cnt++]=BYTE1(_temp);
data_to_send[_cnt++]=BYTE0(_temp);
_temp = (int)((TIM8->CNT/180)*100);
data_to_send[_cnt++]=BYTE1(_temp);
data_to_send[_cnt++]=BYTE0(_temp);
_temp = (int)(TIM5->CNT*100);
data_to_send[_cnt++]=BYTE1(_temp);
data_to_send[_cnt++]=BYTE0(_temp);
// _temp = (int)(TIM8->CNT)*100;
// data_to_send[_cnt++]=BYTE1(_temp);
// data_to_send[_cnt++]=BYTE0(_temp);
// __IO int32_t _temp2 = (int)(Pressure*100);
// data_to_send[_cnt++]=BYTE3(_temp2);
// data_to_send[_cnt++]=BYTE2(_temp2);
// data_to_send[_cnt++]=BYTE1(_temp2);
// data_to_send[_cnt++]=BYTE0(_temp2);
//
// if(ARMED==0) data_to_send[_cnt++]=0xA0; //锁定
// else if(ARMED==1) data_to_send[_cnt++]=0xA1;
//
data_to_send[_cnt++]=0xA1;
data_to_send[3] = _cnt-4;
uint8_t sum = 0;
for(uint8_t i=0;i<_cnt;i++)
sum += data_to_send[i];
data_to_send[_cnt++]=sum;
//#ifdef DATA_TRANSFER_USE_USART
// Uart1_Put_Buf(data_to_send,_cnt);
HAL_UART_Transmit_DMA(&huart2, data_to_send, _cnt);
//#else
// NRF_TxPacket(data_to_send,_cnt);
//#endif
}
