PWM高电平被压在0.999毫秒

c627933142 · 发表于 2018-5-17 19:07:20

花了近1个月的时间写完了飞控代码准备调PID了结果发现无论怎么给油门PWM高电平持续时间被压制在了0.999ms 真尼玛恶心
现在束手无策不知道从哪里找问题求大神指点迷津
代码量有点大我分楼发吧

c627933142 · 发表于 2018-5-17 19:09:43

先是I2C与MPU6050
#include <struct_all.h>
#include <I2C_MPU6050.h>

#define SCL_Pin GPIO_Pin_10
#define SDA_Pin GPIO_Pin_11

#define SCL_H    {GPIOB->BSRRH = SCL_Pin;I2C_delay(4);}    //SCL¸ßµçÆ½
#define SCL_L    {GPIOB->BSRRL  = SCL_Pin;I2C_delay(4);}    //SCLµÍµçÆ½
#define SDA_H    {GPIOB->BSRRH = SDA_Pin;I2C_delay(4);}    //SDA¸ßµçÆ½
#define SDA_L    {GPIOB->BSRRL  = SDA_Pin;I2C_delay(4);}    //SDAµÍµçÆ½
#define SDA_Read GPIOB->IDR  & SDA_Pin    //SDA¶ÁÊý¾Ý

uint8_t GYRO_Offset = 0;//²»×Ô¶¯Ð£Õý
uint8_t ACC_Offset  = 0;
uint32_t I2C_Erro=0;
static uint8_t MPU6050_Buffer[14];

void I2C2_Init(void)
{
GPIO_InitTypeDef GPIO_InitStructure;
RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOB , ENABLE); //´ò¿ªÍâÉèBµÄÊ±ÖÓ

GPIO_InitStructure.GPIO_Pin = Debug1_Pin | Debug2_Pin | Debug3_Pin; //ÓÃÓÚ²âÁ¿³ÌÐòÔËÐÐËÙÂÊ
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_OUT;
GPIO_InitStructure.GPIO_OType= GPIO_OType_PP;
GPIO_Init(GPIOB, &GPIO_InitStructure);

GPIO_InitStructure.GPIO_Pin = SCL_Pin; //SCL
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_OUT;
GPIO_InitStructure.GPIO_OType= GPIO_OType_OD;
GPIO_Init(GPIOB, &GPIO_InitStructure);

GPIO_InitStructure.GPIO_Pin = SDA_Pin; //SDA
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_OUT;
GPIO_InitStructure.GPIO_OType= GPIO_OType_OD;
GPIO_Init(GPIOB, &GPIO_InitStructure);

}

static void I2C_delay(int delay)
{
while (delay)
      delay--;
}

static uint8_t I2C_Start(void)
{
SCL_L;
SDA_H;
SCL_H;

if(!SDA_Read)
return 0; //SDAÏßÎªµÍµçÆ½Ôò×ÜÏßÃ¦,ÍË³ö
SDA_L;
if(SDA_Read)
return 0; //SDAÏßÎª¸ßµçÆ½Ôò×ÜÏß³ö´í,ÍË³ö

SDA_L;
return 1;
}

static void I2C_Stop(void)
{
SCL_L;
SDA_L;
SCL_H;
SDA_H;
}

static void I2C_ACK(void)
{
SCL_L;
__nop();
SDA_L; //Ð´Ó¦´ðÐÅºÅ
__nop();
SCL_H;
__nop();
SCL_L;
}

static void I2C_NACK(void)
{
SCL_L;
// __nop();
SDA_H; //²»Ð´Ó¦´ðÐÅºÅ
// __nop();
SCL_H;
// __nop();
SCL_L;
}

static uint8_t I2C_WaitAck(void)
{
SCL_L;
SDA_H;
SCL_H;

if(SDA_Read)
{
SCL_L;
return 0;
}

SCL_L;
return 1;
}

static void I2C_WriteByte(unsigned char SendByte)
{
uint8_t i=8;

while(i--)
{
SCL_L;
if( SendByte&0x80 )
SDA_H
else
SDA_L
SendByte <<= 1;
SCL_H;
  }
SCL_L;
}

static uint8_t I2C_ReadByte(void)
{
uint8_t i=8;
uint8_t ReceiveByte =0;

SDA_H;
while(i--)
{
SCL_L;
__nop();
SCL_H;
ReceiveByte <<= 1;
if(SDA_Read)
ReceiveByte |= 0x01;
}
SCL_L;
return ReceiveByte;
}

static uint8_t Single_WriteI2C(unsigned char Regs_Addr,unsigned char Regs_Data)
{
if( !I2C_Start() )
return 0; //I2CÆðÊ¼´íÎó£¬·µ»Ø
I2C_WriteByte(MPU6050Address); //Ð´SlaveµØÖ·£¬²¢ÅäÖÃ³ÉÐ´Ä£Ê½
if( !I2C_WaitAck() )
{
I2C_Stop();
return 0; //ÎÞACK£¬·µ»Ø
}
I2C_WriteByte(Regs_Addr); //Ð´¼Ä´æÆ÷µØÖ·
I2C_WaitAck();
I2C_WriteByte(Regs_Data); //Ð´¼Ä´æÆ÷Êý¾Ý
I2C_WaitAck();
I2C_Stop();
return 1;
}

static uint8_t Single_ReadI2C(unsigned char Regs_Addr)
{
uint8_t ret;

if( !I2C_Start() )
return 0; //I2CÆðÊ¼´íÎó£¬·µ»Ø
I2C_WriteByte(MPU6050Address); //Ð´SlaveµØÖ·£¬²¢ÅäÖÃ³ÉÐ´Ä£Ê½
if( !I2C_WaitAck() )
{
I2C_Stop();
return 0; //ÎÞACK£¬·µ»Ø
}

I2C_WriteByte(Regs_Addr); //Ð´¼Ä´æÆ÷µØÖ·
I2C_WaitAck();
I2C_Start();
I2C_WriteByte(MPU6050Address+1);//Ð´SlaveµØÖ·£¬²¢ÅäÖÃ³É¶ÁÄ£Ê½
I2C_WaitAck();

ret=I2C_ReadByte(); //´Ó´«¸ÐÆ÷ÖÐ¶Á³öÊý¾Ý

I2C_NACK(); //ÎÞÓ¦´ð
I2C_Stop();                   //½áÊø±¾¶ÎIIC½ø³Ì
return ret;
}

uint8_t MPU6050_SequenceRead(void)
{
uint8_t index=14;

if( !I2C_Start() )
return 0; //I2CÆðÊ¼´íÎó£¬·µ»Ø
I2C_WriteByte(MPU6050Address); //Ð´SlaveµØÖ·£¬²¢ÅäÖÃ³ÉÐ´Ä£Ê½
if( !I2C_WaitAck() )
{
I2C_Stop();
return 0; //ÎÞACK£¬·µ»Ø
}
I2C_WriteByte(ACCEL_XOUT_H); //Ð´¼Ä´æÆ÷µØÖ·
I2C_WaitAck();

I2C_Start();
I2C_WriteByte(MPU6050Address+1);//Ð´SlaveµØÖ·£¬²¢ÅäÖÃ³É¶ÁÄ£Ê½
I2C_WaitAck();

while(--index) //Á¬¶Á13Î»¼Ä´æÆ÷
{
MPU6050_Buffer[13 - index] = I2C_ReadByte();
I2C_ACK();
}

MPU6050_Buffer[13] = I2C_ReadByte(); //¶ÁµÚ14¼Ä´æÆ÷
I2C_NACK();
I2C_Stop();

return 1;
}

void MPU6050_SingleRead(void)
{
MPU6050_Buffer[0] = Single_ReadI2C(ACCEL_XOUT_H);
MPU6050_Buffer[1] = Single_ReadI2C(ACCEL_XOUT_L);
MPU6050_Buffer[2] = Single_ReadI2C(ACCEL_YOUT_H);
MPU6050_Buffer[3] = Single_ReadI2C(ACCEL_YOUT_L);
MPU6050_Buffer[4] = Single_ReadI2C(ACCEL_ZOUT_H);
MPU6050_Buffer[5] = Single_ReadI2C(ACCEL_ZOUT_L);

MPU6050_Buffer[8]  = Single_ReadI2C(GYRO_XOUT_H);
MPU6050_Buffer[9]  = Single_ReadI2C(GYRO_XOUT_L);
MPU6050_Buffer[10] = Single_ReadI2C(GYRO_YOUT_H);
MPU6050_Buffer[11] = Single_ReadI2C(GYRO_YOUT_L);
MPU6050_Buffer[12] = Single_ReadI2C(GYRO_ZOUT_H);
MPU6050_Buffer[13] = Single_ReadI2C(GYRO_ZOUT_L);
}

uint8_t InitMPU6050(void)
{
if( Single_ReadI2C(WHO_AM_I) != 0x68)//¼ì²éMPU6050ÊÇ·ñÕý³£
{
return 0;
}

Single_WriteI2C(PWR_MGMT_1, 0x00); //µçÔ´¹ÜÀí£¬µäÐÍÖµ£º0x00£¬Õý³£Ä£Ê½
I2C_delay(20000); //Ô¼2.5msÑÓÊ±
Single_WriteI2C(SMPLRT_DIV, 0x00); //ÍÓÂÝÒÇ²ÉÑùÂÊ£¬µäÐÍÖµ£º0x00£¬²»·ÖÆµ£¨8KHZ£©
I2C_delay(20000);
Single_WriteI2C(CONFIG2, 0x00);    //µÍÍ¨ÂË²¨ÆµÂÊ£¬µäÐÍÖµ£º0x00£¬²»ÆôÓÃMPU6050×Ô´øÂË²¨
I2C_delay(20000);
Single_WriteI2C(GYRO_CONFIG, 0x18); //ÍÓÂÝÒÇ×Ô¼ì¼°²âÁ¿·¶Î§£¬µäÐÍÖµ£º0x18(²»×Ô¼ì£¬2000deg/s)
I2C_delay(20000);
Single_WriteI2C(ACCEL_CONFIG, 0x1F);//¼ÓËÙ¼Æ×Ô¼ì¡¢²âÁ¿·¶Î§¼°¸ßÍ¨ÂË²¨ÆµÂÊ£¬µäÐÍÖµ£º0x1F(²»×Ô¼ì£¬16G)
I2C_delay(20000);

return 1;
}

void MPU6050_Compose(void)
{
acc.x  = ((((int16_t)MPU6050_Buffer[0]) << 8) | MPU6050_Buffer[1]) - offset_acc.x; //¼õÈ¥ÁãÆ«
acc.y  = ((((int16_t)MPU6050_Buffer[2]) << 8) | MPU6050_Buffer[3]) - offset_acc.y;
acc.z  = ((((int16_t)MPU6050_Buffer[4]) << 8) | MPU6050_Buffer[5]) - offset_acc.z;

gyro.x = ((((int16_t)MPU6050_Buffer[8])  << 8) | MPU6050_Buffer[9])  - offset_gyro.x;
gyro.y = ((((int16_t)MPU6050_Buffer[10]) << 8) | MPU6050_Buffer[11]) - offset_gyro.y;
gyro.z = ((((int16_t)MPU6050_Buffer[12]) << 8) | MPU6050_Buffer[13]) - offset_gyro.z;

}

c627933142 · 发表于 2018-5-17 19:10:51

然后是滤波
static float ACC_IIR_FACTOR;

void Calculate_FilteringCoefficient(float Time, float Cut_Off)
{
ACC_IIR_FACTOR = Time /( Time + 1/(2.0f*Pi*Cut_Off) );
}

void ACC_IIR_Filter(struct _acc *Acc_in,struct _acc *Acc_out)
{
Acc_out->x = Acc_out->x + ACC_IIR_FACTOR*(Acc_in->x - Acc_out->x);
Acc_out->y = Acc_out->y + ACC_IIR_FACTOR*(Acc_in->y - Acc_out->y);
Acc_out->z = Acc_out->z + ACC_IIR_FACTOR*(Acc_in->z - Acc_out->z);
}

#define Filter_Num 2

void Gyro_Filter(struct _gyro *Gyro_in,struct _gyro *Gyro_out)
{
static int16_t Filter_x[Filter_Num],Filter_y[Filter_Num],Filter_z[Filter_Num];
static uint8_t Filter_count;
int32_t Filter_sum_x=0,Filter_sum_y=0,Filter_sum_z=0;
uint8_t i=0;

Filter_x[Filter_count] = Gyro_in->x;
Filter_y[Filter_count] = Gyro_in->y;
Filter_z[Filter_count] = Gyro_in->z;

for(i=0;i<Filter_Num;i++)
{
Filter_sum_x += Filter_x[i];
Filter_sum_y += Filter_y[i];
Filter_sum_z += Filter_z[i];
}

Gyro_out->x = Filter_sum_x / Filter_Num;
Gyro_out->y = Filter_sum_y / Filter_Num;
Gyro_out->z = Filter_sum_z / Filter_Num;

Filter_count++;
if(Filter_count == Filter_Num)
Filter_count=0;
}

c627933142 · 发表于 2018-5-17 19:11:30

角速度转弧度制
#include "Maths.h"
#include "struct_all.h"

void Get_Radian(struct _gyro *Gyro_in,struct _SI_float *Gyro_out)
{
Gyro_out->x = (float)(Gyro_in->x * RawData_to_Radian);
Gyro_out->y = (float)(Gyro_in->y * RawData_to_Radian);
Gyro_out->z = (float)(Gyro_in->z * RawData_to_Radian);
}

float invSqrt(float x)
{
  float halfx = 0.5f * x;
  float y = x;
  long i = *(long*)&y;
  i = 0x5f3759df - (i>>1);
  y = *(float*)&i;
  y = y * (1.5f - (halfx * y * y));
  return y;
}

c627933142 · 发表于 2018-5-17 19:12:08

四元数转姿态角
#include <IMU.h>
#include <math.h>
#include <Maths.h>
#include <I2C_MPU6050.h>

#define Kp 1.0f // ±ÈÀý³£Êý
#define Ki 0.001f  // »ý·Ö³£Êý
#define halfT 0.0005f//°ëÖÜÆÚ
#define T 0.001f  // ÖÜÆÚÎª1ms

float q0 = 1, q1 = 0, q2 = 0, q3 = 0;      // ËÄÔªÊý
float exInt = 0, eyInt = 0, ezInt = 0; // Îó²î»ý·ÖÀÛ¼ÆÖµ

void IMUpdate(float gx, float gy, float gz, float ax, float ay, float az)
{
float norm;
float vx, vy, vz;
float ex, ey, ez;

  //ËÄÔªÊý»ý·Ö£¬ÇóµÃµ±Ç°µÄ×ËÌ¬
float q0_last = q0;
float q1_last = q1;
float q2_last = q2;
float q3_last = q3;

//°Ñ¼ÓËÙ¶È¼ÆµÄÈýÎ¬ÏòÁ¿×ª³Éµ¥Î»ÏòÁ¿
norm = invSqrt(ax*ax + ay*ay + az*az);
ax = ax * norm;
ay = ay * norm;
az = az * norm;

//¹À¼ÆÖØÁ¦¼ÓËÙ¶È·½ÏòÔÚ·ÉÐÐÆ÷×ø±êÏµÖÐµÄ±íÊ¾£¬ÎªËÄÔªÊý±íÊ¾µÄÐý×ª¾ØÕóµÄµÚÈýÐÐ
vx = 2*(q1*q3 - q0*q2);
vy = 2*(q0*q1 + q2*q3);
vz = q0*q0 - q1*q1 - q2*q2 + q3*q3;

//¼ÓËÙ¶È¼Æ¶ÁÈ¡µÄ·½ÏòÓëÖØÁ¦¼ÓËÙ¶È·½ÏòµÄ²îÖµ£¬ÓÃÏòÁ¿²æ³Ë¼ÆËã
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;

//ÓÃ²æ»ýÎó²îÀ´×&ouml

IÐÞÕýÍÓÂÝÁãÆ«£¬¼´µÖÏûÍÓÂÝ¶ÁÊýÖÐµÄÆ«ÒÆÁ¿
gx = gx + Kp*ex + exInt;
gy = gy + Kp*ey + eyInt;
gz = gz + Kp*ez + ezInt;

//Ò»½×½üËÆËã·¨
q0 = q0_last + (-q1_last*gx - q2_last*gy - q3_last*gz)*halfT;
q1 = q1_last + ( q0_last*gx + q2_last*gz - q3_last*gy)*halfT;
q2 = q2_last + ( q0_last*gy - q1_last*gz + q3_last*gx)*halfT;
q3 = q3_last + ( q0_last*gz + q1_last*gy - q2_last*gx)*halfT;

//ËÄÔªÊý¹æ·¶»¯
norm = invSqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3);
q0 = q0 * norm;
q1 = q1 * norm;
q2 = q2 * norm;
q3 = q3 * norm;

out_angle.yaw += filter_gyro.z * RawData_to_Angle * 0.001f;
}

void Get_Eulerian_Angle(struct _out_angle *angle)
{
angle->pitch = -atan2(2.0f*(q0*q1 + q2*q3),q0*q0 - q1*q1 - q2*q2 + q3*q3)*Radian_to_Angle;
angle->roll = asin (2.0f*(q0*q2 - q1*q3))*Radian_to_Angle;
}

c627933142 · 发表于 2018-5-17 19:12:56

串级PID
#include "Maths.h"
#include "Control.h"
#include "struct_all.h"

float thr=0;

float Pitch_i,Roll_i,Yaw_i;
float Pitch_old,Roll_old,Yaw_old;
float Pitch_d,Roll_d,Yaw_d;

float RC_Pitch=0,RC_Roll=0,RC_Yaw=0;

float Pitch_shell_out,Roll_shell_out,Yaw_shell_out;

float Pitch_shell_kp=140;
float Pitch_shell_ki=0.2;
float Pitch_shell_kd=0.8;

float Roll_shell_kp=150;
float Roll_shell_ki=0.2;
float Roll_shell_kd=0.8;

float Yaw_shell_kp=15;//0.87
float Yaw_shell_ki=0.002;
float Yaw_shell_kd=0.02;
float Gyro_radian_old_x,Gyro_radian_old_y,Gyro_radian_old_z;
float pitch_core_kp_out,pitch_core_ki_out,pitch_core_kd_out,Roll_core_kp_out,\
                     Roll_core_ki_out,Roll_core_kd_out,Roll_core_ki_out,Yaw_core_kp_out,\
                     Yaw_core_ki_out,Yaw_core_kd_out;

float Pitch_core_out,Roll_core_out,Yaw_core_out;

float Pitch_core_kp=0.02;
float Pitch_core_ki=0;
float Pitch_core_kd=0.001;//0.001

float Roll_core_kp=0.02;
float Roll_core_ki=0;
float Roll_core_kd=0.001;

float Yaw_core_kp=0.001;
float Yaw_core_ki=0;
float Yaw_core_kd=0.001;

int16_t moto1=1613,moto2=1613,moto3=1613,moto4=1613;

void CONTROL_PID(struct _out_angle *angle)
{

  RC_Pitch=(300000 -1499.0f )/50;
  RC_Pitch-=3.3f;

  Pitch_i+=(angle->pitch-RC_Pitch);

  if(Pitch_i>300) Pitch_i=300;
  else if(Pitch_i<-300) Pitch_i=-300;

  Pitch_d=angle->pitch-Pitch_old;

  Pitch_shell_out = Pitch_shell_kp*(angle->pitch-RC_Pitch) + Pitch_shell_ki*Pitch_i + Pitch_shell_kd*Pitch_d;

  Pitch_old=angle->pitch;

  RC_Roll=(300000-1502)/50;
  Roll_i+=(angle->roll-RC_Roll);

  if(Roll_i>300) Roll_i=300;
  else if(Roll_i<-300) Roll_i=-300;

  Roll_d=angle->roll-Roll_old;

  Roll_shell_out  = Roll_shell_kp*(angle->roll-RC_Roll) + Roll_shell_ki*Roll_i + Roll_shell_kd*Roll_d;

  Roll_old=angle->roll;

  Yaw_d=SI_gyro.z-Yaw_old;

  Yaw_shell_out  = Yaw_shell_kp*(SI_gyro.z-RC_Yaw) + Yaw_shell_ki*Yaw_i + Yaw_shell_kd*Yaw_d;

  Yaw_old=SI_gyro.z;

  pitch_core_kp_out = Pitch_core_kp * (Pitch_shell_out + SI_gyro.y );
  pitch_core_kd_out = Pitch_core_kd * (SI_gyro.y - Gyro_radian_old_y);

  Roll_core_kp_out  = Roll_core_kp  * (Roll_shell_out  + SI_gyro.x );
  Roll_core_kd_out  = Roll_core_kd  * (SI_gyro.x - Gyro_radian_old_x);

  Yaw_core_kp_out  = Yaw_core_kp  * (Yaw_shell_out  + SI_gyro.z);
  Yaw_core_kd_out  = Yaw_core_kd  * (SI_gyro.z - Gyro_radian_old_z);

  Pitch_core_out = pitch_core_kp_out + pitch_core_kd_out;
  Roll_core_out  = Roll_core_kp_out  + Roll_core_kd_out;
  Yaw_core_out = Yaw_core_kp_out + Yaw_core_kd_out;

  Gyro_radian_old_y = SI_gyro.y;
  Gyro_radian_old_x = SI_gyro.x;
  Gyro_radian_old_z = SI_gyro.z;

  thr=1300;
  moto1=(int16_t)(thr - Roll_core_out - Pitch_core_out- Yaw_core_out);
  moto2=(int16_t)(thr + Roll_core_out - Pitch_core_out+ Yaw_core_out);
  moto3=(int16_t)(thr + Roll_core_out + Pitch_core_out- Yaw_core_out);
  moto4=(int16_t)(thr - Roll_core_out + Pitch_core_out+ Yaw_core_out);

  Motor_Out(moto1,moto2,moto3,moto4);
}

c627933142 · 发表于 2018-5-17 19:13:21

主函数
#include <stm32f4xx.h>
#include <Struct_All.h>
#include <drivers_init.h>
#include <Filter.h>
#include <Maths.h>
#include <IMU.h>
#include <Control.h>
#include <Timer3.h>
#include <delay.h>
#include <NVIC.h>

int main()
{
delay_init(72);
Nvic_Init();
drivers_init();
while(1)
{
if(Count_1ms>=1)
{
Count_1ms=0;
MPU6050_SingleRead();
MPU6050_Compose();
ACC_IIR_Filter(&acc,&filter_acc);
Gyro_Filter(&gyro,&filter_gyro);
Get_Radian(&filter_gyro,&SI_gyro);
IMUpdate(SI_gyro.x,SI_gyro.y,SI_gyro.z,filter_acc.x,filter_acc.y,filter_acc.z);
}
if(Count_4ms>=4)
{
      Count_4ms=0;
      Get_Eulerian_Angle(&out_angle);
      CONTROL_PID(&out_angle);
}
}

}