void SCI_DIS(u32 ch0, u32 ch1, u32 ch2) //显示数据范围为-4000~+4000
{
buf[0] = ((ch0 + 4000) / 90 + ' ');
buf[1] = ((ch0 + 4000) % 90 + ' ');
buf[2] = ((ch1 + 4000) / 90 + ' ');
buf[3] = ((ch1 + 4000) % 90 + ' ');
buf[4] = ((ch2 + 4000) / 90 + ' ');
buf[5] = ((ch2 + 4000) % 90 + ' ');
DMA1_USART1_SEND((u32)buf, 6);
}
//发送PID数据
void Data_Send_PID(float Roll_P,float Roll_I,float Roll_D,
float Pitch_P,float Pitch_I,float Pitch_D,
float Yaw_P,float Yaw_I,float Yaw_D)
{
u8 updata[32];
u8 i,sum,point=0;
u16 temp;
updata[point++]=0x88;
updata[point++]=0xAC;
updata[point++]=0x1C;
updata[point++]=0xAD;
temp = Roll_P * 1000;
updata[point++]=temp>>8;
updata[point++]=temp&0xFF;
temp = Roll_I * 1000;
updata[point++]=temp>>8;
updata[point++]=temp&0xFF;
temp = Roll_D * 1000;
updata[point++]=temp>>8;
updata[point++]=temp&0xFF;
temp = Pitch_P * 1000;
updata[point++]=temp>>8;
updata[point++]=temp&0xFF;
temp = Pitch_I * 1000;
updata[point++]=temp>>8;
updata[point++]=temp&0xFF;
temp = Pitch_D * 1000;
updata[point++]=temp>>8;
updata[point++]=temp&0xFF;
temp = Yaw_P * 1000;
updata[point++]=temp>>8;
updata[point++]=temp&0xFF;
temp = Yaw_I * 1000;
updata[point++]=temp>>8;
updata[point++]=temp&0xFF;
temp = Yaw_D * 1000;
updata[point++]=temp>>8;
updata[point++]=temp&0xFF;
sum = 0;
for(i=0;i<31;i++)
{
sum += updata[i];
}
updata[31]=sum;
DMA1_USART1_SEND((u32)updata,32);
}
//发送遥控数据 和 电机PWM数据
void Data_Send_Control(u16 *rc_ch,u16 PWM1,u16 PWM2,u16 PWM3,u16 PWM4,u16 votage)
{
u8 updata[32];
u8 i,sum,point=0;
updata[point++] = 0x88;
updata[point++] = 0xAE;
updata[point++] = 0x12;
updata[point++] = rc_ch[3-1]>>8;//3通道 油门
updata[point++] = rc_ch[3-1]&0xFF;
updata[point++] = rc_ch[4-1]>>8;//4通道 偏航
updata[point++] = rc_ch[4-1]&0xFF;
updata[point++] = rc_ch[1-1]>>8;//1通道 横滚
updata[point++] = rc_ch[1-1]&0xFF;
updata[point++] = rc_ch[2-1]>>8;//2通道 俯仰
updata[point++] = rc_ch[2-1]&0xFF;
for(i=0;i<5;i++)//遥控器数值
{
updata[point++] = 0;//高8位
updata[point++] = 0;//低8位
}
updata[point++] = PWM1>>8;
updata[point++] = PWM1&0xFF;
updata[point++] = PWM2>>8;
updata[point++] = PWM2&0xFF;
updata[point++] = PWM3>>8;
updata[point++] = PWM3&0xFF;
updata[point++] = PWM4>>8;
updata[point++] = PWM4&0xFF;
updata[point++] = votage>>8;
updata[point++] = votage&0xFF;
for(i=0;i<31;i++)//校验和
{
sum += updata[i];
}
updata[31] = sum;
DMA1_USART1_SEND((u32)updata,32);
}
//发送姿态角和传感器数据
void Data_Send_Attitude(s16* acc,s16* gyro,s16* mag,float rool,float pitch,float yaw)
{
u8 updata[32];
u8 sum,i,point=0;
s16 angle[3];
angle[0] = (s16)(rool*100);
angle[1] = (s16)(pitch*100);
angle[2] = (s16)(yaw*10);
updata[point++] = 0x88;
updata[point++] = 0xAF;
updata[point++] = 0x1C;
for(i=0;i<3;i++)
{
updata[point++] = acc[i]>>8;
updata[point++] = acc[i]&0xFF;
}
for(i=0;i<3;i++)
{
updata[point++] = gyro[i]>>8;
updata[point++] = gyro[i]&0xFF;
}
for(i=0;i<3;i++)
{
updata[point++] = mag[i]>>8;
updata[point++] = mag[i]&0xFF;
}
for(i=0;i<3;i++)
{
updata[point++] = angle[i]>>8;
updata[point++] = angle[i]&0xFF;
}
for(i=0;i<4;i++)
{
updata[point++] = 0x00;
}
for(i=0;i<31;i++)//校验和
{
sum += updata[i];
}
updata[31] = sum;
DMA1_USART1_SEND((u32)updata,32);
}
//发送偏移数据
void Data_Send_Offset(s16 acc_offset_x,s16 acc_offset_y,s16 acc_offset_z,s16 gyro_offset_x,s16 gyro_offset_y,s16 gyro_offset_z)
{
u8 updata[32];
u8 i,sum,point=0;
updata[point++]=0x88;
updata[point++]=0xAC;
updata[point++]=0x1C;
updata[point++]=0xAC;
updata[point++]=acc_offset_x>>8;
updata[point++]=acc_offset_x&0xFF;
updata[point++]=acc_offset_y>>8;
updata[point++]=acc_offset_y&0xFF;
updata[point++]=acc_offset_z>>8;
updata[point++]=acc_offset_z&0xFF;
updata[point++]=gyro_offset_x>>8;
updata[point++]=gyro_offset_x&0xFF;
updata[point++]=gyro_offset_y>>8;
updata[point++]=gyro_offset_y&0xFF;
updata[point++]=gyro_offset_z>>8;
updata[point++]=gyro_offset_z&0xFF;
sum = 0;
for(i=0;i<31;i++)
{
sum += updata[i];
}
updata[31]=sum;
//NRF_TxPacket(NRF24L01_TXDATA,32);
DMA1_USART1_SEND((u32)updata,32);
}
/**************************实现函数********************************************
*函数原型: void UART1_ReportIMU(s16 yaw,s16 pitch,s16 roll
,s16 alt,s16 tempr,s16 press)
*功 能: 向上位机发送经过解算后的姿态数据
输入参数：
s16 yaw 经过解算后的航向角度。单位为0.1度 0 -> 3600 对应 0 -> 360.0度
s16 pitch 解算得到的俯仰角度，单位 0.1度。-900 - 900 对应 -90.0 -> 90.0 度
s16 roll 解算后得到的横滚角度，单位0.1度。 -1800 -> 1800 对应 -180.0 -> 180.0度
s16 alt 气压高度。 单位0.1米。 范围一个整型变量
s16 tempr 温度 。 单位0.1摄氏度 范围：直到你的电路板不能正常工作
s16 press 气压压力。单位10Pa 一个大气压强在101300pa 这个已经超过一个整型的范围。需要除以10再发给上位机
s16 IMUpersec 姿态解算速率。运算IMUpersec每秒。
输出参数：没有
*******************************************************************************/
void DMA_UART1_ReportIMU(s16 yaw, s16 pitch, s16 roll, s16 alt, s16 tempr, s16 press, s16 IMUpersec)
{
unsigned int temp = 0xaF + 2;
static char ctemp;
//USART_SendData(USART1,0xa5);
//USART_SendData(USART1,0x5a);
//USART_SendData(USART1,14+2);
//USART_SendData(USART1,0xA1);
buf[0] = 0xA5;
buf[1] = 0x5a;
buf[2] = 16;
buf[3] = 0xa1;
if (yaw < 0)yaw = 32768 - yaw;
ctemp = yaw >> 8;
//USART_SendData(USART1,ctemp);
buf[4] = ctemp;
temp += ctemp;
ctemp = yaw;
//USART_SendData(USART1,ctemp);
buf[5] = ctemp;
temp += ctemp;
if (pitch < 0)pitch = 32768 - pitch;
ctemp = pitch >> 8;
//USART_SendData(USART1,ctemp);
buf[6] = ctemp;
temp += ctemp;
ctemp = pitch;
///USART_SendData(USART1,ctemp);
buf[7] = ctemp;
temp += ctemp;
if (roll < 0)roll = 32768 - roll;
ctemp = roll >> 8;
//USART_SendData(USART1,ctemp);
buf[8] = ctemp;
temp += ctemp;
ctemp = roll;
//USART_SendData(USART1,ctemp);
buf[9] = ctemp;
temp += ctemp;
if (alt < 0)alt = 32768 - alt;
ctemp = alt >> 8;
//USART_SendData(USART1,ctemp);
buf[10] = ctemp;
temp += ctemp;
ctemp = alt;
//USART_SendData(USART1,ctemp);
buf[11] = ctemp;
temp += ctemp;
if (tempr < 0)tempr = 32768 - tempr;
ctemp = tempr >> 8;
//USART_SendData(USART1,ctemp);
buf[12] = ctemp;
temp += ctemp;
ctemp = tempr;
//USART_SendData(USART1,ctemp);
buf[13] = ctemp;
temp += ctemp;
if (press < 0)press = 32768 - press;
ctemp = press >> 8;
//USART_SendData(USART1,ctemp);
buf[14] = ctemp;
temp += ctemp;
ctemp = press;
//USART_SendData(USART1,ctemp);
buf[15] = ctemp;
temp += ctemp;
//USART_SendData(USART1,temp%256);
//USART_SendData(USART1,0xaa);
buf[16] = temp % 256;
buf[17] = 0xaa;
DMA1_USART1_SEND((u32)buf, 18);
}
void DMA_UART1_ReportMotion(s16 ax, s16 ay, s16 az, s16 gx, s16 gy, s16 gz,
s16 hx, s16 hy, s16 hz)
{
unsigned int temp = 0xaF + 9;
static char ctemp;
//USART_SendData(USART1,0xa5);
//USART_SendData(USART1,0x5a);
//USART_SendData(USART1,14+8);
//USART_SendData(USART1,0xA2);
buf[0] = 0xA5;
buf[1] = 0x5a;
buf[2] = 22;
buf[3] = 0xA2;
if (ax < 0)ax = 32768 - ax;
ctemp = ax >> 8;
//USART_SendData(USART1,ctemp);
buf[4] = ctemp;
temp += ctemp;
ctemp = ax;
//USART_SendData(USART1,ctemp);
buf[5] = ctemp;
temp += ctemp;
if (ay < 0)ay = 32768 - ay;
ctemp = ay >> 8;
//USART_SendData(USART1,ctemp);
buf[6] = ctemp;
temp += ctemp;
ctemp = ay;
//USART_SendData(USART1,ctemp);
buf[7] = ctemp;
temp += ctemp;
if (az < 0)az = 32768 - az;
ctemp = az >> 8;
//USART_SendData(USART1,ctemp);
buf[8] = ctemp;
temp += ctemp;
ctemp = az;
//USART_SendData(USART1,ctemp);
buf[9] = ctemp;
temp += ctemp;
if (gx < 0)gx = 32768 - gx;
ctemp = gx >> 8;
//USART_SendData(USART1,ctemp);
buf[10] = ctemp;
temp += ctemp;
ctemp = gx;
//USART_SendData(USART1,ctemp);
buf[11] = ctemp;
temp += ctemp;
if (gy < 0)gy = 32768 - gy;
ctemp = gy >> 8;
//USART_SendData(USART1,ctemp);
buf[12] = ctemp;
temp += ctemp;
ctemp = gy;
//USART_SendData(USART1,ctemp);
buf[13] = ctemp;
temp += ctemp;
//-------------------------
if (gz < 0)gz = 32768 - gz;
ctemp = gz >> 8;
//USART_SendData(USART1,ctemp);
buf[14] = ctemp;
temp += ctemp;
ctemp = gz;
//USART_SendData(USART1,ctemp);
buf[15] = ctemp;
temp += ctemp;
if (hx < 0)hx = 32768 - hx;
ctemp = hx >> 8;
//USART_SendData(USART1,ctemp);
buf[16] = ctemp;
temp += ctemp;
ctemp = hx;
//USART_SendData(USART1,ctemp);
buf[17] = ctemp;
temp += ctemp;
if (hy < 0)hy = 32768 - hy;
ctemp = hy >> 8;
//USART_SendData(USART1,ctemp);
buf[18] = ctemp;
temp += ctemp;
ctemp = hy;
//USART_SendData(USART1,ctemp);
buf[19] = ctemp;
temp += ctemp;
if (hz < 0)hz = 32768 - hz;
ctemp = hz >> 8;
//USART_SendData(USART1,ctemp);
buf[20] = ctemp;
temp += ctemp;
ctemp = hz;
//USART_SendData(USART1,ctemp);
buf[21] = ctemp;
temp += ctemp;
//USART_SendData(USART1,temp%256);
//USART_SendData(USART1,0xaa);
buf[22] = temp % 256;
buf[23] = 0xaa;
DMA1_USART1_SEND((u32)buf, 24);
}
