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_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);
}
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
}
/* -------------发送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_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 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
}
//■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);
}
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);
}
/*
* 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));
}
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;
}
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);
}
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;
}
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;
}
// 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
}
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
}
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;
data_to_send[8]=PID_PIT.P;
data_to_send[9]=PID_PIT.I;
data_to_send[10]=PID_PIT.D;
data_to_send[11]=PID_ROL.P;
data_to_send[12]=PID_ROL.I;
data_to_send[13]=PID_ROL.D;
data_to_send[14]=PID_YAW.P;
data_to_send[15]=PID_YAW.I;
data_to_send[16]=PID_YAW.D;
// NRF_TxPacket(data_to_send,8);
Uart2_Put_Buf(data_to_send,17);
}
void uart_putshort(uint16_t DataToSend)
{
Tx_Buf[0] = BYTE0(DataToSend);
uart_putchar(Tx_Buf[0]);
Tx_Buf[1] = BYTE1(DataToSend);
uart_putchar(Tx_Buf[1]);
}
void Data_Send_PID(void)
{
uint8_t _cnt=0;
data_to_send[_cnt++]=0xAA;
data_to_send[_cnt++]=0xAA;
data_to_send[_cnt++]=0xB0;
data_to_send[_cnt++]=0;
__IO int16_t _temp;
_temp = PID_Pitch_Struct.Kp * 100;
data_to_send[_cnt++]=BYTE1(_temp);
data_to_send[_cnt++]=BYTE0(_temp);
_temp = PID_Pitch_Struct.Ti * 100;
data_to_send[_cnt++]=BYTE1(_temp);
data_to_send[_cnt++]=BYTE0(_temp);
_temp = PID_Pitch_Struct.Td * 100;
data_to_send[_cnt++]=BYTE1(_temp);
data_to_send[_cnt++]=BYTE0(_temp);
_temp = PID_Roll_Struct.Kp * 100;
data_to_send[_cnt++]=BYTE1(_temp);
data_to_send[_cnt++]=BYTE0(_temp);
_temp = PID_Roll_Struct.Ti * 100;
data_to_send[_cnt++]=BYTE1(_temp);
data_to_send[_cnt++]=BYTE0(_temp);
_temp = PID_Roll_Struct.Td * 100;
data_to_send[_cnt++]=BYTE1(_temp);
data_to_send[_cnt++]=BYTE0(_temp);
_temp = PID_Z_Struct.Kp * 100;
data_to_send[_cnt++]=BYTE1(_temp);
data_to_send[_cnt++]=BYTE0(_temp);
_temp = PID_Z_Struct.Ti * 100;
data_to_send[_cnt++]=BYTE1(_temp);
data_to_send[_cnt++]=BYTE0(_temp);
_temp = PID_Z_Struct.Td * 100;
data_to_send[_cnt++]=BYTE1(_temp);
data_to_send[_cnt++]=BYTE0(_temp);
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
}
/*
* 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)
{
int i;
u8 *dest = (u8 *) to;
u16 *src = (u16 *) (map->map_priv_1 + from);
u16 data;
for (i = 0; i < (len / 2); i++) {
data = src[i];
dest[i * 2] = BYTE0(data);
dest[i * 2 + 1] = BYTE1(data);
}
if (len & 1)
dest[len - 1] = BYTE0(src[i]);
}
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
bufPutSInt(Buffer b, ULong i)
{
bufAdd1(b, BYTE0(i));
bufAdd1(b, BYTE1(i));
bufAdd1(b, BYTE2(i));
bufAdd1(b, BYTE3(i));
}
void Send_PID2(void)
{
u8 _cnt=0;
u8 i;
vs16 _temp;
u8 sum = 0;
data_to_send[_cnt++]=0xAA;
data_to_send[_cnt++]=0xAA;
data_to_send[_cnt++]=0x11;
data_to_send[_cnt++]=0;
_temp = fc.pid[PIDALT].kP;
data_to_send[_cnt++]=BYTE1(_temp);
data_to_send[_cnt++]=BYTE0(_temp);
_temp = fc.pid[PIDALT].kI;
data_to_send[_cnt++]=BYTE1(_temp);
data_to_send[_cnt++]=BYTE0(_temp);
_temp = fc.pid[PIDALT].kD;
data_to_send[_cnt++]=BYTE1(_temp);
data_to_send[_cnt++]=BYTE0(_temp);
_temp = fc.pid[PIDLEVEL].kP;
data_to_send[_cnt++]=BYTE1(_temp);
data_to_send[_cnt++]=BYTE0(_temp);
_temp = fc.pid[PIDLEVEL].kI;
data_to_send[_cnt++]=BYTE1(_temp);
data_to_send[_cnt++]=BYTE0(_temp);
_temp = fc.pid[PIDLEVEL].kD;
data_to_send[_cnt++]=BYTE1(_temp);
data_to_send[_cnt++]=BYTE0(_temp);
_temp = fc.pid[PIDMAG].kP;
data_to_send[_cnt++]=BYTE1(_temp);
data_to_send[_cnt++]=BYTE0(_temp);
_temp = fc.pid[PIDMAG].kI;
data_to_send[_cnt++]=BYTE1(_temp);
data_to_send[_cnt++]=BYTE0(_temp);
_temp = fc.pid[PIDMAG].kD;
data_to_send[_cnt++]=BYTE1(_temp);
data_to_send[_cnt++]=BYTE0(_temp);
data_to_send[3] = _cnt-4;
for(i=0;i<_cnt;i++)
sum += data_to_send[i];
data_to_send[_cnt++]=sum;
Send_Data(data_to_send, _cnt);
}
void Send_PID1(void)
{
u8 _cnt=0;
u8 i;
vs16 _temp;
u8 sum = 0;
data_to_send[_cnt++]=0xAA;
data_to_send[_cnt++]=0xAA;
data_to_send[_cnt++]=0x10;
data_to_send[_cnt++]=0;
_temp = fc.pid[PIDROLL].kP ;
data_to_send[_cnt++]=BYTE1(_temp);
data_to_send[_cnt++]=BYTE0(_temp);
_temp = fc.pid[PIDROLL].kI ;
data_to_send[_cnt++]=BYTE1(_temp);
data_to_send[_cnt++]=BYTE0(_temp);
_temp = fc.pid[PIDROLL].kD ;
data_to_send[_cnt++]=BYTE1(_temp);
data_to_send[_cnt++]=BYTE0(_temp);
_temp = fc.pid[PIDPITCH].kP ;
data_to_send[_cnt++]=BYTE1(_temp);
data_to_send[_cnt++]=BYTE0(_temp);
_temp = fc.pid[PIDPITCH].kI ;
data_to_send[_cnt++]=BYTE1(_temp);
data_to_send[_cnt++]=BYTE0(_temp);
_temp = fc.pid[PIDPITCH].kD ;
data_to_send[_cnt++]=BYTE1(_temp);
data_to_send[_cnt++]=BYTE0(_temp);
_temp = fc.pid[PIDYAW].kP;
data_to_send[_cnt++]=BYTE1(_temp);
data_to_send[_cnt++]=BYTE0(_temp);
_temp = fc.pid[PIDYAW].kI;
data_to_send[_cnt++]=BYTE1(_temp);
data_to_send[_cnt++]=BYTE0(_temp);
_temp = fc.pid[PIDYAW].kD;
data_to_send[_cnt++]=BYTE1(_temp);
data_to_send[_cnt++]=BYTE0(_temp);
data_to_send[3] = _cnt-4;
for(i=0;i<_cnt;i++)
sum += data_to_send[i];
data_to_send[_cnt++]=sum;
Send_Data(data_to_send, _cnt);
}
void Data_Send_Senser(T_RC_Sensor *data)
{
rt_uint8_t _cnt=0;
data_to_send[_cnt++]=0XAA;
data_to_send[_cnt++]=0xAA;
data_to_send[_cnt++]=0x02;
data_to_send[_cnt++]=0;
rt_int16_t _temp;
_temp = data->ACC.X;
data_to_send[_cnt++]=BYTE1(_temp);
data_to_send[_cnt++]=BYTE0(_temp);
_temp = data->ACC.Y;
data_to_send[_cnt++]=BYTE1(_temp);
data_to_send[_cnt++]=BYTE0(_temp);
_temp = data->ACC.Z;
data_to_send[_cnt++]=BYTE1(_temp);
data_to_send[_cnt++]=BYTE0(_temp);
_temp = data->GYR.X;
data_to_send[_cnt++]=BYTE1(_temp);
data_to_send[_cnt++]=BYTE0(_temp);
_temp = data->GYR.Y;
data_to_send[_cnt++]=BYTE1(_temp);
data_to_send[_cnt++]=BYTE0(_temp);
_temp = data->GYR.Z;
data_to_send[_cnt++]=BYTE1(_temp);
data_to_send[_cnt++]=BYTE0(_temp);
_temp = data->MAG.X;
data_to_send[_cnt++]=BYTE1(_temp);
data_to_send[_cnt++]=BYTE0(_temp);
_temp = data->MAG.Y;
data_to_send[_cnt++]=BYTE1(_temp);
data_to_send[_cnt++]=BYTE0(_temp);
_temp = data->MAG.Z;
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);
}
