基于STM32平台
#include "CONTROL.h"
#include "BSP.H"
#include "UART2.h"
#include "IMU.h"
#include "MPU6050.h"
#include "MOTO.h"
PID PID_ROL,PID_PIT,PID_YAW;
u8 ARMED = 0;
u16 moto1=0,moto2=0,moto3=0,moto4=0;
float Get_MxMi(float num,float max,float min)//将num限制在min与max之间
{
if(num>max)
return max;
else if(num<min)
return min;
else
return num;
} //解算出来的自我姿态，根据此得出自稳所需改变角度 //遥控发来的所需要改变的角度以进行倾斜的飞行，进行前进后退或转向
//Q_ANGLE.X , Q_ANGLE.Y, Q_ANGLE.Z, //RC_Target_ROL, RC_Target_PIT, RC_Target_YAW
void CONTROL(float rol_now, float pit_now, float yaw_now, float rol_tar, float pit_tar, float yaw_tar)
{
vs16 throttle;
vs16 yaw_d;
float rol = rol_tar + rol_now;//将所有要改变的值加起来
float pit = pit_tar + pit_now;
float yaw = yaw_tar + yaw_now;
throttle = Rc_Get.THROTTLE - 1000;//油门，代表电机的转速
if(throttle<0) throttle=0;//消除实际操作误差
PID_ROL.IMAX = throttle/2; //积分项限幅=throttle的1/2，为什么和throttle有关？
Get_MxMi(PID_ROL.IMAX,1000,0);//并将积分项限幅限制在0-1000之间
PID_PIT.IMAX = PID_ROL.IMAX;
/**************************** P比例调节 ***********************************/
PID_ROL.pout = PID_ROL.P * rol;
PID_PIT.pout = PID_PIT.P * pit;
PID_YAW.pout = PID_YAW.P * yaw;
/**************************** I积分调节 **********************************/
if(rol_tar*rol_tar<0.1 && pit_tar*pit_tar<0.1 && rol_now*rol_now<30 && pit_now*pit_now<30 && throttle>300)
{//target遥控几乎未动，now当前比较接近水平，油门很大
PID_ROL.iout += PID_ROL.I * rol;
PID_PIT.iout += PID_PIT.I * pit; //积分项容易超调
PID_ROL.iout = Get_MxMi(PID_ROL.iout,PID_ROL.IMAX,-PID_ROL.IMAX);//积分项限制在积分项限幅内
PID_PIT.iout = Get_MxMi(PID_PIT.iout,PID_PIT.IMAX,-PID_PIT.IMAX);//防止大角度积累造成积分超调
}
else if(throttle<200)//油门很小不需要积分调节，为什么？
{
PID_ROL.iout = 0;
PID_PIT.iout = 0;
}
/*************************** D微分调节 ***********************************/
PID_ROL.dout = PID_ROL.D * MPU6050_GYRO_LAST.X;//，飞机头朝X正方向，ROL绕X轴转
PID_PIT.dout = PID_PIT.D * MPU6050_GYRO_LAST.Y;//6050测得角速度，角度的微分就是角速度
if(1480>Rc_Get.YAW || Rc_Get.YAW>1520)//YAW的微分调节，1500是什么也不改变的零点值
{ //自己飘的YAW值 //遥控所需要的改变量，*10提高权重,加快响应
yaw_d = MPU6050_GYRO_LAST.Z + (Rc_Get.YAW-1500)*10;
GYRO_I.Z = 0;//假设遥控不动的情况下YAW不可能自己飘，没有地磁作参考直接置零算了
} //在IMU的preparedata()中GYRO_I.Z += MPU6050_GYRO_LAST.Z*Gyro_G*0.001;//0.001为dt
//GYRO_I.Z相当于绕Z轴（即yaw）旋转的总角度
//在IMU的 IMUupdata ()中Q_ANGLE.Z = GYRO_I.Z
else //为什么要分两种情况//遥控的左转右转功能不要了
yaw_d = MPU6050_GYRO_LAST.Z;//很小的遥控改变量就不要了
PID_YAW.dout = PID_YAW.D * yaw_d;
/**************************************************************************************/
PID_ROL.OUT = PID_ROL.pout + PID_ROL.iout + PID_ROL.dout;//最终的输出量
PID_PIT.OUT = PID_PIT.pout + PID_PIT.iout + PID_PIT.dout;
PID_YAW.OUT = PID_YAW.pout + PID_YAW.iout + PID_YAW.dout;
if(throttle>200)// - + + +
{ //两正两负，调整roll的话PID_ROL.OUT要 - +,调整pit的话PID_PIT.OUT 要 - -
moto3= throttle - PID_ROL.OUT - PID_PIT.OUT + PID_YAW.OUT;//赋给电机值
moto1= throttle + PID_ROL.OUT - PID_PIT.OUT - PID_YAW.OUT;//加减号一定两正两负，电机对称
moto2 = throttle + PID_ROL.OUT + PID_PIT.OUT + PID_YAW.OUT;//要改，先注释掉其他三个
moto4= throttle - PID_ROL.OUT + PID_PIT.OUT - PID_YAW.OUT;
}
else//定义电机的死区，避免误操作
{
moto1 = 0;
moto2 = 0;
moto3 = 0;
moto4 = 0;
}
if(ARMED) {PCout(13)=0;Moto_PwmRflash(moto1,moto2,moto3,moto4);}//使能电机
else {PCout(13)=1;Moto_PwmRflash(0,0,0,0);}
}#include "imu.h"
#include "MPU6050.h"
#include "math.h"
#define RtA 57.324841 //弧度到角度
#define AtR 0.0174533 //度到角度
#define Acc_G 0.0011963 //加速度变成G
#define Gyro_G 0.0610351 //角速度变成度 此参数对应陀螺2000度每秒
#define Gyro_Gr 0.0010653 //角速度变成弧度 此参数对应陀螺2000度每秒
#define FILTER_NUM 20
S_INT16_XYZ ACC_AVG; //平均值滤波后的ACC
S_FLOAT_XYZ GYRO_I; //陀螺仪积分
S_FLOAT_XYZ EXP_ANGLE; //期望角度
S_FLOAT_XYZ DIF_ANGLE; //期望角度与实际角度差
S_FLOAT_XYZ Q_ANGLE; //四元数计算出的角度
int16_t ACC_X_BUF[FILTER_NUM],ACC_Y_BUF[FILTER_NUM],ACC_Z_BUF[FILTER_NUM]; //加速度滑动窗口滤波数组
//filter是滤波的意思
void Prepare_Data(void)
{
static uint8_t filter_cnt=0;
int32_t temp1=0,temp2=0,temp3=0;
uint8_t i;
MPU6050_Read();
MPU6050_Dataanl();
ACC_X_BUF[filter_cnt] = MPU6050_ACC_LAST.X;//更新滑动窗口数组
ACC_Y_BUF[filter_cnt] = MPU6050_ACC_LAST.Y;
ACC_Z_BUF[filter_cnt] = MPU6050_ACC_LAST.Z;
for(i=0;i<FILTER_NUM;i++)
{
temp1 += ACC_X_BUF[i];
temp2 += ACC_Y_BUF[i];
temp3 += ACC_Z_BUF[i];
}
ACC_AVG.X = temp1 / FILTER_NUM;
ACC_AVG.Y = temp2 / FILTER_NUM;
ACC_AVG.Z = temp3 / FILTER_NUM;
filter_cnt++;
if(filter_cnt==FILTER_NUM) filter_cnt=0;
//GYRO_I.X += MPU6050_GYRO_LAST.X*Gyro_G*0.001;//0.001是时间间隔,两次prepare的执行周期
//GYRO_I.Y += MPU6050_GYRO_LAST.Y*Gyro_G*0.001;//这里已经使用四元数进行结算，因此不用直接累加
GYRO_I.Z += MPU6050_GYRO_LAST.Z*Gyro_G*0.001;
}
void Get_Attitude(void)
{
IMUupdate(MPU6050_GYRO_LAST.X*Gyro_Gr,
MPU6050_GYRO_LAST.Y*Gyro_Gr,
MPU6050_GYRO_LAST.Z*Gyro_Gr,
ACC_AVG.X,ACC_AVG.Y,ACC_AVG.Z); //*0.0174转成弧度=π%180
}
#define Kp 10.0f // proportional gain governs rate of convergence to accelerometer/magnetometer
#define Ki 0.008f // integral gain governs rate of convergence of gyroscope biases
#define halfT 0.001f // half the sample period采样周期的一半
float q0 = 1, q1 = 0, q2 = 0, q3 = 0; // quaternion elements representing the estimated orientation
float exInt = 0, eyInt = 0, ezInt = 0; // scaled integral error
void IMUupdate(float gx, float gy, float gz, float ax, float ay, float az)
{
float norm;
// float hx, hy, hz, bx, bz;
float vx, vy, vz;// wx, wy, wz;
float ex, ey, ez;
// 先把这些用得到的值算好
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 = sqrt(ax*ax + ay*ay + az*az); //acc数据归一化
ax = ax /norm;
ay = ay / norm;
az = az / norm;
// estimated direction of gravity and flux (v and w) 估计重力方向和流量/变迁
vx = 2*(q1q3 - q0q2); //四元素中xyz的表示
vy = 2*(q0q1 + q2q3);
vz = q0q0 - q1q1 - q2q2 + q3q3 ;
// error is sum of cross product between reference direction of fields and direction measured by sensors
ex = (ay*vz - az*vy) ; //向量外积在相减得到差分就是误差
ey = (az*vx - ax*vz) ;
ez = (ax*vy - ay*vx) ;
exInt = exInt + ex * Ki; //对误差进行积分
eyInt = eyInt + ey * Ki;
ezInt = ezInt + ez * Ki;
// adjusted gyroscope measurements
gx = gx + Kp*ex + exInt; //将误差PI后补偿到陀螺仪，即补偿零点漂移
gy = gy + Kp*ey + eyInt;
gz = gz + Kp*ez + ezInt; //这里的gz由于没有观测者进行矫正会产生漂移，表现出来的就是积分自增或自减
// integrate quaternion rate and normalise //四元素的微分方程
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;
// normalise quaternion
norm = sqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3);
q0 = q0 / norm;
q1 = q1 / norm;
q2 = q2 / norm;
q3 = q3 / norm;
Q_ANGLE.Z = GYRO_I.Z;//atan2(2 * q1 * q2 + 2 * q0 * q3, -2 * q2*q2 - 2 * q3* q3 + 1)* 57.3; // yaw
Q_ANGLE.Y = asin(-2 * q1 * q3 + 2 * q0* q2)* 57.3; // pitch
Q_ANGLE.X = atan2(2 * q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 - 2 * q2* q2 + 1)* 57.3; // roll
}#include "MOTO.h"
int16_t MOTO1_PWM = 0;
int16_t MOTO2_PWM = 0;
int16_t MOTO3_PWM = 0;
int16_t MOTO4_PWM = 0;
void Moto_PwmRflash(int16_t MOTO1_PWM,int16_t MOTO2_PWM,int16_t MOTO3_PWM,int16_t MOTO4_PWM)
{
if(MOTO1_PWM>Moto_PwmMax) MOTO1_PWM = Moto_PwmMax;
if(MOTO2_PWM>Moto_PwmMax) MOTO2_PWM = Moto_PwmMax;
if(MOTO3_PWM>Moto_PwmMax) MOTO3_PWM = Moto_PwmMax;
if(MOTO4_PWM>Moto_PwmMax) MOTO4_PWM = Moto_PwmMax;
if(MOTO1_PWM<0) MOTO1_PWM = 0;
if(MOTO2_PWM<0) MOTO2_PWM = 0;
if(MOTO3_PWM<0) MOTO3_PWM = 0;
if(MOTO4_PWM<0) MOTO4_PWM = 0;
TIM4->CCR1 =MOTO1_PWM;
TIM4->CCR2 = MOTO2_PWM;
TIM4->CCR3 = MOTO3_PWM;
TIM4->CCR4 = MOTO4_PWM;
}
void Tim4_init(void)
{
TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure;
TIM_OCInitTypeDef TIM_OCInitStructure;
uint16_t PrescalerValue = 0;
/* -----------------------------------------------------------------------
TIM3 Configuration（配置）: generate 4 PWM signals with 4 different duty cycles（占空比）:
The TIM3CLK frequency（频率） is set to SystemCoreClock (Hz), to get TIM3 counter
clock at 24 MHz the Prescaler is computed(计算） as following:
- Prescaler = (TIM3CLK / TIM3 counter clock) - 1
SystemCoreClock is set to 72 MHz for Low-density（密度）, Medium-density, High-density
and Connectivity（连通性） line devices and to 24 MHz for Low-Density Value line and
Medium-Density Value line devices
The TIM3 is running at 36 KHz: TIM3 Frequency = TIM3 counter clock/(ARR + 1)
= 24 MHz / 1000 = 24 KHz
TIM3 Channel1 duty cycle = (TIM3_CCR1/ TIM3_ARR)* 100 = 50%
TIM3 Channel2 duty cycle = (TIM3_CCR2/ TIM3_ARR)* 100 = 37.5%
TIM3 Channel3 duty cycle = (TIM3_CCR3/ TIM3_ARR)* 100 = 25%
TIM3 Channel4 duty cycle = (TIM3_CCR4/ TIM3_ARR)* 100 = 12.5%
----------------------------------------------------------------------- */
RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM4, ENABLE);//TIM3和TIM4同在APB1时钟下
/* Compute the prescaler value */
PrescalerValue = (uint16_t) (SystemCoreClock / 24000000) - 1;
/* Time base configuration */
TIM_TimeBaseStructure.TIM_Period = 999; //计数上线 ，CNT=999时中断计数器并结束一个周期，进行下一个从零开始的周期
TIM_TimeBaseStructure.TIM_Prescaler = PrescalerValue; //pwm时钟分频 72MHz多大分频分成24MHz
TIM_TimeBaseStructure.TIM_ClockDivision = 0;
TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up; //向上计数
TIM_TimeBaseInit(TIM4, &TIM_TimeBaseStructure);
/* PWM1 Mode configuration: Channel */
TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_PWM1;//CNT<CCR1时为有效电平
TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable;
TIM_OCInitStructure.TIM_Pulse = 0;//初始占空比为0
TIM_OCInitStructure.TIM_OCPolarity = TIM_OCPolarity_High;//低电平有效
TIM_OC1Init(TIM4, &TIM_OCInitStructure);
TIM_OC1PreloadConfig(TIM4, TIM_OCPreload_Enable);
TIM_OC2Init(TIM4, &TIM_OCInitStructure);
TIM_OC2PreloadConfig(TIM4, TIM_OCPreload_Enable);
TIM_OC3Init(TIM4, &TIM_OCInitStructure);
TIM_OC3PreloadConfig(TIM4, TIM_OCPreload_Enable);
TIM_OC4Init(TIM4, &TIM_OCInitStructure);
TIM_OC4PreloadConfig(TIM4, TIM_OCPreload_Enable);
TIM_ARRPreloadConfig(TIM4, ENABLE);
TIM_Cmd(TIM4, ENABLE);
}
void Moto_Init(void)
{
GPIO_InitTypeDef GPIO_InitStructure;
//使能电机用的时钟
RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB , ENABLE);
//设置电机使用到得管脚
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_9 | GPIO_Pin_6 | GPIO_Pin_7 | GPIO_Pin_8 ;
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_PP;
GPIO_Init(GPIOB, &GPIO_InitStructure);
Tim4_init();
}#include "MPU6050.h"
#include "Tim.h"
u8 mpu6050_buffer[14]; //iic读取后存放数据
S_INT16_XYZ GYRO_OFFSET,ACC_OFFSET; //零漂
u8 GYRO_OFFSET_OK = 1;
u8 ACC_OFFSET_OK = 1;
S_INT16_XYZ MPU6050_ACC_LAST,MPU6050_GYRO_LAST; //最新一次读取值
/**************************实现函数*****************************************************
将iic读取到得数据分拆,放入相应寄存器
如果上位机要计算零漂，便发回OFFSET数据
****************************************************************************************/
void MPU6050_Dataanl(void)
{
if(!GYRO_OFFSET_OK)//没有计算零偏,一开始不会计算零漂，
//只有上位机将GYRO_OFFSET_OK置零后才将其置零
{
static int32_t tempgx=0,tempgy=0,tempgz=0;
static uint8_t cnt_g=0;
// LED1_ON;
if(cnt_g==0)
{
GYRO_OFFSET.X=0;
GYRO_OFFSET.Y=0;
GYRO_OFFSET.Z=0;
tempgx = 0;
tempgy = 0;
tempgz = 0;
cnt_g = 1;
return;
}
tempgx+= MPU6050_GYRO_LAST.X;
tempgy+= MPU6050_GYRO_LAST.Y;
tempgz+= MPU6050_GYRO_LAST.Z;
if(cnt_g==200)
{
GYRO_OFFSET.X=tempgx/cnt_g;//累加200次取平均值
GYRO_OFFSET.Y=tempgy/cnt_g;
GYRO_OFFSET.Z=tempgz/cnt_g;
cnt_g = 0;
GYRO_OFFSET_OK = 1;
// EE_SAVE_GYRO_OFFSET();//保存数据
return;
}
cnt_g++;
}
if(!ACC_OFFSET_OK)
{
static int32_t tempax=0,tempay=0,tempaz=0;
static uint8_t cnt_a=0;
// LED1_ON;
if(cnt_a==0)
{
ACC_OFFSET.X = 0;
ACC_OFFSET.Y = 0;
ACC_OFFSET.Z = 0;
tempax = 0;
tempay = 0;
tempaz = 0;
cnt_a = 1;
return;
}
tempax+= MPU6050_ACC_LAST.X;
tempay+= MPU6050_ACC_LAST.Y;
//tempaz+= MPU6050_ACC_LAST.Z;
if(cnt_a==200)
{
ACC_OFFSET.X=tempax/cnt_a;
ACC_OFFSET.Y=tempay/cnt_a;
ACC_OFFSET.Z=tempaz/cnt_a;
cnt_a = 0;
ACC_OFFSET_OK = 1;
// EE_SAVE_ACC_OFFSET();//保存数据
return;
}
cnt_a++;
}
}
/***************** 底层的读取MPU6050_ACC_LAST,MPU6050_GYRO_LAST 数据 *****************/
void MPU6050_Read(void)
{
MPU_Get_Accelerometer(&MPU6050_ACC_LAST.X ,&MPU6050_ACC_LAST.Y ,&MPU6050_ACC_LAST.Z ); //得到加速度传感器数据
MPU_Get_Gyroscope(&MPU6050_GYRO_LAST.X ,&MPU6050_GYRO_LAST.Y ,&MPU6050_GYRO_LAST.Z ); //得到陀螺仪数据
}
/************************** 初始化 MPU6050 以进入可用状态 *****************************/
u8 MPU6050_Init(void)
{
u8 res;
MPU_IIC_Init();//初始化IIC总线
MPU_Write_Byte(MPU_PWR_MGMT1_REG,0X80); //复位MPU6050
delay_ms(100);
MPU_Write_Byte(MPU_PWR_MGMT1_REG,0X00); //唤醒MPU6050
MPU_Set_Gyro_Fsr(3); //陀螺仪传感器,±2000dps
MPU_Set_Accel_Fsr(0); //加速度传感器,±2g
MPU_Set_Rate(50); //设置采样率50Hz
MPU_Write_Byte(MPU_INT_EN_REG,0X00); //关闭所有中断
MPU_Write_Byte(MPU_USER_CTRL_REG,0X00); //I2C主模式关闭
MPU_Write_Byte(MPU_FIFO_EN_REG,0X00); //关闭FIFO
MPU_Write_Byte(MPU_INTBP_CFG_REG,0X80); //INT引脚低电平有效
res=MPU_Read_Byte(MPU_DEVICE_ID_REG);
if(res==MPU_ADDR)//器件ID正确
{
MPU_Write_Byte(MPU_PWR_MGMT1_REG,0X01); //设置CLKSEL,PLL X轴为参考
MPU_Write_Byte(MPU_PWR_MGMT2_REG,0X00); //加速度与陀螺仪都工作
MPU_Set_Rate(50); //设置采样率为50Hz
}else return 1;
return 0;
}
/********************** 设置MPU6050的传感器 ********************************/
//陀螺仪传感器满量程范围
//fsr:0,±250dps;1,±500dps;2,±1000dps;3,±2000dps
//返回值:0,设置成功
// 其他,设置失败
u8 MPU_Set_Gyro_Fsr(u8 fsr)
{
return MPU_Write_Byte(MPU_GYRO_CFG_REG,fsr<<3);//设置陀螺仪满量程范围
}
//设置MPU6050加速度传感器满量程范围
//fsr:0,±2g;1,±4g;2,±8g;3,±16g
//返回值:0,设置成功
// 其他,设置失败
u8 MPU_Set_Accel_Fsr(u8 fsr)
{
return MPU_Write_Byte(MPU_ACCEL_CFG_REG,fsr<<3);//设置加速度传感器满量程范围
}
//设置MPU6050的数字低通滤波器
//lpf:数字低通滤波频率(Hz)
//返回值:0,设置成功
// 其他,设置失败
u8 MPU_Set_LPF(u16 lpf)
{
u8 data=0;
if(lpf>=188)data=1;
else if(lpf>=98)data=2;
else if(lpf>=42)data=3;
else if(lpf>=20)data=4;
else if(lpf>=10)data=5;
else data=6;
return MPU_Write_Byte(MPU_CFG_REG,data);//设置数字低通滤波器
}
//设置MPU6050的采样率(假定Fs=1KHz)
//rate:4~1000(Hz)
//返回值:0,设置成功
// 其他,设置失败
u8 MPU_Set_Rate(u16 rate)
{
u8 data;
if(rate>1000)rate=1000;
if(rate<4)rate=4;
data=1000/rate-1;
data=MPU_Write_Byte(MPU_SAMPLE_RATE_REG,data); //设置数字低通滤波器
return MPU_Set_LPF(rate/2); //自动设置LPF为采样率的一半
}
/*********************** 读取六轴以及温度原始数据,并减去零偏 **************************/
//得到温度值
//返回值:温度值(扩大了100倍)
short MPU_Get_Temperature(void)
{
u8 buf[2];
short raw;
float temp;
MPU_Read_Len(MPU_ADDR,MPU_TEMP_OUTH_REG,2,buf);
raw=((u16)buf[0]<<8)|buf[1];
temp=36.53+((double)raw)/340;
return temp*100;;
}
//得到陀螺仪值(原始值)
//gx,gy,gz:陀螺仪x,y,z轴的原始读数(带符号)
//返回值:0,成功
// 其他,错误代码
u8 MPU_Get_Gyroscope(short *gx,short *gy,short *gz)
{
u8 buf[6],res;
res=MPU_Read_Len(MPU_ADDR,MPU_GYRO_XOUTH_REG,6,buf);
if(res==0)
{
*gx=(((u16)buf[0]<<8)|buf[1])-GYRO_OFFSET.X;
*gy=(((u16)buf[2]<<8)|buf[3])-GYRO_OFFSET.Y;
*gz=(((u16)buf[4]<<8)|buf[5])-GYRO_OFFSET.Z;
}
return res;;
}
//得到加速度值(原始值)
//gx,gy,gz:陀螺仪x,y,z轴的原始读数(带符号)
//返回值:0,成功
// 其他,错误代码
u8 MPU_Get_Accelerometer(short *ax,short *ay,short *az)
{
u8 buf[6],res;
res=MPU_Read_Len(MPU_ADDR,MPU_ACCEL_XOUTH_REG,6,buf);
if(res==0)
{
*ax=(((u16)buf[0]<<8)|buf[1])-ACC_OFFSET.X;
*ay=(((u16)buf[2]<<8)|buf[3])-ACC_OFFSET.Y;
*az=(((u16)buf[4]<<8)|buf[5])-ACC_OFFSET.Z;
}
return res;;
}
/*************************** IIC读写器件寄存器函数 ******************************************/
//IIC连续写
//addr:器件地址
//reg:寄存器地址
//len:写入长度
//buf:数据区
//返回值:0,正常
// 其他,错误代码
u8 MPU_Write_Len(u8 addr,u8 reg,u8 len,u8 *buf)
{
u8 i;
MPU_IIC_Start();
MPU_IIC_Send_Byte((addr<<1)|0);//发送器件地址+写命令
if(MPU_IIC_Wait_Ack()) //等待应答
{
MPU_IIC_Stop();
return 1;
}
MPU_IIC_Send_Byte(reg); //写寄存器地址
MPU_IIC_Wait_Ack(); //等待应答
for(i=0;i<len;i++)
{
MPU_IIC_Send_Byte(buf[i]); //发送数据
if(MPU_IIC_Wait_Ack()) //等待ACK
{
MPU_IIC_Stop();
return 1;
}
}
MPU_IIC_Stop();
return 0;
}
//IIC连续读
//addr:器件地址
//reg:要读取的寄存器地址
//len:要读取的长度
//buf:读取到的数据存储区
//返回值:0,正常
// 其他,错误代码
u8 MPU_Read_Len(u8 addr,u8 reg,u8 len,u8 *buf)
{
MPU_IIC_Start();
MPU_IIC_Send_Byte((addr<<1)|0);//发送器件地址+写命令
if(MPU_IIC_Wait_Ack()) //等待应答
{
MPU_IIC_Stop();
return 1;
}
MPU_IIC_Send_Byte(reg); //写寄存器地址
MPU_IIC_Wait_Ack(); //等待应答
MPU_IIC_Start();
MPU_IIC_Send_Byte((addr<<1)|1);//发送器件地址+读命令
MPU_IIC_Wait_Ack(); //等待应答
while(len)
{ //可以连续读取6各数据，而不用写6个寄存器地址
if(len==1)*buf=MPU_IIC_Read_Byte(0);//读数据,发送nACK,直到此时读取完毕
else *buf=MPU_IIC_Read_Byte(1); //读数据,发送ACK,接着读取下一个地址的寄存器中的数据
len--;
buf++;
}
MPU_IIC_Stop(); //产生一个停止条件
return 0;
}
//IIC写一个字节
//reg:寄存器地址
//data:数据
//返回值:0,正常
// 其他,错误代码
u8 MPU_Write_Byte(u8 reg,u8 data)
{
MPU_IIC_Start();
MPU_IIC_Send_Byte((MPU_ADDR<<1)|0);//发送器件地址+写命令
if(MPU_IIC_Wait_Ack()) //等待应答
{
MPU_IIC_Stop();
return 1;
}
MPU_IIC_Send_Byte(reg); //写寄存器地址
MPU_IIC_Wait_Ack(); //等待应答
MPU_IIC_Send_Byte(data);//发送数据
if(MPU_IIC_Wait_Ack()) //等待ACK
{
MPU_IIC_Stop();
return 1;
}
MPU_IIC_Stop();
return 0;
}
//IIC读一个字节
//reg:寄存器地址
//返回值:读到的数据
u8 MPU_Read_Byte(u8 reg)
{
u8 res;
MPU_IIC_Start();
MPU_IIC_Send_Byte((MPU_ADDR<<1)|0);//发送器件地址+写命令
MPU_IIC_Wait_Ack(); //等待应答
MPU_IIC_Send_Byte(reg); //写寄存器地址
MPU_IIC_Wait_Ack(); //等待应答
MPU_IIC_Start();
MPU_IIC_Send_Byte((MPU_ADDR<<1)|1);//发送器件地址+读命令
MPU_IIC_Wait_Ack(); //等待应答
res=MPU_IIC_Read_Byte(0);//读取数据,发送nACK
MPU_IIC_Stop(); //产生一个停止条件
return res;
}#include "MPU6050iic.h"
/******************************* 延时函数 *************************************/
static u8 fac_us=0;//us延时倍乘数
static u16 fac_ms=0;//ms延时倍乘数
void delay_init()
{
SysTick_CLKSourceConfig(SysTick_CLKSource_HCLK_Div8); //选择外部时钟 HCLK/8
fac_us=SystemCoreClock/8000000; //为系统时钟的1/8
fac_ms=(u16)fac_us*1000;//非ucos下,代表每个ms需要的systick时钟数
}
void delay_ms(u16 nms)
{
u32 temp;
SysTick->LOAD=(u32)nms*fac_ms;//时间加载(SysTick->LOAD为24bit)
SysTick->VAL =0x00; //清空计数器
SysTick->CTRL|=SysTick_CTRL_ENABLE_Msk ; //开始倒数
do
{
temp=SysTick->CTRL;
}
while(temp&0x01&&!(temp&(1<<16)));//等待时间到达
SysTick->CTRL&=~SysTick_CTRL_ENABLE_Msk; //关闭计数器
SysTick->VAL =0X00; //清空计数器
}
//MPU IIC 延时函数
void MPU_IIC_Delay(void)//2us
{
u32 temp;
SysTick_CLKSourceConfig(SysTick_CLKSource_HCLK_Div8); //选择外部时钟 HCLK/8
SysTick->LOAD=2*fac_us; //时间加载,2us
SysTick->VAL=0x00; //清空计数器
SysTick->CTRL|=SysTick_CTRL_ENABLE_Msk ; //开始倒数
do
{
temp=SysTick->CTRL;
}
while(temp&0x01&&!(temp&(1<<16)));//等待时间到达
SysTick->CTRL&=~SysTick_CTRL_ENABLE_Msk; //关闭计数器
SysTick->VAL =0X00; //清空计数器
}
/******************************* 初始化IIC的IO口 ***************************************/
void MPU_IIC_Init(void)
{
GPIO_InitTypeDef GPIO_InitStructure;
RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB,ENABLE);//先使能外设IO PORTC时钟
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_10|GPIO_Pin_11; // 端口配置
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP; //推挽输出，可输出高低电平，连接数字器件
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz; //IO口速度为50MHz
GPIO_Init(GPIOB, &GPIO_InitStructure); //根据设定参数初始化GPIO
GPIO_SetBits(GPIOB,GPIO_Pin_10|GPIO_Pin_11); //PB10,PB11 输出高
}
/*************************** IIC底层读写的时序函数 **************************************/
//产生IIC起始信号
void MPU_IIC_Start(void)
{
MPU_SDA_OUT(); //sda线输出
MPU_IIC_SDA=1;
MPU_IIC_SCL=1;
MPU_IIC_Delay();
MPU_IIC_SDA=0;//START:when CLK is high,DATA change form high to low
MPU_IIC_Delay();
MPU_IIC_SCL=0;//钳住I2C总线，准备发送或接收数据
}
//产生IIC停止信号
void MPU_IIC_Stop(void)
{
MPU_SDA_OUT();//sda线输出
MPU_IIC_SCL=0;
MPU_IIC_SDA=0;//STOP:when CLK is high DATA change form low to high
MPU_IIC_Delay();
MPU_IIC_SCL=1;
MPU_IIC_SDA=1;//发送I2C总线结束信号
MPU_IIC_Delay();
}
//等待应答信号到来
//返回值：1，接收应答失败
// 0，接收应答成功
u8 MPU_IIC_Wait_Ack(void)
{
u8 ucErrTime=0;
MPU_SDA_IN(); //SDA设置为输入
MPU_IIC_SDA=1;MPU_IIC_Delay();
MPU_IIC_SCL=1;MPU_IIC_Delay();
while(MPU_READ_SDA)
{
ucErrTime++;
if(ucErrTime>250)
{
MPU_IIC_Stop();
return 1;
}
}
MPU_IIC_SCL=0;//时钟输出0
return 0;
}
//产生ACK应答
void MPU_IIC_Ack(void)
{
MPU_IIC_SCL=0;
MPU_SDA_OUT();
MPU_IIC_SDA=0;
MPU_IIC_Delay();
MPU_IIC_SCL=1;
MPU_IIC_Delay();
MPU_IIC_SCL=0;
}
//不产生ACK应答
void MPU_IIC_NAck(void)
{
MPU_IIC_SCL=0;
MPU_SDA_OUT();
MPU_IIC_SDA=1;
MPU_IIC_Delay();
MPU_IIC_SCL=1;
MPU_IIC_Delay();
MPU_IIC_SCL=0;
}
//IIC发送一个字节
//返回从机有无应答
//1，有应答
//0，无应答
void MPU_IIC_Send_Byte(u8 txd)
{
u8 t;
MPU_SDA_OUT();
MPU_IIC_SCL=0;//拉低时钟开始数据传输
for(t=0;t<8;t++)
{
MPU_IIC_SDA=(txd&0x80)>>7;
txd<<=1;
MPU_IIC_SCL=1;
MPU_IIC_Delay();
MPU_IIC_SCL=0;
MPU_IIC_Delay();
}
}
//读1个字节，ack=1时，发送ACK，ack=0，发送nACK
u8 MPU_IIC_Read_Byte(unsigned char ack)
{
unsigned char i,receive=0;
MPU_SDA_IN();//SDA设置为输入
for(i=0;i<8;i++ )
{
MPU_IIC_SCL=0;
MPU_IIC_Delay();
MPU_IIC_SCL=1;
receive<<=1;
if(MPU_READ_SDA)receive++;
MPU_IIC_Delay();
}
if (!ack)
MPU_IIC_NAck();//发送nACK
else
MPU_IIC_Ack(); //发送ACK
return receive;
}#include "rc.h"
#include "BSP.h"
//#include "app/uart/uart1.h"
//#include "app/imu/imu.h"
//#include "app/control/control.h"
#define BYTE0(dwTemp) (*(char *)(&dwTemp))
#define BYTE1(dwTemp) (*((char *)(&dwTemp) + 1))
#define BYTE2(dwTemp) (*((char *)(&dwTemp) + 2))
#define BYTE3(dwTemp) (*((char *)(&dwTemp) + 3))
#define RX_DR 6 //中断标志
#define TX_DS 5
#define MAX_RT 4
RC_GETDATA Rc_Get;//接收到的RC数据,1000~2000
float RC_Target_ROL=0,RC_Target_PIT=0,RC_Target_YAW=0;
void RC_FUN(void)
{
//判断解锁
static u16 _armed_cnt1 = 0,_armed_cnt2 = 0;
if(ARMED==0 && Rc_Get.THROTTLE < 1100 && Rc_Get.YAW <1100)
_armed_cnt1++;
else
_armed_cnt1=0;
if(_armed_cnt1>100) ARMED = 1; //解锁
if(ARMED==1 && Rc_Get.THROTTLE <1100 && Rc_Get.YAW >1900)
_armed_cnt2++;
else
_armed_cnt2=0;
if(_armed_cnt2>100) ARMED = 0;
}
void HC05_DataAnl(void)
{
u8 sum = 0;
for(u8 i=0;i<31;i++)
sum += NRF24L01_RXDATA[i];
if(!(sum==NRF24L01_RXDATA[31])) return; //判断sum，校验和
if(!(NRF24L01_RXDATA[0]==0x8A)) return; //判断帧头
if(NRF24L01_RXDATA[1]==0x8A) //判断功能字,=0x8a,为遥控数据
{
Rc_Get.THROTTLE = (vs16)(NRF24L01_RXDATA[3]<<8)|NRF24L01_RXDATA[4];//数据合并,一次接受八位数据，
Rc_Get.YAW = (vs16)(NRF24L01_RXDATA[5]<<8)|NRF24L01_RXDATA[6];//但是需要合成16位
Rc_Get.ROLL = (vs16)(NRF24L01_RXDATA[7]<<8)|NRF24L01_RXDATA[8];
Rc_Get.PITCH = (vs16)(NRF24L01_RXDATA[9]<<8)|NRF24L01_RXDATA[10];
Rc_Get.AUX1 = (vs16)(NRF24L01_RXDATA[11]<<8)|NRF24L01_RXDATA[12];
Rc_Get.AUX2 = (vs16)(NRF24L01_RXDATA[13]<<8)|NRF24L01_RXDATA[14];
Rc_Get.AUX3 = (vs16)(NRF24L01_RXDATA[15]<<8)|NRF24L01_RXDATA[16];
Rc_Get.AUX4 = (vs16)(NRF24L01_RXDATA[17]<<8)|NRF24L01_RXDATA[18];
Rc_Get.AUX5 = (vs16)(NRF24L01_RXDATA[19]<<8)|NRF24L01_RXDATA[20];
RC_FUN();
RC_Target_ROL = (Rc_Get.ROLL-1500)/30; //一打摇杆偏得很可以将30改大
RC_Target_PIT = (Rc_Get.PITCH-1500)/30;
RC_Target_YAW = (Rc_Get.YAW-1500)/30;
}
if(NRF24L01_RXDATA[1]==0X8B) //判断功能字,=0x8B,为控制数据
{
if(NRF24L01_RXDATA[3]==0xAA) //校准传感器
{
if(NRF24L01_RXDATA[4]==0xA2) GYRO_OFFSET_OK = 0;
if(NRF24L01_RXDATA[4]==0xA1) ACC_OFFSET_OK = 0;
if(NRF24L01_RXDATA[4]==0xA3) {GYRO_OFFSET_OK = 0;ACC_OFFSET_OK = 0;}
}
if(NRF24L01_RXDATA[3]==0xA0) {ARMED = 0;NRF_Send_ARMED();}
if(NRF24L01_RXDATA[3]==0xA1) {ARMED = 1;NRF_Send_ARMED();}
if(NRF24L01_RXDATA[3]==0xAB) //接收OFFSET
{
ACC_OFFSET.X = (NRF24L01_RXDATA[4]<<8) + NRF24L01_RXDATA[5];
ACC_OFFSET.Y = (NRF24L01_RXDATA[6]<<8) + NRF24L01_RXDATA[7];
EE_SAVE_ACC_OFFSET();
//EE_SAVE_GYRO_OFFSET();
}
if(NRF24L01_RXDATA[3]==0xAC) NRF_Send_OFFSET();
if(NRF24L01_RXDATA[3]==0xAD) NRF_Send_PID();
if(NRF24L01_RXDATA[3]==0xAE) //接收PID
{
PID_ROL.P = (float)((vs16)(NRF24L01_RXDATA[4]<<8)|NRF24L01_RXDATA[5])/100;
PID_ROL.I = (float)((vs16)(NRF24L01_RXDATA[6]<<8)|NRF24L01_RXDATA[7])/100;
PID_ROL.D = (float)((vs16)(NRF24L01_RXDATA[8]<<8)|NRF24L01_RXDATA[9])/100;
PID_PIT.P = (float)((vs16)(NRF24L01_RXDATA[10]<<8)|NRF24L01_RXDATA[11])/100;
PID_PIT.I = (float)((vs16)(NRF24L01_RXDATA[12]<<8)|NRF24L01_RXDATA[13])/100;
PID_PIT.D = (float)((vs16)(NRF24L01_RXDATA[14]<<8)|NRF24L01_RXDATA[15])/100;
PID_YAW.P = (float)((vs16)(NRF24L01_RXDATA[16]<<8)|NRF24L01_RXDATA[17])/100;
PID_YAW.I = (float)((vs16)(NRF24L01_RXDATA[18]<<8)|NRF24L01_RXDATA[19])/100;
PID_YAW.D = (float)((vs16)(NRF24L01_RXDATA[20]<<8)|NRF24L01_RXDATA[21])/100;
EE_SAVE_PID();
}
}
}
void Nrf_Check_Event(void)
{
u8 sta = NRF_Read_Reg(NRF_READ_REG + NRFRegSTATUS);//读取状态寄存器
if(sta & (1<<RX_DR))//检验sta是不是接收中断
{
u8 rx_len = NRF_Read_Reg(R_RX_PL_WID);
if(rx_len<33)
{
NRF_Read_Buf(RD_RX_PLOAD,NRF24L01_RXDATA,RX_PLOAD_WIDTH);// read receive payload from RX_FIFO buffer
NRF_DataAnl();//进行数据分析
LED1_ONOFF();
}
else//当前数据有误
{
NRF_Write_Reg(FLUSH_RX,0xff);//清空缓冲区
}
}
if(sta & (1<<TX_DS))
{
LED1_ONOFF();
}
static uint8_t led2_state = 0;
if(sta & (1<<MAX_RT))//??????????
{
if(led2_state)
{
LED2_OFF;
led2_state = 0;
}
else
{
LED2_ON;
led2_state = 1;
}
if(sta & 0x01) //TX FIFO FULL
{
NRF_Write_Reg(FLUSH_TX,0xff);
}
}
NRF_Write_Reg(NRF_WRITE_REG + NRFRegSTATUS, sta);
}
void NRF_Send_AF(void)
{
uint8_t i,sum;
uint16_t _temp;
NRF24L01_TXDATA[0]=0x88;
NRF24L01_TXDATA[1]=0xAF;
NRF24L01_TXDATA[2]=0x1C;
NRF24L01_TXDATA[3]=BYTE1(MPU6050_ACC_LAST.X);
NRF24L01_TXDATA[4]=BYTE0(MPU6050_ACC_LAST.X);
NRF24L01_TXDATA[5]=BYTE1(MPU6050_ACC_LAST.Y);
NRF24L01_TXDATA[6]=BYTE0(MPU6050_ACC_LAST.Y);
NRF24L01_TXDATA[7]=BYTE1(MPU6050_ACC_LAST.Z);
NRF24L01_TXDATA[8]=BYTE0(MPU6050_ACC_LAST.Z);
NRF24L01_TXDATA[9]=BYTE1(MPU6050_GYRO_LAST.X);
NRF24L01_TXDATA[10]=BYTE0(MPU6050_GYRO_LAST.X);
NRF24L01_TXDATA[11]=BYTE1(MPU6050_GYRO_LAST.Y);
NRF24L01_TXDATA[12]=BYTE0(MPU6050_GYRO_LAST.Y);
NRF24L01_TXDATA[13]=BYTE1(MPU6050_GYRO_LAST.Z);
NRF24L01_TXDATA[14]=BYTE0(MPU6050_GYRO_LAST.Z);
NRF24L01_TXDATA[15]=0;
NRF24L01_TXDATA[16]=0;
NRF24L01_TXDATA[17]=0;
NRF24L01_TXDATA[18]=0;
NRF24L01_TXDATA[19]=0;
NRF24L01_TXDATA[20]=0;
_temp = (int)(Q_ANGLE.X*100);
NRF24L01_TXDATA[21]=BYTE1(_temp);
NRF24L01_TXDATA[22]=BYTE0(_temp);
_temp = (int)(Q_ANGLE.Y*100);
NRF24L01_TXDATA[23]=BYTE1(_temp);
NRF24L01_TXDATA[24]=BYTE0(_temp);
_temp = (int)(Q_ANGLE.Z*10);
NRF24L01_TXDATA[25]=BYTE1(_temp);
NRF24L01_TXDATA[26]=BYTE0(_temp);
if(ARMED==0) NRF24L01_TXDATA[27]=0xA0;
else if(ARMED==1) NRF24L01_TXDATA[27]=0xA1;
sum = 0;
for(i=0;i<31;i++)
sum += NRF24L01_TXDATA[i];
NRF24L01_TXDATA[31]=sum;
NRF_TxPacket(NRF24L01_TXDATA,32);
}
void NRF_Send_AE(void)
{
uint8_t i,sum;
uint16_t _temp;
NRF24L01_TXDATA[0]=0x88;
NRF24L01_TXDATA[1]=0xAE;
NRF24L01_TXDATA[2]=0x1C;
NRF24L01_TXDATA[3]=BYTE1(Rc_Get.THROTTLE);
NRF24L01_TXDATA[4]=BYTE0(Rc_Get.THROTTLE);
NRF24L01_TXDATA[5]=BYTE1(Rc_Get.YAW);
NRF24L01_TXDATA[6]=BYTE0(Rc_Get.YAW);
NRF24L01_TXDATA[7]=BYTE1(Rc_Get.ROLL);
NRF24L01_TXDATA[8]=BYTE0(Rc_Get.ROLL);
NRF24L01_TXDATA[9]=BYTE1(Rc_Get.PITCH);
NRF24L01_TXDATA[10]=BYTE0(Rc_Get.PITCH);
NRF24L01_TXDATA[11]=BYTE1(Rc_Get.AUX1);
NRF24L01_TXDATA[12]=BYTE0(Rc_Get.AUX1);
NRF24L01_TXDATA[13]=BYTE1(Rc_Get.AUX2);
NRF24L01_TXDATA[14]=BYTE0(Rc_Get.AUX2);
NRF24L01_TXDATA[15]=BYTE1(Rc_Get.AUX3);
NRF24L01_TXDATA[16]=BYTE0(Rc_Get.AUX3);
NRF24L01_TXDATA[17]=BYTE1(Rc_Get.AUX4);
NRF24L01_TXDATA[18]=BYTE0(Rc_Get.AUX4);
NRF24L01_TXDATA[19]=BYTE1(Rc_Get.AUX5);
NRF24L01_TXDATA[20]=BYTE0(Rc_Get.AUX5);
_temp = TIM2->CCR1/10;
NRF24L01_TXDATA[21]=BYTE1(_temp);
NRF24L01_TXDATA[22]=BYTE0(_temp);
_temp = TIM2->CCR2/10;
NRF24L01_TXDATA[23]=BYTE1(_temp);
NRF24L01_TXDATA[24]=BYTE0(_temp);
_temp = TIM2->CCR3/10;
NRF24L01_TXDATA[25]=BYTE1(_temp);
NRF24L01_TXDATA[26]=BYTE0(_temp);
_temp = TIM2->CCR4/10;
NRF24L01_TXDATA[27]=BYTE1(_temp);
NRF24L01_TXDATA[28]=BYTE0(_temp);
_temp = ADC_ConvertedValue/6;
NRF24L01_TXDATA[29]=BYTE1(_temp);
NRF24L01_TXDATA[30]=BYTE0(_temp);
sum = 0;
for(i=0;i<31;i++)
sum += NRF24L01_TXDATA[i];
NRF24L01_TXDATA[31]=sum;
NRF_TxPacket(NRF24L01_TXDATA,32);
}
void NRF_Send_OFFSET(void)
{
uint8_t i,sum;
NRF24L01_TXDATA[0]=0x88;
NRF24L01_TXDATA[1]=0xAC;
NRF24L01_TXDATA[2]=0x1C;
NRF24L01_TXDATA[3]=0xAC;
NRF24L01_TXDATA[4]=BYTE1(ACC_OFFSET.X);
NRF24L01_TXDATA[5]=BYTE0(ACC_OFFSET.X);
NRF24L01_TXDATA[6]=BYTE1(ACC_OFFSET.Y);
NRF24L01_TXDATA[7]=BYTE0(ACC_OFFSET.Y);
NRF24L01_TXDATA[8]=BYTE1(ACC_OFFSET.Z);
NRF24L01_TXDATA[9]=BYTE0(ACC_OFFSET.Z);
NRF24L01_TXDATA[10]=BYTE1(GYRO_OFFSET.X);
NRF24L01_TXDATA[11]=BYTE0(GYRO_OFFSET.X);
NRF24L01_TXDATA[12]=BYTE1(GYRO_OFFSET.Y);
NRF24L01_TXDATA[13]=BYTE0(GYRO_OFFSET.Y);
NRF24L01_TXDATA[14]=BYTE1(GYRO_OFFSET.Z);
NRF24L01_TXDATA[15]=BYTE0(GYRO_OFFSET.Z);
sum = 0;
for(i=0;i<31;i++)
sum += NRF24L01_TXDATA[i];
NRF24L01_TXDATA[31]=sum;
NRF_TxPacket(NRF24L01_TXDATA,32);
}
void NRF_Send_PID(void)
{
uint8_t i,sum;
u16 _temp;
NRF24L01_TXDATA[0]=0x88;
NRF24L01_TXDATA[1]=0xAC;
NRF24L01_TXDATA[2]=0x1C;
NRF24L01_TXDATA[3]=0xAD;
_temp = PID_ROL.P * 100;
NRF24L01_TXDATA[4]=BYTE1(_temp);
NRF24L01_TXDATA[5]=BYTE0(_temp);
_temp = PID_ROL.I * 100;
NRF24L01_TXDATA[6]=BYTE1(_temp);
NRF24L01_TXDATA[7]=BYTE0(_temp);
_temp = PID_ROL.D * 100;
NRF24L01_TXDATA[8]=BYTE1(_temp);
NRF24L01_TXDATA[9]=BYTE0(_temp);
_temp = PID_PIT.P * 100;
NRF24L01_TXDATA[10]=BYTE1(_temp);
NRF24L01_TXDATA[11]=BYTE0(_temp);
_temp = PID_PIT.I * 100;
NRF24L01_TXDATA[12]=BYTE1(_temp);
NRF24L01_TXDATA[13]=BYTE0(_temp);
_temp = PID_PIT.D * 100;
NRF24L01_TXDATA[14]=BYTE1(_temp);
NRF24L01_TXDATA[15]=BYTE0(_temp);
_temp = PID_YAW.P * 100;
NRF24L01_TXDATA[16]=BYTE1(_temp);
NRF24L01_TXDATA[17]=BYTE0(_temp);
_temp = PID_YAW.I * 100;
NRF24L01_TXDATA[18]=BYTE1(_temp);
NRF24L01_TXDATA[19]=BYTE0(_temp);
_temp = PID_YAW.D * 100;
NRF24L01_TXDATA[20]=BYTE1(_temp);
NRF24L01_TXDATA[21]=BYTE0(_temp);
sum = 0;
for(i=0;i<31;i++)
sum += NRF24L01_TXDATA[i];
NRF24L01_TXDATA[31]=sum;
NRF_TxPacket(NRF24L01_TXDATA,32);
}
void NRF_Send_ARMED(void)//解锁信息
{
uint8_t i,sum;
NRF24L01_TXDATA[0]=0x88;
NRF24L01_TXDATA[1]=0xAC;
NRF24L01_TXDATA[2]=0x1C;
if(ARMED) NRF24L01_TXDATA[3]=0xA1;
else NRF24L01_TXDATA[3]=0xA0;
sum = 0;
for(i=0;i<31;i++)
sum += NRF24L01_TXDATA[i];
NRF24L01_TXDATA[31]=sum;
NRF_TxPacket(NRF24L01_TXDATA,32);
}
void NRF_SEND_test(void)
{
u8 sum;
static u8 temp=0;
temp++;
NRF24L01_TXDATA[0]=0x88;
NRF24L01_TXDATA[1]=0xA1;
NRF24L01_TXDATA[2]=28;
NRF24L01_TXDATA[3]=BYTE3(Q_ANGLE.X);
NRF24L01_TXDATA[4]=BYTE2(Q_ANGLE.X);
NRF24L01_TXDATA[5]=BYTE1(Q_ANGLE.X);
NRF24L01_TXDATA[6]=BYTE0(Q_ANGLE.X);
NRF24L01_TXDATA[7]=temp;
for(u8 i=0;i<31;i++)
sum += NRF24L01_TXDATA[i];
NRF24L01_TXDATA[31]=sum;
NRF_TxPacket(NRF24L01_TXDATA,32);
}#include "UART2.h"
#include "CONTROL.h"
#include "stm32f10x.h"
#include "Uart1.h"
#include "MPU6050iic.h"
RC_GETDATA Rc_Get;//接收到的RC数据,1000~2000
float RC_Target_ROL=0,RC_Target_PIT=0,RC_Target_YAW=0;
int arm=0;
/************************* 飞控与手机APP的通讯 **************************************/
/*********串口2的初始化函数***********/
void Uart2_Init(u32 br_num)//时钟低电平活动，禁用了同步通讯,因此只有Uart，即只是异步通讯
{
USART_InitTypeDef USART_InitStructure;
USART_ClockInitTypeDef USART_ClockInitStruct;
GPIO_InitTypeDef GPIO_InitStructure;
RCC_APB1PeriphClockCmd(RCC_APB1Periph_USART2, ENABLE); //开启USART1时钟
RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA,ENABLE);
//配置PA2作为USART1 Tx
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_2;
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_PP;
GPIO_Init(GPIOA , &GPIO_InitStructure);
//配置PA3作为USART1 Rx
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_3;
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IN_FLOATING;
GPIO_Init(GPIOA , &GPIO_InitStructure);
//配置USART1
//中断被屏蔽了
USART_InitStructure.USART_BaudRate = br_num; //波特率可以通过地面站配置
USART_InitStructure.USART_WordLength = USART_WordLength_8b; //8位数据
USART_InitStructure.USART_StopBits = USART_StopBits_1; //在帧结尾传输1个停止位
USART_InitStructure.USART_Parity = USART_Parity_No; //禁用奇偶校验
USART_InitStructure.USART_HardwareFlowControl = USART_HardwareFlowControl_None; //硬件流控制失能
USART_InitStructure.USART_Mode = USART_Mode_Tx | USART_Mode_Rx; //发送、接收使能
//配置USART1时钟
USART_ClockInitStruct.USART_Clock = USART_Clock_Disable; //时钟低电平活动
USART_ClockInitStruct.USART_CPOL = USART_CPOL_Low; //SLCK引脚上时钟输出的极性->低电平
USART_ClockInitStruct.USART_CPHA = USART_CPHA_2Edge; //时钟第二个边沿进行数据捕获
USART_ClockInitStruct.USART_LastBit = USART_LastBit_Disable; //最后一位数据的时钟脉冲不从SCLK输出
USART_Init(USART2, &USART_InitStructure);
USART_ClockInit(USART2, &USART_ClockInitStruct);
//使能USART1接收中断
USART_ITConfig(USART2, USART_IT_RXNE, ENABLE);
//使能USART1
USART_Cmd(USART2, ENABLE);
}
uint8_t Tx2Buffer[0xff];
u8 Tx2Counter=0;
u8 count2=32;
u8 Rx2_Buf[2][32]; //两个32字节的串口接收缓存
u8 Rx2_Act=0; //正在使用的buf号
u8 Rx2_Adr=0; //正在接收第几字节
u8 Rx2_Ok0 = 0;
u8 Rx2_Ok1 = 0;
/**************************串口1的收发中断********************************************/
void Uart2_IRQ(void)//与PC上位机的通讯
{
u8 com_data;
if(USART2->SR & USART_IT_ORE)
{
com_data=USART2->SR;
}
/****************接收中断 (接收寄存器非空)************/
//接收中断 (接收寄存器非空)
if(USART_GetITStatus(USART2, USART_IT_RXNE) != RESET)
{
arm=0;
//别忘了，只要接收到数据就是解锁（即APP连接了蓝牙）
com_data = USART2->DR;
if(Rx2_Adr==0) //寻找帧头0X8A
{
if(com_data==0xAA) //接收数据如果是0X8A,则写入缓存
{
Rx2_Buf[Rx2_Act][0] = com_data;
Rx2_Adr = 1;
}
}
else //正在接收数据
{
Rx2_Buf[Rx2_Act][Rx2_Adr] = com_data;
Rx2_Adr ++;
}
if(Rx2_Adr==25) //数据接收完毕//如果adr等于了32,
//那么这次act=0的数据接收完成，切换缓存到act=1；
{
Rx2_Adr = 0;
if(Rx2_Act)
{
Rx2_Ok1 = 1; //标志act=1的数组接收完成
Rx2_Act = 0; //切换缓存，去接收act=0的数组
}
else
{
Rx2_Act = 1;
Rx2_Ok0 = 1;
}
}
USART_ClearITPendingBit(USART2,USART_IT_RXNE);//清除接收中断标志位
}
}
/**********************************串口接收的缓存数据de分析*********************************/
void Uart2_DataAnl(u8 buf_num) //
{
u8 sum = 0,i=0;
if(Rx2_Buf[buf_num][1]!=0xAF) return; //检验第二位是不是AF
if(Rx2_Buf[buf_num][2]==0x03)//下面进行对遥控数据的处理,应该在Uart2蓝牙 //判断功能字,=0x,为遥控数据
{
for(i=0;i<24;i++) //判断校验字节是否正确
sum += Rx2_Buf[buf_num][i]; //sum是8位，自动截取最后8位
if(!(sum==Rx2_Buf[buf_num][24])) return; //判断sum
Rc_Get.THROTTLE = (vs16)(Rx2_Buf[buf_num][4]<<8)|Rx2_Buf[buf_num][5]; //首先进行8位数据的合并成16位赋值给遥控变量
Rc_Get.YAW = (vs16)(Rx2_Buf[buf_num][6]<<8)|Rx2_Buf[buf_num][7];
Rc_Get.ROLL = (vs16)(Rx2_Buf[buf_num][8]<<8)|Rx2_Buf[buf_num][9];
Rc_Get.PITCH = (vs16)(Rx2_Buf[buf_num][10]<<8)|Rx2_Buf[buf_num][11];
Rc_Get.AUX1 = (vs16)(Rx2_Buf[buf_num][12]<<8)|Rx2_Buf[buf_num][13];
Rc_Get.AUX2 = (vs16)(Rx2_Buf[buf_num][14]<<8)|Rx2_Buf[buf_num][15];
RC_Target_ROL = (1500-Rc_Get.ROLL)/30; //可以调遥控的灵敏度，一打杆偏大了，把30改大
RC_Target_PIT = (1500-Rc_Get.PITCH)/30; //要改变的RC_Target就变小了
RC_Target_YAW = (Rc_Get.YAW-1500)/30;
}
if(Rx2_Buf[buf_num][2]==0x01)
{
if(Rx2_Buf[buf_num][4]==0xA0) {ARMED = 0;TxCounter=0;Uart1_Send_ARMED();}
if(Rx2_Buf[buf_num][4]==0xA1) {ARMED = 1;TxCounter=0;Uart1_Send_ARMED();}
}
}
void Uart2_RXCheckEvent(void)
{
//USART_ITConfig(USART2, USART_IT_RXNE, DISABLE);//处理某一组数据时不能被UART2的接收中断打断
//因为UART2的中断优先级低于定时器中断，所以在执行定时器中断的程序（即Uart2_RXCheckEvent（））时，
//不可能被UART2的接收中断所打断
if(Rx2_Ok0)
{
Rx2_Ok0 = 0;
Uart2_DataAnl(0);
}
if(Rx2_Ok1)
{
Rx2_Ok1 = 0;
Uart2_DataAnl(1);
}
//USART_ITConfig(USART2, USART_IT_RXNE, ENABLE); //重新打开接收中断
}
/******************************************************************************************//*此文件由HC05和Uart1调用*/
#include "stm32f10x.h"
#include "usart.h"
#define BYTE0(dwTemp) (*(char *)(&dwTemp))
#define BYTE1(dwTemp) (*((char *)(&dwTemp) + 1))
#define BYTE2(dwTemp) (*((char *)(&dwTemp) + 2))
#define BYTE3(dwTemp) (*((char *)(&dwTemp) + 3))
typedef union {unsigned char byte[4];float num;}t_floattobyte;
t_floattobyte floattobyte;
void Uart1_Init(u32 br_num)//时钟低电平活动，禁用了同步通讯,因此只有Uart
{
USART_InitTypeDef USART_InitStructure;
USART_ClockInitTypeDef USART_ClockInitStruct;
GPIO_InitTypeDef GPIO_InitStructure;
RCC_APB2PeriphClockCmd(RCC_APB2Periph_USART1, ENABLE); //开启USART1时钟
RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA,ENABLE);
//配置PA9作为USART1 Tx
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_9;
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_PP;
GPIO_Init(GPIOA , &GPIO_InitStructure);
//配置PA10作为USART1 Rx
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_10;
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IN_FLOATING;
GPIO_Init(GPIOA , &GPIO_InitStructure);
//配置USART1
//中断被屏蔽了
USART_InitStructure.USART_BaudRate = br_num; //波特率可以通过地面站配置
USART_InitStructure.USART_WordLength = USART_WordLength_8b; //8位数据
USART_InitStructure.USART_StopBits = USART_StopBits_1; //在帧结尾传输1个停止位
USART_InitStructure.USART_Parity = USART_Parity_No; //禁用奇偶校验
USART_InitStructure.USART_HardwareFlowControl = USART_HardwareFlowControl_None; //硬件流控制失能
USART_InitStructure.USART_Mode = USART_Mode_Tx | USART_Mode_Rx; //发送、接收使能
//配置USART1时钟
USART_ClockInitStruct.USART_Clock = USART_Clock_Disable; //时钟低电平活动
USART_ClockInitStruct.USART_CPOL = USART_CPOL_Low; //SLCK引脚上时钟输出的极性->低电平
USART_ClockInitStruct.USART_CPHA = USART_CPHA_2Edge; //时钟第二个边沿进行数据捕获
USART_ClockInitStruct.USART_LastBit = USART_LastBit_Disable; //最后一位数据的时钟脉冲不从SCLK输出
USART_Init(USART1, &USART_InitStructure);
USART_ClockInit(USART1, &USART_ClockInitStruct);
//使能USART1接收中断
USART_ITConfig(USART1, USART_IT_RXNE, ENABLE);
//使能USART1
USART_Cmd(USART1, ENABLE);
}
uint8_t TxBuffer[0xff];
u8 TxCounter=0;
u8 count=;
u8 Rx_Buf[2][32]; //两个32字节的串口接收缓存
u8 Rx_Act=0; //正在使用的buf号
u8 Rx_Adr=0; //正在接收第几字节
u8 Rx_Ok0 = 0;
u8 Rx_Ok1 = 0;
void Uart1_IRQ(void)//与PC上位机的通讯
{
if(USART1->SR & USART_IT_ORE)//有数据过载了，赶进读
{
USART1->SR;//没什么用
}
/*******************发送中断***********************/
if((USART1->SR & (1<<7))&&(USART1->CR1 & USART_CR1_TXEIE))//if(USART_GetITStatus(USART1,USART_IT_TXE)!=RESET)
{ //如果打开了写中断使能
USART1->DR = TxBuffer[TxCounter++]; //写数据到DR
if(TxCounter == count)//如果TxCounter达到了最大量count
{
USART1->CR1 &= ~USART_CR1_TXEIE; //关闭TXE写入中断
//USART_ITConfig(USART1,USART_IT_TXE,DISABLE);
}
}
/****************接收中断 (接收寄存器非空)************/
if(USART1->SR & (1<<5))//if(USART_GetITStatus(USART1, USART_IT_RXNE) != RESET)
{ //接收中断为1代表接收数据寄存器已满
u8 com_data = USART1->DR;//赶进读
//寻找接收的数据中帧头0XAAAF
if(Rx_Adr==0)//检验Rx_Buf[Rx_Act][0,1,2,3]
{ //对应0xＡＡ，ＡＦ，FUN，LEN
if(com_data==0xAA)
{
Rx_Buf[Rx_Act][0] = com_data;
Rx_Adr = 1;
}
}
else if(Rx_Adr==1)
{
if(com_data==0xAF)
{
Rx_Buf[Rx_Act][1] = com_data;
Rx_Adr = 2;
}
else
Rx_Adr = 0;
}
else if(Rx_Adr==2) //FUN
{
Rx_Buf[Rx_Act][2] = com_data;
Rx_Adr = 3;
}
else if(Rx_Adr==3) //LEN
{
Rx_Buf[Rx_Act][3] = com_data;
Rx_Adr = 4;
}
else
{
Rx_Buf[Rx_Act][Rx_Adr] = com_data;
Rx_Adr ++;
}
if(Rx_Adr==Rx_Buf[Rx_Act][3]+5)//如果adr等于了前面接收到的LEN
{ //那么这次act=0的数据接收完成，切换缓存到act=1；
Rx_Adr = 0;
if(Rx_Act)
{
Rx_Act = 0; //切换缓存
Rx_Ok1 = 1;
}
else
{
Rx_Act = 1;
Rx_Ok0 = 1;
}
}
}
}
/**********************************串口接收的缓存数据分析*********************************/
void Uart_DataAnl(u8 buf_num) //
{
if((Rx_Buf[buf_num][1]==0xAA)&&(Rx_Buf[buf_num][2]==0xAF))//如果串口收到的是AA，AF
//是PC上位机发送给飞控的数据
{
}
}
void Uart_CheckEvent(void)
{
if(Rx_Ok0)
{
Rx_Ok0 = 0;
Uart_DataAnl(0);
}
if(Rx_Ok1)
{
Rx_Ok1 = 0;
Uart_DataAnl(1);
}
}
/******************************************************************************************/
/**************************实现函数********************************************
*******************************************************************************/
uint8_t Uart1_Put_Char(unsigned char DataToSend)
{
TxBuffer[count++] = DataToSend;
USART_ITConfig(USART1, USART_IT_TXE, ENABLE);
return DataToSend;
}
uint8_t Uart1_Put_Int16(uint16_t DataToSend)
{
uint8_t sum = 0;
TxBuffer[count++] = BYTE1(DataToSend);
TxBuffer[count++] = BYTE0(DataToSend);
USART_ITConfig(USART1, USART_IT_TXE, ENABLE);
sum += BYTE1(DataToSend);
sum += BYTE0(DataToSend);
return sum;
}
uint8_t Uart1_Put_Float(float DataToSend)
{
uint8_t sum = 0;
floattobyte.num=DataToSend;
TxBuffer[count++] = floattobyte.byte[3];
TxBuffer[count++] = floattobyte.byte[2];
TxBuffer[count++] = floattobyte.byte[1];
TxBuffer[count++] = floattobyte.byte[0];
USART_ITConfig(USART1, USART_IT_TXE, ENABLE);
sum += BYTE3(DataToSend);
sum += BYTE2(DataToSend);
sum += BYTE1(DataToSend);
sum += BYTE0(DataToSend);
return sum;
}
void Uart1_Put_String(unsigned char *Str)
{
//判断Str指向的数据是否有效.
while(*Str)
{
//是否是回车字符 如果是,则发送相应的回车 0x0d 0x0a
if(*Str=='\r')Uart1_Put_Char(0x0d);
else if(*Str=='\n')Uart1_Put_Char(0x0a);
else Uart1_Put_Char(*Str);
//指针++ 指向下一个字节.
Str++;
}
}/*此文件由HC05和Uart1调用*/
#include "stm32f10x.h"
#include "usart.h"
#define BYTE0(dwTemp) (*(char *)(&dwTemp))
#define BYTE1(dwTemp) (*((char *)(&dwTemp) + 1))
#define BYTE2(dwTemp) (*((char *)(&dwTemp) + 2))
#define BYTE3(dwTemp) (*((char *)(&dwTemp) + 3))
typedef union {unsigned char byte[4];float num;}t_floattobyte;
t_floattobyte floattobyte;
void Uart1_Init(u32 br_num)//时钟低电平活动，禁用了同步通讯,因此只有Uart
{
USART_InitTypeDef USART_InitStructure;
USART_ClockInitTypeDef USART_ClockInitStruct;
GPIO_InitTypeDef GPIO_InitStructure;
RCC_APB2PeriphClockCmd(RCC_APB2Periph_USART1, ENABLE); //开启USART1时钟
RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA,ENABLE);
//配置PA9作为USART1 Tx
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_9;
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_PP;
GPIO_Init(GPIOA , &GPIO_InitStructure);
//配置PA10作为USART1 Rx
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_10;
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IN_FLOATING;
GPIO_Init(GPIOA , &GPIO_InitStructure);
//配置USART1
//中断被屏蔽了
USART_InitStructure.USART_BaudRate = br_num; //波特率可以通过地面站配置
USART_InitStructure.USART_WordLength = USART_WordLength_8b; //8位数据
USART_InitStructure.USART_StopBits = USART_StopBits_1; //在帧结尾传输1个停止位
USART_InitStructure.USART_Parity = USART_Parity_No; //禁用奇偶校验
USART_InitStructure.USART_HardwareFlowControl = USART_HardwareFlowControl_None; //硬件流控制失能
USART_InitStructure.USART_Mode = USART_Mode_Tx | USART_Mode_Rx; //发送、接收使能
//配置USART1时钟
USART_ClockInitStruct.USART_Clock = USART_Clock_Disable; //时钟低电平活动
USART_ClockInitStruct.USART_CPOL = USART_CPOL_Low; //SLCK引脚上时钟输出的极性->低电平
USART_ClockInitStruct.USART_CPHA = USART_CPHA_2Edge; //时钟第二个边沿进行数据捕获
USART_ClockInitStruct.USART_LastBit = USART_LastBit_Disable; //最后一位数据的时钟脉冲不从SCLK输出
USART_Init(USART1, &USART_InitStructure);
USART_ClockInit(USART1, &USART_ClockInitStruct);
//使能USART1接收中断
USART_ITConfig(USART1, USART_IT_RXNE, ENABLE);
//使能USART1
USART_Cmd(USART1, ENABLE);
}
uint8_t TxBuffer[0xff];
u8 TxCounter=0;
u8 count=;
u8 Rx_Buf[2][32]; //两个32字节的串口接收缓存
u8 Rx_Act=0; //正在使用的buf号
u8 Rx_Adr=0; //正在接收第几字节
u8 Rx_Ok0 = 0;
u8 Rx_Ok1 = 0;
void Uart1_IRQ(void)//与PC上位机的通讯
{
if(USART1->SR & USART_IT_ORE)//有数据过载了，赶进读
{
USART1->SR;//没什么用
}
/*******************发送中断***********************/
if((USART1->SR & (1<<7))&&(USART1->CR1 & USART_CR1_TXEIE))//if(USART_GetITStatus(USART1,USART_IT_TXE)!=RESET)
{ //如果打开了写中断使能
USART1->DR = TxBuffer[TxCounter++]; //写数据到DR
if(TxCounter == count)//如果TxCounter达到了最大量count
{
USART1->CR1 &= ~USART_CR1_TXEIE; //关闭TXE写入中断
//USART_ITConfig(USART1,USART_IT_TXE,DISABLE);
}
}
/****************接收中断 (接收寄存器非空)************/
if(USART1->SR & (1<<5))//if(USART_GetITStatus(USART1, USART_IT_RXNE) != RESET)
{ //接收中断为1代表接收数据寄存器已满
u8 com_data = USART1->DR;//赶进读
//寻找接收的数据中帧头0XAAAF
if(Rx_Adr==0)//检验Rx_Buf[Rx_Act][0,1,2,3]
{ //对应0xＡＡ，ＡＦ，FUN，LEN
if(com_data==0xAA)
{
Rx_Buf[Rx_Act][0] = com_data;
Rx_Adr = 1;
}
}
else if(Rx_Adr==1)
{
if(com_data==0xAF)
{
Rx_Buf[Rx_Act][1] = com_data;
Rx_Adr = 2;
}
else
Rx_Adr = 0;
}
else if(Rx_Adr==2) //FUN
{
Rx_Buf[Rx_Act][2] = com_data;
Rx_Adr = 3;
}
else if(Rx_Adr==3) //LEN
{
Rx_Buf[Rx_Act][3] = com_data;
Rx_Adr = 4;
}
else
{
Rx_Buf[Rx_Act][Rx_Adr] = com_data;
Rx_Adr ++;
}
if(Rx_Adr==Rx_Buf[Rx_Act][3]+5)//如果adr等于了前面接收到的LEN
{ //那么这次act=0的数据接收完成，切换缓存到act=1；
Rx_Adr = 0;
if(Rx_Act)
{
Rx_Act = 0; //切换缓存
Rx_Ok1 = 1;
}
else
{
Rx_Act = 1;
Rx_Ok0 = 1;
}
}
}
}
/**********************************串口接收的缓存数据分析*********************************/
void Uart_DataAnl(u8 buf_num) //
{
if((Rx_Buf[buf_num][1]==0xAA)&&(Rx_Buf[buf_num][2]==0xAF))//如果串口收到的是AA，AF
//是PC上位机发送给飞控的数据
{
}
}
void Uart_CheckEvent(void)
{
if(Rx_Ok0)
{
Rx_Ok0 = 0;
Uart_DataAnl(0);
}
if(Rx_Ok1)
{
Rx_Ok1 = 0;
Uart_DataAnl(1);
}
}
/******************************************************************************************/
/**************************实现函数********************************************
*******************************************************************************/
uint8_t Uart1_Put_Char(unsigned char DataToSend)
{
TxBuffer[count++] = DataToSend;
USART_ITConfig(USART1, USART_IT_TXE, ENABLE);
return DataToSend;
}
uint8_t Uart1_Put_Int16(uint16_t DataToSend)
{
uint8_t sum = 0;
TxBuffer[count++] = BYTE1(DataToSend);
TxBuffer[count++] = BYTE0(DataToSend);
USART_ITConfig(USART1, USART_IT_TXE, ENABLE);
sum += BYTE1(DataToSend);
sum += BYTE0(DataToSend);
return sum;
}
uint8_t Uart1_Put_Float(float DataToSend)
{
uint8_t sum = 0;
floattobyte.num=DataToSend;
TxBuffer[count++] = floattobyte.byte[3];
TxBuffer[count++] = floattobyte.byte[2];
TxBuffer[count++] = floattobyte.byte[1];
TxBuffer[count++] = floattobyte.byte[0];
USART_ITConfig(USART1, USART_IT_TXE, ENABLE);
sum += BYTE3(DataToSend);
sum += BYTE2(DataToSend);
sum += BYTE1(DataToSend);
sum += BYTE0(DataToSend);
return sum;
}
void Uart1_Put_String(unsigned char *Str)
{
//判断Str指向的数据是否有效.
while(*Str)
{
//是否是回车字符 如果是,则发送相应的回车 0x0d 0x0a
if(*Str=='\r')Uart1_Put_Char(0x0d);
else if(*Str=='\n')Uart1_Put_Char(0x0a);
else Uart1_Put_Char(*Str);
//指针++ 指向下一个字节.
Str++;
}
}
本文章为转载内容，我们尊重原作者对文章享有的著作权。如有内容错误或侵权问题，欢迎原作者联系我们进行内容更正或删除文章。
