void Data_Send_PID6(void)
{
u8 _cnt = 0;
data_to_send[_cnt++] = 0xAA;
data_to_send[_cnt++] = 0xAA;
data_to_send[_cnt++] = 0x15;
data_to_send[_cnt++] = 0;
vs16 _temp;
_temp = (vs16)(PID_PID_11.P * PID_PID_11_P_MULTIPLYING); data_to_send[_cnt++] = BYTE1(_temp); data_to_send[_cnt++] = BYTE0(_temp);
_temp = (vs16)(PID_PID_11.I * PID_PID_11_I_MULTIPLYING); data_to_send[_cnt++] = BYTE1(_temp); data_to_send[_cnt++] = BYTE0(_temp);
_temp = (vs16)(PID_PID_11.D * PID_PID_11_D_MULTIPLYING); data_to_send[_cnt++] = BYTE1(_temp); data_to_send[_cnt++] = BYTE0(_temp);
_temp = (vs16)(PID_PID_12.P * PID_PID_12_P_MULTIPLYING); data_to_send[_cnt++] = BYTE1(_temp); data_to_send[_cnt++] = BYTE0(_temp);
_temp = (vs16)(PID_PID_12.I * PID_PID_12_I_MULTIPLYING); data_to_send[_cnt++] = BYTE1(_temp); data_to_send[_cnt++] = BYTE0(_temp);
_temp = (vs16)(PID_PID_12.D * PID_PID_12_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_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)
{
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
}
void Data_Send_F1(void)
{
u8 _cnt = 0;
data_to_send[_cnt++] = 0xAA;
data_to_send[_cnt++] = 0xAA;
data_to_send[_cnt++] = 0xF1;
data_to_send[_cnt++] = 0;
// __IO float _tempf;
// _tempf = position_x;data_to_send[_cnt++] = BYTE3(_tempf);data_to_send[_cnt++] = BYTE2(_tempf);data_to_send[_cnt++] = BYTE1(_tempf);data_to_send[_cnt++] = BYTE0(_tempf);
// _tempf = position_y;data_to_send[_cnt++] = BYTE3(_tempf);data_to_send[_cnt++] = BYTE2(_tempf);data_to_send[_cnt++] = BYTE1(_tempf);data_to_send[_cnt++] = BYTE0(_tempf);
// _tempf = position_z;data_to_send[_cnt++] = BYTE3(_tempf);data_to_send[_cnt++] = BYTE2(_tempf);data_to_send[_cnt++] = BYTE1(_tempf);data_to_send[_cnt++] = BYTE0(_tempf);
vs16 _temp16;
_temp16 = (vs16)(Acc.x * PID_PID_8_P_MULTIPLYING ); data_to_send[_cnt++] = BYTE1(_temp16); data_to_send[_cnt++] = BYTE0(_temp16);
_temp16 = (vs16)(Average_Acc.x * PID_PID_8_I_MULTIPLYING ); data_to_send[_cnt++] = BYTE1(_temp16); data_to_send[_cnt++] = BYTE0(_temp16);
_temp16 = (vs16)(Average2_Acc.x * PID_PID_8_P_MULTIPLYING ); data_to_send[_cnt++] = BYTE1(_temp16); data_to_send[_cnt++] = BYTE0(_temp16);
// _temp16 = (vs16)(Average3_Acc.x * PID_PID_8_I_MULTIPLYING ); data_to_send[_cnt++] = BYTE1(_temp16); data_to_send[_cnt++] = BYTE0(_temp16);
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
void Data_Send_F2(void)
{
u8 _cnt = 0;
data_to_send[_cnt++] = 0xAA;
data_to_send[_cnt++] = 0xAA;
data_to_send[_cnt++] = 0xF2;
data_to_send[_cnt++] = 0;
vs16 _temp16;
vs32 _temp32;
//_temp16 = (vs16)(PID_PIT.OUT ); data_to_send[_cnt++] = BYTE1(_temp16); data_to_send[_cnt++] = BYTE0(_temp16);
_temp32 = (vs32)(Alt_bmp); data_to_send[_cnt++] = BYTE3(_temp32); data_to_send[_cnt++] = BYTE2(_temp32); data_to_send[_cnt++] = BYTE1(_temp32); data_to_send[_cnt++] = BYTE0(_temp32);
_temp32 = (vs32)(Alt_bmp1); data_to_send[_cnt++] = BYTE3(_temp32); data_to_send[_cnt++] = BYTE2(_temp32); data_to_send[_cnt++] = BYTE1(_temp32); data_to_send[_cnt++] = BYTE0(_temp32);
_temp32 = (vs32)(Alt_bmp2); data_to_send[_cnt++] = BYTE3(_temp32); data_to_send[_cnt++] = BYTE2(_temp32); data_to_send[_cnt++] = BYTE1(_temp32); data_to_send[_cnt++] = BYTE0(_temp32);
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_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_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
}
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
}
void NRF_FTLR( void ) // First Tx Last Rx
{
u8 i = 0;
u8 Sta = ERROR;
static u8 FSM_STA = 0;
switch(FSM_STA) {
/************************** FSM Tx **************************************/
case 0:
// FSM_Tx
// 資料寫入 TX BUF
for(i=0; i<32; i++) {
TxBuf[i] = RxBuf[i] + i;
if(TxBuf[i]>220)
TxBuf[i] = 0;
}
do {
Sta = NRF_TxPacket(TxBuf);
} while(Sta == NRF_STA_MAX_RT);
// FSM_Tx End
FSM_STA = 1;
break;
/************************** FSM Rx **************************************/
case 1:
// FSM_Rx
NRF_RX_Mode();
Sta = NRF_RxPacket(RxBuf);
if(Sta == NRF_STA_RX_DR) {
LED_B = !LED_B;
}
// FSM_Rx End
FSM_STA = 2;
break;
/************************** FSM USART **************************************/
case 2:
// FSM_USART
Delay_10ms(20);
RS232_SendStr((u8*)"\f");
RS232_SendStr((u8*)"***NRF_MODE_FTLR\r\n");
for(i=0; i<32; i++) {
RS232_SendStr((u8*)"RxBuf[");
RS232_SendNum(Type_D, 2, i);
RS232_SendStr((u8*)"] = ");
RS232_SendNum(Type_D, 3, TxBuf[i]);
RS232_SendStr((u8*)"\r\n");
}
RS232_SendStr((u8*)"\r\n");
// FSM_USART End
FSM_STA = 0;
break;
}
}
u8 NRF_TxAll(void* pBuf,u8 len) //成功返回1
{
u8 *ptemp = (u8*)pBuf;
u8 t = len/32;
while(t--)
{
if(TX_OK != NRF_TxPacket(ptemp))return 0;
ptemp +=32;
delay_ms(1);
}
if(len%32)
{
if(TX_OK != NRF_TxPacket(ptemp))return 0;
}
return 1;
}
void Data_Send_PID3(int flag)
{
u8 _cnt = 0;
data_to_send[_cnt++] = 0xAA;
if(flag==1)//发送
data_to_send[_cnt++] = 0xAF;
else if(flag==0)//返回 验证
data_to_send[_cnt++] = 0xAA;
data_to_send[_cnt++] = 0x12;
data_to_send[_cnt++] = 0;
vs16 _temp;
_temp = PID_PID_2.P * 100;
data_to_send[_cnt++] = BYTE1(_temp);
data_to_send[_cnt++] = BYTE0(_temp);
_temp = PID_PID_2.I * 100;
data_to_send[_cnt++] = BYTE1(_temp);
data_to_send[_cnt++] = BYTE0(_temp);
_temp = PID_PID_2.D * 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;
if(flag==1)
{
NRF_TxPacket(data_to_send, _cnt);
}
else
{
#ifdef DATA_TRANSFER_USE_USART
Sys_sPrintf(DATA_TRANSFER_USARTx, 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 = PID_PID_8.P * 100;
data_to_send[_cnt++]=BYTE1(_temp);
data_to_send[_cnt++]=BYTE0(_temp);
_temp = PID_PID_8.I * 100;
data_to_send[_cnt++]=BYTE1(_temp);
data_to_send[_cnt++]=BYTE0(_temp);
_temp = PID_PID_8.D * 100;
data_to_send[_cnt++]=BYTE1(_temp);
data_to_send[_cnt++]=BYTE0(_temp);
_temp = PID_PID_9.P * 100;
data_to_send[_cnt++]=BYTE1(_temp);
data_to_send[_cnt++]=BYTE0(_temp);
_temp = PID_PID_9.I * 100;
data_to_send[_cnt++]=BYTE1(_temp);
data_to_send[_cnt++]=BYTE0(_temp);
_temp = PID_PID_9.D * 100;
data_to_send[_cnt++]=BYTE1(_temp);
data_to_send[_cnt++]=BYTE0(_temp);
_temp = PID_PID_10.P * 100;
data_to_send[_cnt++]=BYTE1(_temp);
data_to_send[_cnt++]=BYTE0(_temp);
_temp = PID_PID_10.I * 100;
data_to_send[_cnt++]=BYTE1(_temp);
data_to_send[_cnt++]=BYTE0(_temp);
_temp = PID_PID_10.D * 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;
#ifdef DATA_TRANSFER_USE_USART
Uart1_Put_Buf(data_to_send,_cnt);
#else
NRF_TxPacket(data_to_send,_cnt);
#endif
}
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[_cnt++]=BYTE1(Rc_D.AUX1);
data_to_send[_cnt++]=BYTE0(Rc_D.AUX1);
data_to_send[_cnt++]=BYTE1(Rc_D.AUX2);
data_to_send[_cnt++]=BYTE0(Rc_D.AUX2);
data_to_send[_cnt++]=BYTE1(Rc_D.AUX3);
data_to_send[_cnt++]=BYTE0(Rc_D.AUX3);
data_to_send[_cnt++]=BYTE1(Rc_D.AUX4);
data_to_send[_cnt++]=BYTE0(Rc_D.AUX4);
Rc_D.AUX5 = (u16)(ADC_ConvertedValue*2*33/4096);//ËÄÖáµçѹµÄÊ®±¶
data_to_send[_cnt++]=BYTE1(Rc_D.AUX5);
data_to_send[_cnt++]=BYTE0(Rc_D.AUX5);
data_to_send[_cnt++]=BYTE1(Rc_D.AUX6);
data_to_send[_cnt++]=BYTE0(Rc_D.AUX6);
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_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[_cnt++] = BYTE1(Rc_D.AUX1);
data_to_send[_cnt++] = BYTE0(Rc_D.AUX1);
data_to_send[_cnt++] = BYTE1(Rc_D.AUX2);
data_to_send[_cnt++] = BYTE0(Rc_D.AUX2);
data_to_send[_cnt++] = BYTE1(Rc_D.AUX3);
data_to_send[_cnt++] = BYTE0(Rc_D.AUX3);
data_to_send[_cnt++] = BYTE1(Rc_D.AUX4);
data_to_send[_cnt++] = BYTE0(Rc_D.AUX4);
data_to_send[_cnt++] = BYTE1(Rc_D.AUX5);
data_to_send[_cnt++] = BYTE0(Rc_D.AUX5);
data_to_send[_cnt++] = BYTE1(Rc_D.AUX6);
data_to_send[_cnt++] = BYTE0(Rc_D.AUX6);
data_to_send[_cnt++] = BYTE3(Alt_ultrasonic);
data_to_send[_cnt++] = BYTE2(Alt_ultrasonic);
data_to_send[_cnt++] = BYTE1(Alt_ultrasonic);
data_to_send[_cnt++] = BYTE0(Alt_ultrasonic);
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_Check(u16 check)
{
data_to_send[0] = 0xAA;
data_to_send[1] = 0xAA;
data_to_send[2] = 0xF0;
data_to_send[3] = 3;
data_to_send[4] = 0xBA;
data_to_send[5] = BYTE1(check);
data_to_send[6] = BYTE0(check);
u8 sum = 0;
for (u8 i = 0; i < 7; i++)
sum += data_to_send[i];
data_to_send[7] = sum;
#ifdef DATA_TRANSFER_USE_USART
Sys_sPrintf(Printf_USART, data_to_send, 8);
#else
NRF_TxPacket(data_to_send, 8);
#endif
}
void Data_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)(Att_Angle.rol * 100);
data_to_send[_cnt++] = BYTE1(_temp);
data_to_send[_cnt++] = BYTE0(_temp);
_temp = (int)(Att_Angle.pit * 100);
data_to_send[_cnt++] = BYTE1(_temp);
data_to_send[_cnt++] = BYTE0(_temp);
_temp = (int)(Att_Angle.yaw * 100);
data_to_send[_cnt++] = BYTE1(_temp);
data_to_send[_cnt++] = BYTE0(_temp);
_temp = Alt_ultrasonic;
data_to_send[_cnt++] = BYTE1(_temp);
data_to_send[_cnt++] = BYTE0(_temp);
vs32 _temp2 = Alt_bmp;
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 (0 == RC_Control.ARMED)
data_to_send[_cnt++] = 0xA0;
else if (1 == RC_Control.ARMED)
data_to_send[_cnt++] = 0xA1;
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_Senser(void)
{
u8 _cnt=0;
data_to_send[_cnt++]=0xAA;
data_to_send[_cnt++]=0xAA;
data_to_send[_cnt++]=0x02;
data_to_send[_cnt++]=0;
data_to_send[_cnt++]=BYTE1(Acc.X);
data_to_send[_cnt++]=BYTE0(Acc.X);
data_to_send[_cnt++]=BYTE1(Acc.Y);
data_to_send[_cnt++]=BYTE0(Acc.Y);
data_to_send[_cnt++]=BYTE1(Acc.Z);
data_to_send[_cnt++]=BYTE0(Acc.Z);
data_to_send[_cnt++]=BYTE1(Gyr.X);
data_to_send[_cnt++]=BYTE0(Gyr.X);
data_to_send[_cnt++]=BYTE1(Gyr.Y);
data_to_send[_cnt++]=BYTE0(Gyr.Y);
data_to_send[_cnt++]=BYTE1(Gyr.Z);
data_to_send[_cnt++]=BYTE0(Gyr.Z);
data_to_send[_cnt++]=0;
data_to_send[_cnt++]=0;
data_to_send[_cnt++]=0;
data_to_send[_cnt++]=0;
data_to_send[_cnt++]=0;
data_to_send[_cnt++]=0;
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_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)(Att_Angle.rol*100);
data_to_send[_cnt++]=BYTE1(_temp);
data_to_send[_cnt++]=BYTE0(_temp);
_temp = (int)(Att_Angle.pit*100);
data_to_send[_cnt++]=BYTE1(_temp);
data_to_send[_cnt++]=BYTE0(_temp);
_temp = (int)(Att_Angle.yaw*100);
//_temp = (int)(Mag_Heading*100);
data_to_send[_cnt++]=BYTE1(_temp);
data_to_send[_cnt++]=BYTE0(_temp);
vs32 _temp2 = Alt;
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(Rc_C.ARMED==0) data_to_send[_cnt++]=0xA0; //Ëø¶¨
else if(Rc_C.ARMED==1) data_to_send[_cnt++]=0xA1;
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
}
//16
void Data_Send_F3(void)
{
extern u32 Throttle_OUT;
// T_float_angle angle;
extern s16 Alt_Error;
extern u16 Alt_ultrasonic1t;
extern u16 Alt_ultrasonic2t;
extern u32 Throttle_OUT;
extern u32 Throttle_IN;
//extern u32 Throttle_OUT;
u8 _cnt = 0;
data_to_send[_cnt++] = 0xAA;
data_to_send[_cnt++] = 0xAA;
data_to_send[_cnt++] = 0xF3;
data_to_send[_cnt++] = 0;
vs16 _temp16;
_temp16 = (vs16)(Alt_ultrasonic1t); data_to_send[_cnt++] = BYTE1(_temp16); data_to_send[_cnt++] = BYTE0(_temp16);
_temp16 = (vs16)(Alt_ultrasonic2t ); data_to_send[_cnt++] = BYTE1(_temp16); data_to_send[_cnt++] = BYTE0(_temp16);
_temp16 = (vs16)(Throttle_OUT ); data_to_send[_cnt++] = BYTE1(_temp16); data_to_send[_cnt++] = BYTE0(_temp16);
_temp16 = (vs16)(Throttle_IN); data_to_send[_cnt++] = BYTE1(_temp16); data_to_send[_cnt++] = BYTE0(_temp16);
_temp16 = (vs16)(Alt_ultrasonic); data_to_send[_cnt++] = BYTE1(_temp16); data_to_send[_cnt++] = BYTE0(_temp16);
_temp16 = (vs16)(PID_ALT.OUT ); data_to_send[_cnt++] = BYTE1(_temp16); data_to_send[_cnt++] = BYTE0(_temp16);
_temp16 = (vs16)(Alt_Error ); data_to_send[_cnt++] = BYTE1(_temp16); data_to_send[_cnt++] = BYTE0(_temp16);
_temp16 = (vs16)(Balance_Throttle ); data_to_send[_cnt++] = BYTE1(_temp16); data_to_send[_cnt++] = BYTE0(_temp16);
//_temp16 = (vs16)(angle.yaw * PID_PID_8_P_MULTIPLYING ); data_to_send[_cnt++] = BYTE1(_temp16); data_to_send[_cnt++] = BYTE0(_temp16);
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_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(Moto_PWM_2);
data_to_send[_cnt++]=BYTE0(Moto_PWM_2);
data_to_send[_cnt++]=BYTE1(Moto_PWM_1);
data_to_send[_cnt++]=BYTE0(Moto_PWM_1);
data_to_send[_cnt++]=BYTE1(Moto_PWM_3);
data_to_send[_cnt++]=BYTE0(Moto_PWM_3);
data_to_send[_cnt++]=BYTE1(Moto_PWM_4);
data_to_send[_cnt++]=BYTE0(Moto_PWM_4);
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;
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_Senser(void)
{
u8 _cnt = 0;
data_to_send[_cnt++] = 0xAA;
data_to_send[_cnt++] = 0xAA;
data_to_send[_cnt++] = 0x02;
data_to_send[_cnt++] = 0;
vs16 _temp;
#define EX_ACC Acc
data_to_send[_cnt++] = BYTE1(EX_ACC.x);
data_to_send[_cnt++] = BYTE0(EX_ACC.x);
data_to_send[_cnt++] = BYTE1(EX_ACC.y);
data_to_send[_cnt++] = BYTE0(EX_ACC.y);
data_to_send[_cnt++] = BYTE1(EX_ACC.z);
data_to_send[_cnt++] = BYTE0(EX_ACC.z);
data_to_send[_cnt++] = BYTE1(Gyr.x);
data_to_send[_cnt++] = BYTE0(Gyr.x);
data_to_send[_cnt++] = BYTE1(Gyr.y);
data_to_send[_cnt++] = BYTE0(Gyr.y);
data_to_send[_cnt++] = BYTE1(Gyr.z);
data_to_send[_cnt++] = BYTE0(Gyr.z);
data_to_send[_cnt++] = BYTE1(Mag.x);
data_to_send[_cnt++] = BYTE0(Mag.x);
data_to_send[_cnt++] = BYTE1(Mag.y);
data_to_send[_cnt++] = BYTE0(Mag.y);
data_to_send[_cnt++] = BYTE1(Mag.z);
data_to_send[_cnt++] = BYTE0(Mag.z);
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 Inspaire()//这是主函数的while(1)
{
static uint32_t NRF_Counter = 0;
static uint32_t NRF_Check_Sum = 0;
float Power_Check = 0.0;
uint8_t i;
uint32_t RUN_Times = 0;
while(1)
{
//依次扫描遥控的各个摇杆的位置，然后用NRF发出去
//包头以及发送的次数0-99循环发送
NRF24L01_TXDATA[0] = 0XA5;
NRF_Counter ++;
NRF24L01_TXDATA[1] = NRF_Counter;
if(NRF_Counter >= 99)
NRF_Counter = 0;
//检测油门
adcInit1();
ADC_Result = ADC_Result/4;
NRF24L01_TXDATA[2] = (uint8_t)(ADC_Result&0x00ff);
NRF24L01_TXDATA[3] = (uint8_t)((ADC_Result>>8)&0x00ff);
//检测YAW
adcInit0();
ADC_Result = ADC_Result/4;
NRF24L01_TXDATA[4] = (uint8_t)(ADC_Result&0x00ff);
NRF24L01_TXDATA[5] = (uint8_t)((ADC_Result>>8)&0x00ff);
//检测Pitch
adcInit4();
ADC_Result = 4096 - ADC_Result;
ADC_Result = ADC_Result/4;
NRF24L01_TXDATA[6] = (uint8_t)(ADC_Result&0x00ff);
NRF24L01_TXDATA[7] = (uint8_t)((ADC_Result>>8)&0x00ff);
//检测Roll
adcInit6();
ADC_Result = ADC_Result/4;
NRF24L01_TXDATA[8] = (uint8_t)(ADC_Result&0x00ff);
NRF24L01_TXDATA[9] = (uint8_t)((ADC_Result>>8)&0x00ff);
//检测按键
if(KEY1_ON)
NRF24L01_TXDATA[10] = 1;
else if(KEY2_ON)
NRF24L01_TXDATA[10] = 2;
else
NRF24L01_TXDATA[10] = 0;
//检测参数前后初始静偏
adcInit3();
ADC_Result = ADC_Result/16;
NRF24L01_TXDATA[20] = (uint8_t)(ADC_Result&0x00ff);
//检测参数左右初始静偏
adcInit2();
ADC_Result = ADC_Result/16;
NRF24L01_TXDATA[21] = (uint8_t)(ADC_Result&0x00ff);
//检测电量
adcInit5();
Power_Check = ADC_Result*3.3*2/4096.0;
if(Power_Check < 3.7)
{
//灯闪，提示电量不足
if(RUN_Times%12 == 0)
{
LEDALL_OFF;
}
else if(RUN_Times%22 == 0)
{
LEDALL_ON;
}
RUN_Times ++;
if(RUN_Times > 1000000)
{
RUN_Times = 0;
}
}
else
{
LEDALL_ON;
}
//进行和校验处理
NRF_Check_Sum = 0;
for(i=0;i<30;i++)
{
NRF_Check_Sum = NRF_Check_Sum + NRF24L01_TXDATA[i];
}
NRF24L01_TXDATA[30] = (uint8_t)(NRF_Check_Sum&0x00ff);
NRF24L01_TXDATA[31] = (uint8_t)((NRF_Check_Sum>>8)&0x00ff);
delay_Ms_Loop(5);
//发送一个包，每秒发50-100个数据包
//delay_Ms_Loop(10);
while(GPIO_ReadInputDataBit(GPIOB,GPIO_Pin_6));
NRF_IRQ();
NRF_TxPacket(NRF24L01_TXDATA,32);
}
}
