目录
一、背景
前段时间帮我师弟做了一个“无人机喷药”的项目，时间比较仓促，做的也比较简陋，毕竟是毕设嘛，比起外观，更注重功能的完整性。他是自动化专业，之前学过一些电子电路、微机原理、C语言的课程，但是没有相关项目的开发经验，要求我手把手教他完成这个项目，一方面能顺利毕业，另一方面也为今后找工作做铺垫，整个项目前后花了将近3个月的时间。下面我把整个开发过程做了一个详细的记录，有相关需求的小伙伴可以做个借鉴或私信我进行交流，有不对的地方欢迎大家批评并指正。（这里要特别感谢正点原子、小马哥以及匿名科创团队，整个项目得以顺利完成，是因为我站在了巨人的肩膀上。）
哔哩哔哩视频效果演示：https://www.bilibili.com/video/BV1aP4y137b3/?vd_source=646ab3ffb3c97fca658a999badabbc66
二、整体架构
整个无人机主要由以下几部分组成：机架、飞控、遥控器和地面站。其中机架可以根据自己的需求自行进行设计并3D打印，飞控和遥控器可以自己画PCB，自己焊接。（后文有飞控和遥控器全部的原理图、PCB源文件和源代码）。
如果毕设学校不给报销，或者经费不足，飞控可以直接用单片机代替，遥控器可以用地面站（PC端或者手机端）代替，另外再买个6轴姿态传感器和无线透传模块即可，这样可以节省很多费用和时间。推荐组合：正点原子核心板+匿名科创地面站+MPU6050模块+无线模块。
三、硬件设计
四旋翼无人机主要由检测模块、控制模块、驱动模块以及电源模块四个部分组成。
1、电源管理
2、STM32最小系统
3、IMU九轴姿态传感器+气压计
4、2.4G无线通信模块
5、SBUS、串口等其他外围电路
四、软件设计
软件采用FreeRTOS操作系统和裸机两种方式开发，提供了：①PID控制、LPF、滑动窗口滤波等多种无人机相关飞行控制算法；②陀螺仪、加速度计、气压计、超声波、NRF24L01、定时器PWM、GPS、蓝牙、WiFi等多种传感器的驱动；③匿名地面站、SBUS等多种遥控遥测协议的数据解析。所有代码均开源，尤其适合初学者。
1、main创建各任务函数
//姿态控制任务函数
void ctl_task(void *pvParameters)
{
while(1)
{
PID_Calculate();
UAV_Control();
vTaskDelay(4);
}
}
//姿态解算任务函数
void imu_task(void *pvParameters)
{
TickType_t lastWakeTime = xTaskGetTickCount();
while(1)
{
Read_MPU6050_RawData();
Get_MPU6050_Value();
vTaskDelayUntil(&lastWakeTime,(DeltaT*1000)); //2ms
}
}
//高度控制任务函数
void high_task(void *pvParameters)
{
while(1)
{
Get_BMP280_Value();
vTaskDelay(10);
}
}
//遥控遥测任务函数
void controller_task(void *pvParameters)
{
while(1)
{
Get_NRF24l01_Value();
ANO_DT_Data_Exchange();
#if 0
vTaskList(TaskInformationList);
printf("\r\n%s\r\n",TaskInformationList);
#endif
vTaskDelay(10);
}
}
//GPS任务函数
void gps_task(void *pvParameters)
{
while(1)
{
Get_GPS_Value();
vTaskDelay(100-3);
}
}
//flash数据存储任务函数
void flash_task(void *pvParameters)
{
while(1)
{
Flash_PID_Param_Update();
}
}
//匿名参数设置任务函数
void ano_task(void *pvParameters)
{
while(1)
{
ANO_DataPack_Rx();
}
}2、陀螺仪、加速度计姿态数据解算函数
void Read_MPU6050_RawData(void)
{
vTaskSuspendAll();
MPU6050_Read_RawData();
xTaskResumeAll();
MPU6050_Calibrate(); //对MPU6050进行处理，减去零偏。
SortAver_FilterXYZ(&MPU6050_ACC_RAW,&Acc_filt,12);//对加速度原始数据进行去极值滑动窗口滤波
//加速度AD值 转换成 米/平方秒
Acc_filt.X = (float)Acc_filt.X * Acc_Gain * G;
Acc_filt.Y = (float)Acc_filt.Y * Acc_Gain * G;
Acc_filt.Z = (float)Acc_filt.Z * Acc_Gain * G;
//陀螺仪AD值 转换成 弧度/秒
Gyr_rad.X = (float) MPU6050_GYRO_RAW.X * Gyro_Gr;
Gyr_rad.Y = (float) MPU6050_GYRO_RAW.Y * Gyro_Gr;
Gyr_rad.Z = (float) MPU6050_GYRO_RAW.Z * Gyro_Gr;
Acc_filt.X = Acc_filt.X * Kp_New + Acc_filtold.X * Kp_Old;
Acc_filt.Y = Acc_filt.Y * Kp_New + Acc_filtold.Y * Kp_Old;
Acc_filt.Z = Acc_filt.Z * Kp_New + Acc_filtold.Z * Kp_Old;
Acc_filtold.X = Acc_filt.X;
Acc_filtold.Y = Acc_filt.Y;
Acc_filtold.Z = Acc_filt.Z;
accb[0] = Acc_filt.X;
accb[1] = Acc_filt.Y;
accb[2] = Acc_filt.Z;
}
float q0 = 1, q1 = 0, q2 = 0, q3 = 0;
float DCMgb[3][3]; //方向余弦阵（将 惯性坐标系 转化为 机体坐标系）
void IMUupdate(FLOAT_XYZ *Gyr_rad,FLOAT_XYZ *Acc_filt,FLOAT_ANGLE *Att_Angle)
{
uint8_t i;
float matrix[9] = {1.f, 0.0f, 0.0f, 0.0f, 1.f, 0.0f, 0.0f, 0.0f, 1.f };//初始化矩阵
float ax = Acc_filt->X,ay = Acc_filt->Y,az = Acc_filt->Z;
float gx = Gyr_rad->X,gy = Gyr_rad->Y,gz = Gyr_rad->Z;
float vx, vy, vz;
float ex, ey, ez;
float norm;
float q0q0 = q0*q0;
float q0q1 = q0*q1;
float q0q2 = q0*q2;
float q0q3 = q0*q3;
float q1q1 = q1*q1;
float q1q2 = q1*q2;
float q1q3 = q1*q3;
float q2q2 = q2*q2;
float q2q3 = q2*q3;
float q3q3 = q3*q3;
if(ax*ay*az==0)
return;
//加速度计测量的重力向量(机体坐标系)
norm = invSqrt(ax*ax + ay*ay + az*az);
ax = ax * norm;
ay = ay * norm;
az = az * norm;
// printf("ax=%0.2f ay=%0.2f az=%0.2f\r\n",ax,ay,az);
//陀螺仪积分估计重力向量(机体坐标系)
vx = 2*(q1q3 - q0q2);
vy = 2*(q0q1 + q2q3);
vz = q0q0 - q1q1 - q2q2 + q3q3 ;
// printf("vx=%0.2f vy=%0.2f vz=%0.2f\r\n",vx,vy,vz);
//测量的重力向量与估算的重力向量差积求出向量间的误差
ex = (ay*vz - az*vy); //+ (my*wz - mz*wy);
ey = (az*vx - ax*vz); //+ (mz*wx - mx*wz);
ez = (ax*vy - ay*vx); //+ (mx*wy - my*wx);
//用上面求出误差进行积分
exInt = exInt + ex * Ki;
eyInt = eyInt + ey * Ki;
ezInt = ezInt + ez * Ki;
//将误差PI后补偿到陀螺仪
gx = gx + Kp*ex + exInt;
gy = gy + Kp*ey + eyInt;
gz = gz + Kp*ez + ezInt;
//四元素的微分方程
q0 = q0 + (-q1*gx - q2*gy - q3*gz)*halfT;
q1 = q1 + (q0*gx + q2*gz - q3*gy)*halfT;
q2 = q2 + (q0*gy - q1*gz + q3*gx)*halfT;
q3 = q3 + (q0*gz + q1*gy - q2*gx)*halfT;
//单位化四元数
norm = invSqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3);
q0 = q0 * norm;
q1 = q1 * norm;
q2 = q2 * norm;
q3 = q3 * norm;
//矩阵R将惯性坐标系转换到机体坐标系
matrix[0] = q0q0 + q1q1 - q2q2 - q3q3;
matrix[1] = 2.f * (q1q2 + q0q3);
matrix[2] = 2.f * (q1q3 - q0q2);
matrix[3] = 2.f * (q1q2 - q0q3);
matrix[4] = q0q0 - q1q1 + q2q2 - q3q3;
matrix[5] = 2.f * (q2q3 + q0q1);
matrix[6] = 2.f * (q1q3 + q0q2);
matrix[7] = 2.f * (q2q3 - q0q1);
matrix[8] = q0q0 - q1q1 - q2q2 + q3q3;
//四元数转换成欧拉角
Att_Angle->yaw = atan2(2.f * (q1q2 + q0q3), q0q0 + q1q1 - q2q2 - q3q3)* 57.3f; // yaw
Att_Angle->rol = -asin(2.f * (q1q3 - q0q2))* 57.3f; // roll
Att_Angle->pit = atan2(2.f * q2q3 + 2.f * q0q1, q0q0 - q1q1 - q2q2 + q3q3)* 57.3f ; // pitch
for(i=0;i<9;i++)
{
*(&(DCMgb[0][0])+i) = matrix[i];
}
}3、PID控制函数
void PID_Init(struct PID *pid,float kp,float ki,float kd,float Irange,float Ilimit)
{
pid->Kp=kp;
pid->Ki=ki;
pid->Kd=kd;
pid->IntegralRange=Irange;
pid->IntegralLimit=Ilimit;
}
void PID_Control(struct PID *pid,float SetValue,float ActualValue)
{
pid->Error=SetValue-ActualValue;
if( fabs(pid->Error)>pid->IntegralRange || RC_Control.THR<=THROTTLE_MIN)
{
pid->IntegralFlag=0;
}
else
{
pid->IntegralFlag=1;
pid->SumIntegral+=pid->Error;
if(pid->SumIntegral>pid->IntegralLimit) pid->SumIntegral=pid->IntegralLimit;
if(pid->SumIntegral<-pid->IntegralLimit) pid->SumIntegral=-pid->IntegralLimit;
}
pid->Pout=pid->Kp*pid->Error;
pid->Iout=pid->IntegralFlag*pid->Ki*pid->SumIntegral;
pid->Dout=pid->Kd*(pid->Error-pid->PreError);
pid->OutPut=pid->Pout+pid->Iout+pid->Dout;
pid->PreError=pid->Error;
#if 0
printf("PID输出为：%.2f\r\n",pid->OutPut);
#endif
}
void PID_Calculate(void)
{
#if 1
TargetAngle.Rol=(RC_Control.ROL-1500)/25.0f;
MeasureAngle.Rol=Att_Angle.rol;
MeasureRate.RolRate=Gyr_rad.X*RadtoDeg;
PID_Control(&PID_ROL_Angle,TargetAngle.Rol,MeasureAngle.Rol);
PID_Control(&PID_ROL_Rate,PID_ROL_Angle.OutPut,MeasureRate.RolRate);
#endif
#if 1
TargetAngle.Pit=(RC_Control.PIT-1500)/25.0f;
MeasureAngle.Pit=Att_Angle.pit;
MeasureRate.PitRate=Gyr_rad.Y*RadtoDeg;
PID_Control(&PID_PIT_Angle,TargetAngle.Pit,MeasureAngle.Pit);
PID_Control(&PID_PIT_Rate,PID_PIT_Angle.OutPut,MeasureRate.PitRate);
#endif
#if 1
TargetAngle.Yaw=(1500-RC_Control.YAW)/20.0f;
MeasureAngle.Yaw=Att_Angle.yaw;
MeasureRate.YawRate=Gyr_rad.Z*RadtoDeg;
PID_Control(&PID_YAW_Angle,TargetAngle.Yaw,MeasureAngle.Yaw);
PID_Control(&PID_YAW_Rate,PID_YAW_Angle.OutPut,MeasureRate.YawRate);
#endif
#if 1
TargetAngle.Alt=0;
MeasureAngle.Alt=0;
MeasureRate.AltRate=0;
PID_Control(&PID_ALT,TargetAngle.Alt,MeasureAngle.Alt);
PID_Control(&PID_ALT_Rate,PID_ALT.OutPut,MeasureRate.AltRate);
#endif
#if 0
printf("Rol测量值:%.3f,%.3f,%.3f\r\n",TargetAngle.Rol,MeasureAngle.Rol,MeasureRate.RolRate);
printf("Pit测量值:%.3f,%.3f,%.3f\r\n",TargetAngle.Pit,MeasureAngle.Pit,MeasureRate.PitRate);
printf("Yaw测量值:%.3f,%.3f,%.3f\r\n",TargetAngle.Yaw,MeasureAngle.Yaw,MeasureRate.YawRate);
printf("Thr测量值:%d\r\n",RC_Control.THR);
#endif
}
void UAV_Control(void)
{
static u8 DisableFlag=0;
if(RC_Control.THR>THROTTLE_MIN)
{
Moto_PWM_1=RC_Control.THR+PID_ALT_Rate.OutPut+PID_ROL_Rate.OutPut+PID_PIT_Rate.OutPut+PID_YAW_Rate.OutPut+OFFSET_M1;
Moto_PWM_2=RC_Control.THR+PID_ALT_Rate.OutPut-PID_ROL_Rate.OutPut+PID_PIT_Rate.OutPut-PID_YAW_Rate.OutPut+OFFSET_M2;
Moto_PWM_3=RC_Control.THR+PID_ALT_Rate.OutPut-PID_ROL_Rate.OutPut-PID_PIT_Rate.OutPut+PID_YAW_Rate.OutPut+OFFSET_M3;
Moto_PWM_4=RC_Control.THR+PID_ALT_Rate.OutPut+PID_ROL_Rate.OutPut-PID_PIT_Rate.OutPut-PID_YAW_Rate.OutPut+OFFSET_M4;
}
else
{
Moto_PWM_1 = MOTOR_PWM_MIN;
Moto_PWM_2 = MOTOR_PWM_MIN;
Moto_PWM_3 = MOTOR_PWM_MIN;
Moto_PWM_4 = MOTOR_PWM_MIN;
}
if(DisableFlag==1)
{
Moto_PWM_1 = MOTOR_PWM_MIN;
Moto_PWM_2 = MOTOR_PWM_MIN;
Moto_PWM_3 = MOTOR_PWM_MIN;
Moto_PWM_4 = MOTOR_PWM_MIN;
}
Moto_PWM_1=LIMIT(Moto_PWM_1,MOTOR_PWM_MIN,MOTOR_PWM_MAX);
Moto_PWM_2=LIMIT(Moto_PWM_2,MOTOR_PWM_MIN,MOTOR_PWM_MAX);
Moto_PWM_3=LIMIT(Moto_PWM_3,MOTOR_PWM_MIN,MOTOR_PWM_MAX);
Moto_PWM_4=LIMIT(Moto_PWM_4,MOTOR_PWM_MIN,MOTOR_PWM_MAX);
TIM_SetCompare1(TIM3,Moto_PWM_1);
TIM_SetCompare2(TIM3,Moto_PWM_2);
TIM_SetCompare3(TIM3,Moto_PWM_3);
TIM_SetCompare4(TIM3,Moto_PWM_4);
}
4、匿名上位机
extern INT16_XYZ GYRO_OFFSET_RAW,ACC_OFFSET_RAW; //零漂数据
extern xSemaphoreHandle PIDWriteFlashFlag;
void ANO_DT_Data_Receive_Anl(uint8_t *data_buf,uint8_t num)
{
uint8_t sum = 0,i;
for(i=0;i<(num-1);i++)
sum += *(data_buf+i);
if(!(sum==*(data_buf+num-1))) return; //判断sum
if(!(*(data_buf)==0xAA && *(data_buf+1)==0xAF)) return; //判断帧头
if(*(data_buf+2)==0X01)
{
if(*(data_buf+4)==0X01)
{
SENSER_FLAG_SET(ACC_OFFSET);//加速度校准
ACC_OFFSET_RAW.X = 0;ACC_OFFSET_RAW.Y = 0;ACC_OFFSET_RAW.Z = 0;
}
if(*(data_buf+4)==0X02)
{
SENSER_FLAG_SET(GYRO_OFFSET);//陀螺仪校准
GYRO_OFFSET_RAW.X = 0;GYRO_OFFSET_RAW.Y = 0;GYRO_OFFSET_RAW.Z = 0;
}
if(*(data_buf+4)==0X03)
{
SENSER_FLAG_SET(ACC_OFFSET);//加速度校准
SENSER_FLAG_SET(GYRO_OFFSET);//陀螺仪校准
}
if(*(data_buf+4)==0X04)
{
}
if(*(data_buf+4)==0X05)
{
SENSER_FLAG_SET(BAR_OFFSET);//气压计校准
}
}
if(*(data_buf+2)==0X02)
{
if(*(data_buf+4)==0X01)
{
f.send_pid1 = 1;//读取PID1
f.send_pid2 = 1;//读取PID2
f.send_pid3 = 1;//读取PID3
}
if(*(data_buf+4)==0X02)
{
}
if(*(data_buf+4)==0XA0) //读取版本信息
{
f.send_version = 1;
}
if(*(data_buf+4)==0XA1) //恢复默认参数
{
f.send_pid4 = 1; //参数清零
}
}
if(*(data_buf+2)==0X10) //PID1
{
PID_ROL_Rate.Kp = 0.001*( (vs16)(*(data_buf+4)<<8)|*(data_buf+5) );
PID_ROL_Rate.Ki = 0.001*( (vs16)(*(data_buf+6)<<8)|*(data_buf+7) );
PID_ROL_Rate.Kd = 0.001*( (vs16)(*(data_buf+8)<<8)|*(data_buf+9) );
PID_PIT_Rate.Kp = 0.001*( (vs16)(*(data_buf+10)<<8)|*(data_buf+11) );
PID_PIT_Rate.Ki = 0.001*( (vs16)(*(data_buf+12)<<8)|*(data_buf+13) );
PID_PIT_Rate.Kd = 0.001*( (vs16)(*(data_buf+14)<<8)|*(data_buf+15) );
PID_YAW_Rate.Kp = 0.001*( (vs16)(*(data_buf+16)<<8)|*(data_buf+17) );
PID_YAW_Rate.Ki = 0.001*( (vs16)(*(data_buf+18)<<8)|*(data_buf+19) );
PID_YAW_Rate.Kd = 0.001*( (vs16)(*(data_buf+20)<<8)|*(data_buf+21) );
ANO_DT_Send_Check(*(data_buf+2),sum);
}
if(*(data_buf+2)==0X11) //PID2
{
PID_ROL_Angle.Kp = 0.001*( (vs16)(*(data_buf+4)<<8)|*(data_buf+5) );
PID_ROL_Angle.Ki = 0.001*( (vs16)(*(data_buf+6)<<8)|*(data_buf+7) );
PID_ROL_Angle.Kd = 0.001*( (vs16)(*(data_buf+8)<<8)|*(data_buf+9) );
PID_PIT_Angle.Kp = 0.001*( (vs16)(*(data_buf+10)<<8)|*(data_buf+11) );
PID_PIT_Angle.Ki = 0.001*( (vs16)(*(data_buf+12)<<8)|*(data_buf+13) );
PID_PIT_Angle.Kd = 0.001*( (vs16)(*(data_buf+14)<<8)|*(data_buf+15) );
PID_YAW_Angle.Kp = 0.001*( (vs16)(*(data_buf+16)<<8)|*(data_buf+17) );
PID_YAW_Angle.Ki = 0.001*( (vs16)(*(data_buf+18)<<8)|*(data_buf+19) );
PID_YAW_Angle.Kd = 0.001*( (vs16)(*(data_buf+20)<<8)|*(data_buf+21) );
ANO_DT_Send_Check(*(data_buf+2),sum);
}
if(*(data_buf+2)==0X12) //PID3
{
PID_ALT_Rate.Kp = 0.001*( (vs16)(*(data_buf+4)<<8)|*(data_buf+5) );
PID_ALT_Rate.Ki = 0.001*( (vs16)(*(data_buf+6)<<8)|*(data_buf+7) );
PID_ALT_Rate.Kd = 0.001*( (vs16)(*(data_buf+8)<<8)|*(data_buf+9) );
PID_ALT.Kp = 0.001*( (vs16)(*(data_buf+10)<<8)|*(data_buf+11) );
PID_ALT.Ki = 0.001*( (vs16)(*(data_buf+12)<<8)|*(data_buf+13) );
PID_ALT.Kd = 0.001*( (vs16)(*(data_buf+14)<<8)|*(data_buf+15) );;
ANO_DT_Send_Check(*(data_buf+2),sum);
}
if(*(data_buf+2)==0X13)
{
}
if(*(data_buf+2)==0X14)
{
}
if(*(data_buf+2)==0X15)
{
xSemaphoreGive(PIDWriteFlashFlag);
}
}5、NRF24L01无线模块
u8 NRF24L01_TxPacket(u8 *txbuf)
{
u8 sta;
SPI1_SetSpeed(SPI_BaudRatePrescaler_16);//spi速度为10.5Mhz（24L01的最大SPI时钟为10Mhz）
NRF24L01_CE=0;
NRF24L01_Write_Buf(WR_TX_PLOAD,txbuf,TX_PLOAD_WIDTH);//写数据到TX BUF 32个字节
NRF24L01_CE=1;//启动发送
while(NRF24L01_IRQ!=0);//等待发送完成
sta=NRF24L01_Read_Reg(STATUS); //读取状态寄存器的值
NRF24L01_Write_Reg(NRF_WRITE_REG+STATUS,sta); //清除TX_DS或MAX_RT中断标志
if(sta&MAX_TX)//达到最大重发次数
{
NRF24L01_Write_Reg(FLUSH_TX,0xff);//清除TX FIFO寄存器
return MAX_TX;
}
if(sta&TX_OK)//发送完成
{
return TX_OK;
}
return 0xff;//其他原因发送失败
}
u8 NRF24L01_RxPacket(u8 *rxbuf)
{
u8 sta;
SPI1_SetSpeed(SPI_BaudRatePrescaler_16); //spi速度为10.5Mhz（24L01的最大SPI时钟为10Mhz）
sta=NRF24L01_Read_Reg(STATUS); //读取状态寄存器的值
NRF24L01_Write_Reg(NRF_WRITE_REG+STATUS,sta); //清除TX_DS或MAX_RT中断标志
//printf("0x%02x\r\n",sta);
if(sta&RX_OK)//接收到数据
{
NRF24L01_Read_Buf(RD_RX_PLOAD,rxbuf,RX_PLOAD_WIDTH);//读取数据
NRF24L01_Write_Reg(FLUSH_RX,0xff);//清除RX FIFO寄存器
return 0;
}
return 1;//没收到任何数据
}
void NRF24L01_RX_Mode(void)
{
NRF24L01_CE=0;
NRF24L01_Write_Buf(NRF_WRITE_REG+RX_ADDR_P0,(u8*)RX_ADDRESS,RX_ADR_WIDTH);//写RX节点地址
NRF24L01_Write_Reg(NRF_WRITE_REG+EN_AA,0x01); //使能通道0的自动应答
NRF24L01_Write_Reg(NRF_WRITE_REG+EN_RXADDR,0x01);//使能通道0的接收地址
NRF24L01_Write_Reg(NRF_WRITE_REG+RF_CH,45); //设置RF通信频率
NRF24L01_Write_Reg(NRF_WRITE_REG+RX_PW_P0,RX_PLOAD_WIDTH);//选择通道0的有效数据宽度
NRF24L01_Write_Reg(NRF_WRITE_REG+RF_SETUP,0x0f);//设置TX发射参数,0db增益,2Mbps,低噪声增益开启
NRF24L01_Write_Reg(NRF_WRITE_REG+CONFIG, 0x0f);//配置基本工作模式的参数;PWR_UP,EN_CRC,16BIT_CRC,接收模式
NRF24L01_CE = 1; //CE为高,进入接收模式
}
void NRF24L01_TX_Mode(void)
{
NRF24L01_CE=0;
NRF24L01_Write_Buf(NRF_WRITE_REG+TX_ADDR,(u8*)TX_ADDRESS,TX_ADR_WIDTH);//写TX节点地址
NRF24L01_Write_Buf(NRF_WRITE_REG+RX_ADDR_P0,(u8*)RX_ADDRESS,RX_ADR_WIDTH); //设置TX节点地址,主要为了使能ACK
NRF24L01_Write_Reg(NRF_WRITE_REG+EN_AA,0x01); //使能通道0的自动应答
NRF24L01_Write_Reg(NRF_WRITE_REG+EN_RXADDR,0x01); //使能通道0的接收地址
NRF24L01_Write_Reg(NRF_WRITE_REG+SETUP_RETR,0x1a);//设置自动重发间隔时间:500us + 86us;最大自动重发次数:10次
NRF24L01_Write_Reg(NRF_WRITE_REG+RF_CH,45); //设置RF通道为40
NRF24L01_Write_Reg(NRF_WRITE_REG+RF_SETUP,0x0f); //设置TX发射参数,0db增益,2Mbps,低噪声增益开启
NRF24L01_Write_Reg(NRF_WRITE_REG+CONFIG,0x0e); //配置基本工作模式的参数;PWR_UP,EN_CRC,16BIT_CRC,接收模式,开启所有中断
NRF24L01_CE=1;//CE为高,10us后启动发送
}6、GPS数据解析
void NMEA_BDGSV_Analysis(nmea_msg *gpsx,u8 *buf)
{
u8 *p,*p1,dx;
u8 len,i,j,slx=0;
u8 posx;
p=buf;
p1=(u8*)strstr((const char *)p,"$BDGSV");
len=p1[7]-'0'; //得到BDGSV的条数
posx=NMEA_Comma_Pos(p1,3); //得到可见北斗卫星总数
if(posx!=0XFF)gpsx->beidou_svnum=NMEA_Str2num(p1+posx,&dx);
for(i=0;i<len;i++)
{
p1=(u8*)strstr((const char *)p,"$BDGSV");
for(j=0;j<4;j++)
{
posx=NMEA_Comma_Pos(p1,4+j*4);
if(posx!=0XFF)gpsx->beidou_slmsg[slx].beidou_num=NMEA_Str2num(p1+posx,&dx); //得到卫星编号
else break;
posx=NMEA_Comma_Pos(p1,5+j*4);
if(posx!=0XFF)gpsx->beidou_slmsg[slx].beidou_eledeg=NMEA_Str2num(p1+posx,&dx);//得到卫星仰角
else break;
posx=NMEA_Comma_Pos(p1,6+j*4);
if(posx!=0XFF)gpsx->beidou_slmsg[slx].beidou_azideg=NMEA_Str2num(p1+posx,&dx);//得到卫星方位角
else break;
posx=NMEA_Comma_Pos(p1,7+j*4);
if(posx!=0XFF)gpsx->beidou_slmsg[slx].beidou_sn=NMEA_Str2num(p1+posx,&dx); //得到卫星信噪比
else break;
slx++;
}
p=p1+1;//切换到下一个BDGSV信息
}
}
void NMEA_GNGGA_Analysis(nmea_msg *gpsx,u8 *buf)
{
u8 *p1,dx;
u8 posx;
p1=(u8*)strstr((const char *)buf,"$GNGGA");
posx=NMEA_Comma_Pos(p1,6); //得到GPS状态
if(posx!=0XFF)gpsx->gpssta=NMEA_Str2num(p1+posx,&dx);
posx=NMEA_Comma_Pos(p1,7); //得到用于定位的卫星数
if(posx!=0XFF)gpsx->posslnum=NMEA_Str2num(p1+posx,&dx);
posx=NMEA_Comma_Pos(p1,9); //得到海拔高度
if(posx!=0XFF)gpsx->altitude=NMEA_Str2num(p1+posx,&dx);
}
void NMEA_GNGSA_Analysis(nmea_msg *gpsx,u8 *buf)
{
u8 *p1,dx;
u8 posx;
u8 i;
p1=(u8*)strstr((const char *)buf,"$GNGSA");
posx=NMEA_Comma_Pos(p1,2); //得到定位类型
if(posx!=0XFF)gpsx->fixmode=NMEA_Str2num(p1+posx,&dx);
for(i=0;i<12;i++) //得到定位卫星编号
{
posx=NMEA_Comma_Pos(p1,3+i);
if(posx!=0XFF)gpsx->possl[i]=NMEA_Str2num(p1+posx,&dx);
else break;
}
posx=NMEA_Comma_Pos(p1,15); //得到PDOP位置精度因子
if(posx!=0XFF)gpsx->pdop=NMEA_Str2num(p1+posx,&dx);
posx=NMEA_Comma_Pos(p1,16); //得到HDOP位置精度因子
if(posx!=0XFF)gpsx->hdop=NMEA_Str2num(p1+posx,&dx);
posx=NMEA_Comma_Pos(p1,17); //得到VDOP位置精度因子
if(posx!=0XFF)gpsx->vdop=NMEA_Str2num(p1+posx,&dx);
}
void NMEA_GNRMC_Analysis(nmea_msg *gpsx,u8 *buf)
{
u8 *p1,dx;
u8 posx;
u32 temp;
float rs;
p1=(u8*)strstr((const char *)buf,"$GNRMC");//"$GNRMC",经常有&和GNRMC分开的情况,故只判断GPRMC.
posx=NMEA_Comma_Pos(p1,1); //得到UTC时间
if(posx!=0XFF)
{
temp=NMEA_Str2num(p1+posx,&dx)/NMEA_Pow(10,dx); //得到UTC时间,去掉ms
gpsx->utc.hour=temp/10000;
gpsx->utc.min=(temp/100)%100;
gpsx->utc.sec=temp%100;
}
posx=NMEA_Comma_Pos(p1,3); //得到纬度
if(posx!=0XFF)
{
temp=NMEA_Str2num(p1+posx,&dx);
gpsx->latitude=temp/NMEA_Pow(10,dx+2); //得到° mmnn.nnnnn
rs=temp%NMEA_Pow(10,dx+2); //得到'
gpsx->latitude=gpsx->latitude*NMEA_Pow(10,5)+(rs*NMEA_Pow(10,5-dx))/60;//转换为°
}
posx=NMEA_Comma_Pos(p1,4); //南纬还是北纬
if(posx!=0XFF)gpsx->nshemi=*(p1+posx);
posx=NMEA_Comma_Pos(p1,5); //得到经度
if(posx!=0XFF)
{
temp=NMEA_Str2num(p1+posx,&dx);
gpsx->longitude=temp/NMEA_Pow(10,dx+2); //得到°
rs=temp%NMEA_Pow(10,dx+2); //得到'
gpsx->longitude=gpsx->longitude*NMEA_Pow(10,5)+(rs*NMEA_Pow(10,5-dx))/60;//转换为°
}
posx=NMEA_Comma_Pos(p1,6); //东经还是西经
if(posx!=0XFF)gpsx->ewhemi=*(p1+posx);
posx=NMEA_Comma_Pos(p1,9); //得到UTC日期
if(posx!=0XFF)
{
temp=NMEA_Str2num(p1+posx,&dx); //得到UTC日期
gpsx->utc.date=temp/10000;
gpsx->utc.month=(temp/100)%100;
gpsx->utc.year=2000+temp%100;
}
}
void NMEA_GNVTG_Analysis(nmea_msg *gpsx,u8 *buf)
{
u8 *p1,dx;
u8 posx;
p1=(u8*)strstr((const char *)buf,"$GNVTG");
posx=NMEA_Comma_Pos(p1,7); //得到地面速率
if(posx!=0XFF)
{
gpsx->speed=NMEA_Str2num(p1+posx,&dx);
if(dx<3)gpsx->speed*=NMEA_Pow(10,3-dx); //确保扩大1000倍
}
}7、气压计数据解析
u32 bmp280CompensateT(s32 adcT)
{
s32 var1, var2, T;
var1 = ((((adcT >> 3) - ((s32)bmp280Cal.dig_T1 << 1))) * ((s32)bmp280Cal.dig_T2)) >> 11;
var2 = (((((adcT >> 4) - ((s32)bmp280Cal.dig_T1)) * ((adcT >> 4) - ((s32)bmp280Cal.dig_T1))) >> 12) * ((s32)bmp280Cal.dig_T3)) >> 14;
bmp280Cal.t_fine = var1 + var2;
T = (bmp280Cal.t_fine * 5 + 128) >> 8;
return T;
}
u32 bmp280CompensateP(s32 adcP)
{
int64_t var1, var2, p;
var1 = ((int64_t)bmp280Cal.t_fine) - 128000;
var2 = var1 * var1 * (int64_t)bmp280Cal.dig_P6;
var2 = var2 + ((var1*(int64_t)bmp280Cal.dig_P5) << 17);
var2 = var2 + (((int64_t)bmp280Cal.dig_P4) << 35);
var1 = ((var1 * var1 * (int64_t)bmp280Cal.dig_P3) >> 8) + ((var1 * (int64_t)bmp280Cal.dig_P2) << 12);
var1 = (((((int64_t)1) << 47) + var1)) * ((int64_t)bmp280Cal.dig_P1) >> 33;
if (var1 == 0)
return 0;
p = 1048576 - adcP;
p = (((p << 31) - var2) * 3125) / var1;
var1 = (((int64_t)bmp280Cal.dig_P9) * (p >> 13) * (p >> 13)) >> 25;
var2 = (((int64_t)bmp280Cal.dig_P8) * p) >> 19;
p = ((p + var1 + var2) >> 8) + (((int64_t)bmp280Cal.dig_P7) << 4);
return (uint32_t)p;
}
float bmp280PressureToAltitude(float pressure/*, float* groundPressure, float* groundTemp*/)
{
if(pressure > 0)
{
return 44330.f * (powf((1015.7f / pressure), 0.190295f) - 1.0f);
}
else
{
return 0;
}
}8、SBUS数据解析
u8 update_sbus(u8 *buf)
{
int i;
if (buf[23] == 0)
{
SBUS_CH.ConnectState = 1;
SBUS_CH.CH1 = ((int16_t)buf[ 1] >> 0 | ((int16_t)buf[ 2] << 8 )) & 0x07FF;
SBUS_CH.CH2 = ((int16_t)buf[ 2] >> 3 | ((int16_t)buf[ 3] << 5 )) & 0x07FF;
SBUS_CH.CH3 = ((int16_t)buf[ 3] >> 6 | ((int16_t)buf[ 4] << 2 ) | (int16_t)buf[ 5] << 10 ) & 0x07FF;
SBUS_CH.CH4 = ((int16_t)buf[ 5] >> 1 | ((int16_t)buf[ 6] << 7 )) & 0x07FF;
SBUS_CH.CH5 = ((int16_t)buf[ 6] >> 4 | ((int16_t)buf[ 7] << 4 )) & 0x07FF;
SBUS_CH.CH6 = ((int16_t)buf[ 7] >> 7 | ((int16_t)buf[ 8] << 1 ) | (int16_t)buf[9] << 9 ) & 0x07FF;
SBUS_CH.CH7 = ((int16_t)buf[ 9] >> 2 | ((int16_t)buf[10] << 6 )) & 0x07FF;
SBUS_CH.CH8 = ((int16_t)buf[10] >> 5 | ((int16_t)buf[11] << 3 )) & 0x07FF;
SBUS_CH.CH9 = ((int16_t)buf[12] << 0 | ((int16_t)buf[13] << 8 )) & 0x07FF;
SBUS_CH.CH10 = ((int16_t)buf[13] >> 3 | ((int16_t)buf[14] << 5 )) & 0x07FF;
SBUS_CH.CH11 = ((int16_t)buf[14] >> 6 | ((int16_t)buf[15] << 2 ) | (int16_t)buf[16] << 10 ) & 0x07FF;
SBUS_CH.CH12 = ((int16_t)buf[16] >> 1 | ((int16_t)buf[17] << 7 )) & 0x07FF;
SBUS_CH.CH13 = ((int16_t)buf[17] >> 4 | ((int16_t)buf[18] << 4 )) & 0x07FF;
SBUS_CH.CH14 = ((int16_t)buf[18] >> 7 | ((int16_t)buf[19] << 1 ) | (int16_t)buf[20] << 9 ) & 0x07FF;
SBUS_CH.CH15 = ((int16_t)buf[20] >> 2 | ((int16_t)buf[21] << 6 )) & 0x07FF;
SBUS_CH.CH16 = ((int16_t)buf[21] >> 5 | ((int16_t)buf[22] << 3 )) & 0x07FF;
return 1;
}
else
{
SBUS_CH.ConnectState = 0;
return 0;
}
}
u16 sbus_to_pwm(u16 sbus_value)
{
float pwm;
pwm = (float)SBUS_TARGET_MIN + (float)(sbus_value - SBUS_RANGE_MIN) * SBUS_SCALE_FACTOR;
if (pwm > 2000) pwm = 2000;
if (pwm < 1000) pwm = 1000;
return (u16)pwm;
}9、超声波数据解析
#define MAX_SPEED 40.0f
#define MIN_SPEED -40.0f
char US100_RxBuffer[2];
u8 US100_ENABLE_CMD_TBL[1]= {0x55};
extern ThrotMode ThrottleMode;
struct Z_Axis_Vector Z_Axis;
void Get_US100_Value(void)
{
float temp;
US100_Send_Cmd(US100_ENABLE_CMD_TBL,1);
Src_Hight=US100_RxBuffer[0]*256+US100_RxBuffer[1];
if( Src_Hight<0.0f || Src_Hight>4000.0f )
{
return;
}
SortAver_Filter_Multiple(&SortAverFilter_Hight,Src_Hight,&SortAverFilterHight,5,1);
FirstOrderLowPassFilter(&LowPassFilter_Hight,SortAverFilterHight,0.76f,&LowPassFilterHight);
Z_Axis.CurrentHight=(u16)LowPassFilterHight;
temp=(float)(Z_Axis.CurrentHight-Z_Axis.PreHight)/Z_Axis.T; //单位mm/ms
Z_Axis.PreHight=Z_Axis.CurrentHight;
if(ThrottleMode == ThrotUp && temp>0)
{
Z_Axis.Speed=temp;
}
else if(ThrottleMode == ThrotDown && temp<=0)
{
Z_Axis.Speed=temp*Z_Axis.Amplify;
}
else
{
Z_Axis.Speed=0;
}
Z_Axis.Speed = (Z_Axis.Speed>=MAX_SPEED) ? MAX_SPEED : Z_Axis.Speed;
Z_Axis.Speed = (Z_Axis.Speed<=MIN_SPEED) ? MIN_SPEED : Z_Axis.Speed;
if( fabs(Z_Axis.Speed)>8.0f )
{
LED0=~LED0;
}
}五、总结
创作不易，希望大家多多支持，感兴趣的朋友可以扫码进群，互相交流，共同进步。
扫一扫
4972
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
