基于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_LAS
最低0.47元/天 解锁文章
扫一扫
3715
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
