完善资料让更多小伙伴认识你,还能领取20积分哦, 立即完善>
扫一扫,分享给好友
小弟刚学使用PID算法,从网上借鉴到一个比较容易理解的PID算法,可是我不清楚它是一个双环PID还是一个单环PID,所以请大家帮忙看看,源码如下:
void ALG_PID_Att_Loop(T_float_angle *IMU_Angle) { int32_t PIDTerm[3]; uint8_t i; /* 计算姿态差*/ errorAngle[ROLL] = constrain_int32(Rx_Command[ROLL], -(int)FLYANGLE_MAX, +FLYANGLE_MAX - IMU_Angle->Roll); errorAngle[PITCH] = constrain_int32(Rx_Command[PITCH], -(int)FLYANGLE_MAX, +FLYANGLE_MAX - IMU_Angle->Pitch); /* YAW误差计算有待改进 */ errorAngle[YAW] = Rx_Command[YAW]*5 - MEMS_DataStructure.MEMS_Gyro_Z_Data; for(i = 0; i < 3; i++) { //当油门低于检查值时积分清零 if(Rx_Command[THROTTLE] < RC_MINCHECK) { PID_reset_I(&pid_par[i]); } //得到PID输出 PIDTerm[i] = PID_get_pid(errorAngle[i], PID_LOOP_tiME, &pid_par[i]); } PIDTerm[YAW] = -constrain_int32(PIDTerm[YAW], -300 - abs(Rx_Command[YAW]), +300 + abs(Rx_Command[YAW])); } int32_t PID_get_p(int32_t error, PID_Par *pid_par) { return error * pid_par->kP / 64; } int32_t PID_get_i(int32_t error, uint16_t dt, PID_Par *pid_par) { if((pid_par->kI != 0) && (dt != 0)) { pid_par->integrator += (error * dt / 2048 ) * pid_par->kI; //积分限幅 pid_par->integrator = constrain_int32(pid_par->integrator, -(pid_par->iMax), +(pid_par->iMax)); return pid_par->integrator / 8192; } return 0; } void PID_reset_I(PID_Par *pid_par) { pid_par->integrator = 0; } int32_t PID_get_d(int32_t error, uint16_t dt, PID_Par *pid_par) { if ((pid_par->kD != 0) && (dt != 0)) { int32_t derivative; derivative = error - pid_par->last_error; pid_par->last_error = error; derivative = (derivative * ((uint16_t)0xFFFF / (dt / 16 ))) / 64; return (derivative * pid_par->kD) / 4; } return 0; } int32_t PID_get_pi(int32_t error, uint16_t dt, PID_Par *pid_par) { return PID_get_p(error, pid_par) + PID_get_i(error, dt, pid_par); } int32_t PID_get_pid(int32_t error, uint16_t dt, PID_Par *pid_par) { return PID_get_p(error, pid_par) + PID_get_i(error, dt, pid_par) + PID_get_d(error, dt, pid_par); } |
|
相关推荐
1个回答
|
|
应该是双环PID,楼主可以仔细分析一下代码
|
|
|
|
你正在撰写答案
如果你是对答案或其他答案精选点评或询问,请使用“评论”功能。
2049 浏览 1 评论
AD7686芯片不传输数据给STM32,但是手按住就会有数据。
1893 浏览 3 评论
4485 浏览 0 评论
如何解决MPU-9250与STM32通讯时,出现HAL_ERROR = 0x01U
2040 浏览 1 评论
hal库中i2c卡死在HAL_I2C_Master_Transmit
2549 浏览 1 评论
小黑屋| 手机版| Archiver| 电子发烧友 ( 湘ICP备2023018690号 )
GMT+8, 2024-12-21 10:13 , Processed in 1.678689 second(s), Total 71, Slave 55 queries .
Powered by 电子发烧友网
© 2015 bbs.elecfans.com
关注我们的微信
下载发烧友APP
电子发烧友观察
版权所有 © 湖南华秋数字科技有限公司
电子发烧友 (电路图) 湘公网安备 43011202000918 号 电信与信息服务业务经营许可证:合字B2-20210191 工商网监 湘ICP备2023018690号