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); }