当前位置:   article > 正文

【STM32】飞控设计_aux3飞控

aux3飞控

【一些入门知识】

1.飞行原理

 【垂直运动】

mg>F1+F2+F3+F4,此时做下降加速飞行
mg<F1+F2+F3+F4,此时做升高加速飞行
mg=F1+F2+F3+F4 ,此时垂直上保持匀速飞行。

 【偏航飞行】

ω 4 + ω 2 ≠ ω 1+ ω 3  就会产生水平旋转

 【俯仰飞行】

F1+F4<F2+F3 向前飞行
F1+F4>F2+F3 向后飞行

【横滚飞行】

F4+F3>F1+F2 向右飞行
F4+F3<F1+F2 向左飞行

 2.串级PID

3.飞控的控制系统

4.姿态解算

 一.硬件设计(简)

【主控】

1.电源:3.7V锂电池供电 - DCDC升压至5V -  LDO稳压3.3V

2.USB - 上位机

3.SPI - NRF24L01无线通讯

4.I2C - MPU6050陀螺仪

5.4个PWM

6.主控STM32F103C8T6

【遥控】

1.电源:3.7V锂电池供电 - LDO稳压3.3V

2.I2C - AT24CO2

3.4个ADC - 两个遥感

4.8个IO口 - 8个按键

5.SPI - NRF24L01无线通讯

6.主控STM32F103C8T6

二.主控程序

【MPU6050读取飞控三轴加速度、角速度 并且 卡尔曼滤波】

通过 MPU6050 寄存器手册:我们需要读取的三轴加速度和三轴角速度位于寄存器 0x3B~0X48,读取数据后,需要合成 16bit 的数据。
  1. //从 0x3B 读取 6 个字节放到 buffer 里面
  2. #define Acc_Read() i2cRead(0x68, 0X3B,6,buffer)
  3. //从 0x43 读取 6 个字节放到 buffer 里面
  4. #define Gyro_Read() i2cRead(0x68, 0x43,6,&buffer[6])
  5. void MpuGetData(void) //读取陀螺仪数据加滤波
  6. {
  7. uint8_t i;
  8. uint8_t buffer[12];
  9. Acc_Read();//读取加速度
  10. Gyro_Read();//读取角速度
  11. for(i=0;i<6;i++)
  12. {
  13. //整合为 16bit,并减去水平静止校准值
  14. pMpu[i] = (((int16_t)buffer[i<<1] << 8) | buffer[(i<<1)+1])-MpuOffset[i];
  15. if(i < 3)//对加速度做卡尔曼滤波
  16. {
  17. {
  18. //卡尔曼滤波的数据初始化,这个 8192 是初始化默认 1 个 g 的加速度
  19. static struct _1_ekf_filter ekf[3] = {{0.02,0,0,0,0.001,0.543}
  20. {0.02,0, 0,0,0.001,0.543},{0.02,0, 0,0,0.001,0.543}};
  21. kalman_1(&ekf[i],(float)pMpu[i]); //调用一维卡尔曼滤波函数
  22. pMpu[i] = (int16_t)ekf[i].out;//卡尔曼滤波输出
  23. }
  24. }
  25. if(i > 2)//以下对角速度做一阶低通滤波
  26. {
  27. uint8_t k=i-3;
  28. const float factor = 0.15f; //滤波因素,因数越小,滤波力度越大
  29. static float last_mpuData[3];
  30. //滤波并保存滤波数据
  31. last_mpuData[k] = last_mpuData[k] * (1 - factor) + pMpu[i] * factor;
  32. pMpu[i] = last_mpuData[k];//滤波输出
  33. }
  34. }
  35. }

【遥控数据解析】

  1. void RC_Analy(void)
  2. {
  3. static uint16_t cnt;
  4. if(NRF24L01_RxPacket(RC_rxData)==SUCCESS)
  5. {
  6. uint8_t i;
  7. uint8_t CheckSum=0;
  8. uint16_t thr;
  9. cnt = 0;
  10. for(i=0;i<31;i++)
  11. {
  12. CheckSum += RC_rxData[i]; //检查数据的数量是否是31个
  13. }
  14. if(RC_rxData[31]==CheckSum && RC_rxData[0]==0xAA && RC_rxData[1]==0xAF) //如果接收到的遥控数据正确
  15. {
  16. Remote.roll = ((uint16_t)RC_rxData[4]<<8) | RC_rxData[5]; //通道1
  17. Remote.roll = LIMIT(Remote.roll,1000,2000);
  18. Remote.pitch = ((uint16_t)RC_rxData[6]<<8) | RC_rxData[7]; //通道2
  19. Remote.pitch = LIMIT(Remote.pitch,1000,2000);
  20. Remote.thr = ((uint16_t)RC_rxData[8]<<8) | RC_rxData[9]; //通道3
  21. Remote.thr = LIMIT(Remote.thr,1000,2000);
  22. Remote.yaw = ((uint16_t)RC_rxData[10]<<8) | RC_rxData[11]; //通道4
  23. Remote.yaw = LIMIT(Remote.yaw,1000,2000);
  24. Remote.AUX1 = ((uint16_t)RC_rxData[12]<<8) | RC_rxData[13]; //通道5 左上角按键都属于通道5
  25. Remote.AUX1 = LIMIT(Remote.AUX1,1000,2000);
  26. Remote.AUX2 = ((uint16_t)RC_rxData[14]<<8) | RC_rxData[15]; //通道6 右上角按键都属于通道6
  27. Remote.AUX2 = LIMIT(Remote.AUX2,1000,2000);
  28. Remote.AUX3 = ((uint16_t)RC_rxData[16]<<8) | RC_rxData[17]; //通道7 左下边按键都属于通道7
  29. Remote.AUX3 = LIMIT(Remote.AUX3,1000,2000);
  30. Remote.AUX4 = ((uint16_t)RC_rxData[18]<<8) | RC_rxData[19]; //通道8 右下边按键都属于通道6
  31. Remote.AUX4 = LIMIT(Remote.AUX4,1000,4000);
  32. {
  33. const float roll_pitch_ratio = 0.04f;
  34. const float yaw_ratio = 0.3f;
  35. pidPitch.desired =-(Remote.pitch-1500)*roll_pitch_ratio; //将遥杆值作为飞行角度的期望值
  36. pidRoll.desired = -(Remote.roll-1500)*roll_pitch_ratio;
  37. if(Remote.yaw>1820)
  38. {
  39. pidYaw.desired -= yaw_ratio;
  40. }
  41. else if(Remote.yaw <1180)
  42. {
  43. pidYaw.desired += yaw_ratio;
  44. }
  45. }
  46. remote_unlock();
  47. }
  48. }
  49. //如果3秒没收到遥控数据,则判断遥控信号丢失,飞控在任何时候停止飞行,避免伤人。
  50. //意外情况,使用者可紧急关闭遥控电源,飞行器会在3秒后立即关闭,避免伤人。
  51. //立即关闭遥控,如果在飞行中会直接掉落,可能会损坏飞行器。
  52. else
  53. {
  54. cnt++;
  55. if(cnt>500)
  56. {
  57. cnt = 0;
  58. ALL_flag.unlock = 0;
  59. NRF24L01_init();
  60. }
  61. }
  62. }

【PID控制器的设计】

  1. void FlightPidControl(float dt)
  2. {
  3. volatile static uint8_t status=WAITING_1;
  4. switch(status)
  5. {
  6. case WAITING_1: //等待解锁
  7. if(ALL_flag.unlock)
  8. {
  9. status = READY_11;
  10. }
  11. break;
  12. case READY_11: //准备进入控制
  13. pidRest(pPidObject,6); //批量复位PID数据,防止上次遗留的数据影响本次控制
  14. Angle.yaw = pidYaw.desired = pidYaw.measured = 0; //锁定偏航角
  15. status = PROCESS_31;
  16. break;
  17. case PROCESS_31: //正式进入控制
  18. if(Angle.pitch<-50||Angle.pitch>50||Angle.roll<-50||Angle.roll>50)//倾斜检测,大角度判定为意外情况,则紧急上锁
  19. if(Remote.thr>1200)//当油门的很低时不做倾斜检测
  20. ALL_flag.unlock = EMERGENT;//打入紧急情况
  21. pidRateX.measured = MPU6050.gyroX * Gyro_G; //内环测量值 角度/秒
  22. pidRateY.measured = MPU6050.gyroY * Gyro_G;
  23. pidRateZ.measured = MPU6050.gyroZ * Gyro_G;
  24. pidPitch.measured = Angle.pitch; //外环测量值 单位:角度
  25. pidRoll.measured = Angle.roll;
  26. pidYaw.measured = Angle.yaw;
  27. pidUpdate(&pidRoll,dt); //调用PID处理函数来处理外环 横滚角PID
  28. pidRateX.desired = pidRoll.out; //将外环的PID输出作为内环PID的期望值即为串级PID
  29. pidUpdate(&pidRateX,dt); //再调用内环
  30. pidUpdate(&pidPitch,dt); //调用PID处理函数来处理外环 俯仰角PID
  31. pidRateY.desired = pidPitch.out;
  32. pidUpdate(&pidRateY,dt); //再调用内环
  33. CascadePID(&pidRateZ,&pidYaw,dt); //也可以直接调用串级PID函数来处理
  34. break;
  35. case EXIT_255: //退出控制
  36. pidRest(pPidObject,6);
  37. status = WAITING_1;//返回等待解锁
  38. break;
  39. default:
  40. status = EXIT_255;
  41. break;
  42. }
  43. if(ALL_flag.unlock == EMERGENT) //意外情况,请使用遥控紧急上锁,飞控就可以在任何情况下紧急中止飞行,锁定飞行器,退出PID控制
  44. status = EXIT_255;
  45. }

【4路PWM电机驱动】

  1. void TIM2_PWM_Config(void)
  2. {
  3. TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure;
  4. TIM_OCInitTypeDef TIM_OCInitStructure;
  5. GPIO_InitTypeDef GPIO_InitStructure;
  6. /* 使能 GPIOA 时钟时钟 */
  7. RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA, ENABLE);
  8. GPIO_InitStructure.GPIO_Pin = GPIO_Pin_0 | GPIO_Pin_1 | GPIO_Pin_2 |
  9. GPIO_Pin_3;
  10. GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
  11. GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_PP;
  12. GPIO_Init(GPIOA, &GPIO_InitStructure);
  13. /* 使能定时器 2 时钟 */
  14. RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM2, ENABLE);
  15. /* Time base configuration */
  16. TIM_TimeBaseStructure.TIM_Period = 999; //定时器计数周期 0-999 1000
  17. TIM_TimeBaseStructure.TIM_Prescaler = 8; //设置预分频:8+1 分频 8K PWM 频率
  18. TIM_TimeBaseStructure.TIM_ClockDivision = 0; //设置时钟分频系数:不分频
  19. TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up; //向上计数模式
  20. TIM_TimeBaseInit(TIM2, &TIM_TimeBaseStructure);
  21. /* PWM1 Mode configuration: Channel */
  22. TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_PWM1; //配置为 PWM 模式 1
  23. TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable;
  24. TIM_OCInitStructure.TIM_Pulse = 0;
  25. //设置跳变值,当计数器计数到这个值时,电平发生跳变(即占空比) 初始值 0
  26. TIM_OCInitStructure.TIM_OCPolarity = TIM_OCPolarity_High;
  27. //当定时器计数值小于定时设定值时为高电平
  28. /* 使能通道 1 */
  29. TIM_OC1Init(TIM2, &TIM_OCInitStructure);
  30. TIM_OC1PreloadConfig(TIM2, TIM_OCPreload_Enable);
  31. /* 使能通道 2 */
  32. TIM_OC2Init(TIM2, &TIM_OCInitStructure);
  33. TIM_OC2PreloadConfig(TIM2, TIM_OCPreload_Enable);
  34. /* 使能通道 3 */
  35. TIM_OC3Init(TIM2, &TIM_OCInitStructure);
  36. TIM_OC3PreloadConfig(TIM2, TIM_OCPreload_Enable);
  37. /* 使能通道 4 */
  38. TIM_OC4Init(TIM2, &TIM_OCInitStructure);
  39. TIM_OC4PreloadConfig(TIM2, TIM_OCPreload_Enable);
  40. TIM_ARRPreloadConfig(TIM2, ENABLE); // 使能 TIM2 重载寄存器 ARR
  41. TIM_Cmd(TIM2, ENABLE); //使能定时器 2
  42. }

【解锁 - 启动步骤 - 电机动力分配】

  1. void MotorControl(void)
  2. {
  3. volatile static uint8_t status=WAITING_1;
  4. if(ALL_flag.unlock == EMERGENT) //意外情况,请使用遥控紧急上锁,飞控就可以在任何情况下紧急中止飞行,锁定飞行器,退出PID控制
  5. status = EXIT_255;
  6. switch(status)
  7. {
  8. case WAITING_1: //等待解锁
  9. MOTOR1 = MOTOR2 = MOTOR3 = MOTOR4 = 0; //如果锁定,则电机输出都为0
  10. if(ALL_flag.unlock)
  11. {
  12. status = WAITING_2;
  13. }
  14. case WAITING_2: //解锁完成后判断使用者是否开始拨动遥杆进行飞行控制
  15. if(Remote.thr>1100)
  16. {
  17. low_thr_cnt_quiet=0;
  18. low_thr_cnt=0;
  19. pidRest(pPidObject,6);
  20. status = PROCESS_31;
  21. }
  22. break;
  23. case PROCESS_31:
  24. {
  25. int16_t temp,thr;
  26. temp = Remote.thr -1000; //油门+定高输出值
  27. //油门比例规划
  28. thr = 250+0.45f * temp;
  29. if(temp<10) //自动关停判断
  30. {
  31. if(low_thr_cnt<1500)
  32. low_thr_cnt++;
  33. thr = thr-(low_thr_cnt*0.6);//油门摇杆值慢慢降为0
  34. if(MPU6050.accZ<8500&&MPU6050.accZ>7800)
  35. {
  36. low_thr_cnt++;
  37. if(low_thr_cnt>600)//1800ms
  38. {
  39. thr = 0;
  40. pidRest(pPidObject,6);
  41. MOTOR1 = MOTOR2 = MOTOR3 = MOTOR4 =0;
  42. status = WAITING_2;
  43. break;
  44. }
  45. }
  46. }
  47. else low_thr_cnt=0;
  48. MOTOR1 = MOTOR2 = MOTOR3 = MOTOR4 = LIMIT(thr,0,700); //留100给姿态控制
  49. //以下输出的脉冲分配取决于电机PWM分布与飞控坐标体系。请看飞控坐标体系图解,与四个电机PWM分布分布
  50. // 机头
  51. // PWM3 ♂ PWM1
  52. // * *
  53. // * *
  54. // * *
  55. // *
  56. // * *
  57. // * *
  58. // * *
  59. // PWM4 PWM2
  60. // pidRateX.out 横滚角串级PID输出 控制左右,可以看出1 2和3 4,左右两组电机同增同减
  61. // pidRateY.out 俯仰角串级PID输出 控制前后,可以看出2 3和1 4,前后两组电机同增同减
  62. // pidRateZ.out 横滚角串级PID输出 控制旋转,可以看出2 4和1 3,两组对角线电机同增同减
  63. // 正负号取决于算法输出 比如输出是正的话 往前飞必然是尾巴两个电机增加,往右飞必然是左边两个电机增加
  64. MOTOR1 += + pidRateX.out + pidRateY.out + pidRateZ.out;//; 姿态输出分配给各个电机的控制量
  65. MOTOR2 += + pidRateX.out - pidRateY.out - pidRateZ.out ;//;
  66. MOTOR3 += - pidRateX.out + pidRateY.out - pidRateZ.out;
  67. MOTOR4 += - pidRateX.out - pidRateY.out + pidRateZ.out;//;
  68. }
  69. break;
  70. case EXIT_255:
  71. MOTOR1 = MOTOR2 = MOTOR3 = MOTOR4 = 0; //如果锁定,则电机输出都为0
  72. status = WAITING_1;
  73. break;
  74. default:
  75. break;
  76. }
  77. TIM2->CCR1 = LIMIT(MOTOR1,0,1000); //更新PWM
  78. TIM2->CCR2 = LIMIT(MOTOR2,0,1000);
  79. TIM2->CCR3 = LIMIT(MOTOR3,0,1000);
  80. TIM2->CCR4 = LIMIT(MOTOR4,0,1000);
  81. }

【水平校准】

MPU6050 获取的数值要减去水平静止校准值才是真正的飞控可用数据
  1. void MpuGetOffset(void) //校准
  2. {
  3. int32_t buffer[6]={0};
  4. int16_t i;
  5. uint8_t k=30;
  6. const int8_t MAX_GYRO_QUIET = 5;
  7. const int8_t MIN_GYRO_QUIET = -5;
  8. /* wait for calm down */
  9. int16_t LastGyro[3] = {0};
  10. int16_t ErrorGyro[3];
  11. /* set offset initial to zero */
  12. memset(MpuOffset,0,12);
  13. MpuOffset[2] = 8192; //set offset from the 8192
  14. TIM_ITConfig( //使能或者失能指定的TIM中断
  15. TIM1,
  16. TIM_IT_Update ,
  17. DISABLE //使能
  18. );
  19. while(k--)//30次静止则判定飞行器处于静止状态
  20. {
  21. do
  22. {
  23. delay_ms(10);
  24. MpuGetData();
  25. for(i=0;i<3;i++)
  26. {
  27. ErrorGyro[i] = pMpu[i+3] - LastGyro[i];
  28. LastGyro[i] = pMpu[i+3];
  29. }
  30. }while ((ErrorGyro[0] > MAX_GYRO_QUIET )|| (ErrorGyro[0] < MIN_GYRO_QUIET)//标定静止
  31. ||(ErrorGyro[1] > MAX_GYRO_QUIET )|| (ErrorGyro[1] < MIN_GYRO_QUIET)
  32. ||(ErrorGyro[2] > MAX_GYRO_QUIET )|| (ErrorGyro[2] < MIN_GYRO_QUIET)
  33. );
  34. }
  35. /* throw first 100 group data and make 256 group average as offset */
  36. for(i=0;i<356;i++)//水平校准
  37. {
  38. MpuGetData();
  39. if(100 <= i)//取256组数据进行平均
  40. {
  41. uint8_t k;
  42. for(k=0;k<6;k++)
  43. {
  44. buffer[k] += pMpu[k];
  45. }
  46. }
  47. }
  48. for(i=0;i<6;i++)
  49. {
  50. MpuOffset[i] = buffer[i]>>8;
  51. }
  52. TIM_ITConfig( //使能或者失能指定的TIM中断
  53. TIM1,
  54. TIM_IT_Update ,
  55. ENABLE //使能
  56. );
  57. FLASH_write(MpuOffset,6);//将数据写到FLASH中,一共有6个int16数据
  58. }

三.遥控程序

摇杆ADC采集和转换

配置 4 路 ADC 采集遥控摇杆值。DMA 自动采集,转换完成后自动将 ADC 结果存于ADC_ConvertedValue 。
  1. void ADC1_Mode_Config(void)
  2. {
  3. DMA_InitTypeDef DMA_InitStructure;
  4. ADC_InitTypeDef ADC_InitStructure;
  5. /* DMA channel1 configuration */
  6. DMA_DeInit(DMA1_Channel1);
  7. DMA_InitStructure.DMA_PeripheralBaseAddr = ADC1_DR_Address; //ADC 结果寄存器地址
  8. DMA_InitStructure.DMA_MemoryBaseAddr = (u32)&ADC_ConvertedValue;//输入数组地址地址
  9. DMA_InitStructure.DMA_DIR = DMA_DIR_PeripheralSRC;
  10. DMA_InitStructure.DMA_BufferSize = 4;//转换 4 路
  11. DMA_InitStructure.DMA_PeripheralInc = DMA_PeripheralInc_Disable;//外设地址固定
  12. DMA_InitStructure.DMA_MemoryInc = DMA_MemoryInc_Enable; //内存地址固定
  13. DMA_InitStructure.DMA_PeripheralDataSize = DMA_PeripheralDataSize_HalfWord; //半字(12bit ADC存放)
  14. DMA_InitStructure.DMA_MemoryDataSize = DMA_MemoryDataSize_HalfWord;
  15. DMA_InitStructure.DMA_Mode = DMA_Mode_Circular; //循环传输
  16. DMA_InitStructure.DMA_Priority = DMA_Priority_High;
  17. DMA_InitStructure.DMA_M2M = DMA_M2M_Disable;
  18. DMA_Init(DMA1_Channel1, &DMA_InitStructure);
  19. /* Enable DMA channel1 */
  20. DMA_Cmd(DMA1_Channel1, ENABLE);
  21. /* ADC1 configuration */
  22. ADC_InitStructure.ADC_Mode = ADC_Mode_Independent; //独立 ADC 模式
  23. ADC_InitStructure.ADC_ScanConvMode = ENABLE ; //禁止扫描模式,扫描模式用于多通道采集
  24. ADC_InitStructure.ADC_ContinuousConvMode = ENABLE; //开启连续转换模式,即不停地进行 ADC 转换
  25. ADC_InitStructure.ADC_ExternalTrigConv = ADC_ExternalTrigConv_None; //不使用外部触发转换
  26. ADC_InitStructure.ADC_DataAlign = ADC_DataAlign_Right; //采集数据右对齐
  27. ADC_InitStructure.ADC_NbrOfChannel = 4; //4 路 ADC 通道
  28. ADC_Init(ADC1, &ADC_InitStructure);
  29. /*配置 ADC 时钟,为 PCLK2 的 8 分频,即 6MHz,ADC 频率最高不能超过 14MHz*/
  30. RCC_ADCCLKConfig(RCC_PCLK2_Div8);
  31. /*配置 ADC1 的通道 11 为 55. 5 个采样周期,序列为 1 */
  32. ADC_RegularChannelConfig(ADC1, ADC_Channel_0, 1, ADC_SampleTime_55Cycles5);
  33. ADC_RegularChannelConfig(ADC1, ADC_Channel_1, 2, ADC_SampleTime_55Cycles5);
  34. ADC_RegularChannelConfig(ADC1, ADC_Channel_2, 3, ADC_SampleTime_55Cycles5);
  35. ADC_RegularChannelConfig(ADC1, ADC_Channel_3, 4, ADC_SampleTime_55Cycles5);
  36. /* 使能 DMA 外设*/
  37. ADC_DMACmd(ADC1, ENABLE);
  38. /*使能 ADC1 外设 */
  39. ADC_Cmd(ADC1, ENABLE);
  40. /*复位校准寄存器 */
  41. ADC_ResetCalibration(ADC1);
  42. /*等待校准寄存器复位完成 */
  43. while(ADC_GetResetCalibrationStatus(ADC1));
  44. /* ADC 校准 */
  45. ADC_StartCalibration(ADC1);
  46. /* 等待校准完成*/
  47. while(ADC_GetCalibrationStatus(ADC1));
  48. /* 软件启动 ADC 转换 */
  49. ADC_SoftwareStartConvCmd(ADC1, ENABLE);
  50. }
每 10ms 进行一次 ADC 数据的转换为航模遥控数据:
12bitADC(0~4096)*0.25 +1000   ≈  航模标准数据 1000~2000
声明:本文内容由网友自发贡献,不代表【wpsshop博客】立场,版权归原作者所有,本站不承担相应法律责任。如您发现有侵权的内容,请联系我们。转载请注明出处:https://www.wpsshop.cn/w/秋刀鱼在做梦/article/detail/872665
推荐阅读
相关标签
  

闽ICP备14008679号