完善资料让更多小伙伴认识你,还能领取20积分哦, 立即完善>
扫一扫,分享给好友
看到STC给出的四轴飞控开源例程,是俺正想要的,额为欣喜,http://www.stcmcudata.com/STC8F-DATASHEET/%E5%9B%9B%E8%BD%B4%E9%A3%9E%E6%8E%A7-STC8A8K64S4A12-LQFP44-PPM-V10.rar 。可是读过后觉得很有问题,没敢尝试。希望请里手人士看看先,得知道能用不,以免无端侵占耽误俺的太多“瞌睡”。比如用到了6050芯片可是压根就不见读取相关寄存器,形同一顿乱读填至“tp”,然后还一本正经的对莫名奇妙“tp”缓存内容进行处理。不知是何革命原理:
void Read_MPU6050(u8 *buf) { u8 i; I2C_Start(); //起始信号 I2C_SendByte(SlaveAddress); //发送设备地址+写信号 I2C_SendByte(ACCEL_XOUT_H); //内部寄存器地址, I2C_Start(); //起始信号 I2C_SendByte(SlaveAddress+1); //发送设备地址+读信号 for(i=0; i<13; i++) { buf = I2C_RecvByte(); //读出寄存器数据 SDA = 0; //写应答信号 SCL = 1; //拉高时钟线 Delay2us(); //延时 SCL = 0; //拉低时钟线 Delay2us(); //延时 } buf = I2C_RecvByte(); //最后一个字节 SDA = 1; //写非应答信号 SCL = 1; //拉高时钟线 Delay2us(); //延时 SCL = 0; //拉低时钟线 Delay2us(); //延时 I2C_Stop(); //停止信号 } 。。。。。。。。。。。。 //************************************** u8 I2C_RecvByte(void) { u8 i; u8 dat = 0; SDA = 1; //使能内部上拉,准备读取数据, for (i=0; i<8; i++) //8位计数器 { dat <<= 1; SCL = 1; //拉高时钟线 Delay2us(); //延时 dat |= SDA; //读数据 SCL = 0; //拉低时钟线 Delay2us(); //延时 } return dat; } 。。。。。。。。。。。。 //******************************************************************************************** Read_MPU6050(tp); //680us Angle_ax = ((float)(((int *)&tp)[0])) / 8192.0; //加速度处理 结果单位是 +- g Angle_ay = ((float)(((int *)&tp)[1])) / 8192.0; //转换关系 8192 LSB/g, 1g对应读数8192 Angle_az = ((float)(((int *)&tp)[2])) / 8192.0; //加速度量程 +-4g/S Last_Angle_gx = Angle_gx; //储存上一次角速度数据 Last_Angle_gy = Angle_gy; Angle_gx = ((float)(((int *)&tp)[4] - g_x)) / 65.5; //陀螺仪处理 结果单位是 +-度 Angle_gy = ((float)(((int *)&tp)[5] - g_y)) / 65.5; //陀螺仪量程 +-500度/S, 1度/秒 对应读数 65.536 Angle_gz = ((float)(((int *)&tp)[6] - g_z)) / 65.5; //转换关系65.5 LSB/度 IMUupdate(Angle_gx*0.0174533f, Angle_gy*0.0174533f, Angle_gz*0.0174533f, Angle_ax,Angle_ay,Angle_az); //**********************************X轴指向************************************************ Ax = AngleX - a_x - ((float)FRX - 128) / 4.0; //角度控制量加载至角度 if(YM > 35) ERRORX_Out += Ax, ERRORX_Out += Ax, ERRORX_Out += Ax; //外环积分(油门小于某个值时不积分) else ERRORX_Out = 0; //油门小于定值时清除积分值 if(ERRORX_Out > 1500) ERRORX_Out = 1500; else if(ERRORX_Out < -1500) ERRORX_Out = -1500; //积分限幅 Out_PID_X = Ax*Out_XP + ERRORX_Out*Out_XI + (Ax-Last_Ax)*Out_XD; //外环PI Last_Ax = Ax; if(YM > 35) ERRORX_In += (Angle_gy - Out_PID_X); //内环积分(油门小于某个值时不积分) else ERRORX_In = 0; //油门小于定值时清除积分值 if(ERRORX_In > 500) ERRORX_In = 500; else if(ERRORX_In < -500) ERRORX_In = -500; //积分限幅 PID_x = (Angle_gy + Out_PID_X) * In_XP + ERRORX_In * In_XI + (Angle_gy - Last_Angle_gy) * In_XD; //内环PID if(PID_x > 500) PID_x = 500; //输出量限幅 if(PID_x < -500) PID_x = -500; //**************Y轴指向************************************************** Ay = AngleY - a_y + ((float)FRY - 128) / 4.0; //角度控制量加载至角度 if(YM > 35) ERRORY_Out += Ay, ERRORY_Out += Ay, ERRORY_Out += Ay; //外环积分(油门小于某个值时不积分) else ERRORY_Out = 0; //油门小于定值时清除积分值 if(ERRORY_Out > 1500) ERRORY_Out = 1500; else if(ERRORY_Out < -1500) ERRORY_Out = -1500; //积分限幅 Out_PID_Y = Ay * Out_YP + ERRORY_Out * Out_YI + (Ay-Last_Ay)*Out_YD; //外环PID Last_Ay = Ay; if(YM > 35) ERRORY_In += (Angle_gx - Out_PID_Y); //内环积分(油门小于某个值时不积分) else ERRORY_In = 0; //油门小于定值时清除积分值 if(ERRORY_In > 500) ERRORY_In = 500; else if(ERRORY_In < -500) ERRORY_In = -500; //积分限幅 PID_y = (Angle_gx + Out_PID_Y) * In_YP + ERRORY_In * In_YI + (Angle_gx - Last_Angle_gx) * In_YD; //内环PID if(PID_y > 500) PID_y = 500; //输出量限幅 if(PID_y <-500) PID_y = -500; //**************Z轴指向(Z轴随便啦,自旋控制没必要上串级PID)***************************** Az = Angle_gz - ((float)FRZ - 128); if(YM > 35) Z_integral += Az; //Z轴积分 else Z_integral = 0; //油门小于40积分清零 if(Z_integral > 500.0f) Z_integral = 500.0f; //积分限幅 else if(Z_integral < -500.0f) Z_integral = -500.0f; //积分限幅 PID_z = Az * ZP + Z_integral * ZI + (Az - Last_Az) * ZD; Last_Az = Az; if(PID_z > 200) PID_z = 200; //输出量限幅 if(PID_z < -200) PID_z = -200; speed0 = (int)( PID_x + PID_y + PID_z); //M1改为逆时针 speed1 = (int)( PID_x - PID_y - PID_z); speed2 = (int)( -PID_x - PID_y + PID_z); speed3 = (int)( -PID_x + PID_y - PID_z); |
|
相关推荐
2个回答
|
|
问题1、读数据的Read_MPU6050(tp); 并没涉及0x3B~0x48的位置数据寄存器,那么填入tp[0]~tp[13]的是啥内容呢?
问题2、即便是能“特异的”读到数据,(依6050手册描述)那么tp中的14个数据也应为:去掉tp[0]和tp[13]数据帧头尾,从tp[1]~tp[12]顺序分6组16位数据,分别表达加速度x、y、z和角速度x、y、z。而处理程序中却只用到tp[0]~tp[2]和tp[4]~tp[6],嘛意思? |
|
|
|
zdzdzddd 发表于 2017-11-10 21:24 方才看明白mpu6050的STC读取代码。 看来是读取数据的搞法有所不同罢了,比如一咕噜的读出14个8位的数据寄存器,然后从容处理。这比俺原来实验的分时读出然后火急火燎的处理搞法,显然要靠谱些。 估摸原来的数据大幅跳变无常的等等恼人毛病有望得以缓解。 果真得逞的话,俺的STC仪表实验就可使用4块钱的单芯片mpu6050 模块,何必荒唐地采用20块的带单片机处理的模块? 赏析: void Read_MPU6050(u8 *buf) { //俺:读取由”0x3B“开始的14个单字节数据寄存器至”tp“缓存 u8 i; I2C_Start(); //起始信号 I2C_SendByte(SlaveAddress); //发送设备地址+写信号 I2C_SendByte(ACCEL_XOUT_H); //内部寄存器地址=0x3B I2C_Start(); //起始信号 I2C_SendByte(SlaveAddress+1); //发送设备地址+读信号 for(i=0; i<13; i++) //俺:每读取寄存器后地址自动跳升 { //俺:其中buf[6]、buf[7]为温度数据 buf = I2C_RecvByte(); //读出寄存器数据 SDA = 0; //写应答信号 SCL = 1; //拉高时钟线 Delay2us(); //延时 SCL = 0; //拉低时钟线 Delay2us(); //延时 } buf = I2C_RecvByte(); //最后一个字节 SDA = 1; //写非应答信号 SCL = 1; //拉高时钟线 Delay2us(); //延时 SCL = 0; //拉低时钟线 Delay2us(); //延时 I2C_Stop(); //停止信号 } 。。。。。。。。。。。。 //******************************************************************************************** void main(void) { 。。。。。。。。。。。。 Read_MPU6050(tp); //680us Angle_ax = ((float)(((int *)&tp)[0])) / 8192.0; //加速度处理 结果单位是 +- g //俺:把”tp“缓存强制转换为双字节数据。搞法简约、精彩。 Angle_ay = ((float)(((int *)&tp)[1])) / 8192.0; //转换关系 8192 LSB/g, 1g对应读数8192 Angle_az = ((float)(((int *)&tp)[2])) / 8192.0; //加速度量程 +-4g/S Last_Angle_gx = Angle_gx; //储存上一次角速度数据 Last_Angle_gy = Angle_gy; Angle_gx = ((float)(((int *)&tp)[4] - g_x)) / 65.5; //陀螺仪处理 结果单位是 +-度 Angle_gy = ((float)(((int *)&tp)[5] - g_y)) / 65.5; //陀螺仪量程 +-500度/S, 1度/秒 对应读数 65.536 Angle_gz = ((float)(((int *)&tp)[6] - g_z)) / 65.5; //转换关系65.5 LSB/度 IMUupdate(Angle_gx*0.0174533f, Angle_gy*0.0174533f, Angle_gz*0.0174533f, Angle_ax,Angle_ay,Angle_az); 。。。。。。。。。。。。。 STC四轴飞控例程下载: 四轴飞控-STC8A8K64S4A12-LQFP44-PPM-V10.rar (3958 K) 下载次数:1 http://www.stcmcudata.com/STC8F-DATASHEET/%E5%9B%9B%E8%BD%B4%E9%A3%9E%E6%8E%A7-STC8A8K64S4A12-LQFP44-PPM-V10.rar |
|
|
|
你正在撰写答案
如果你是对答案或其他答案精选点评或询问,请使用“评论”功能。
嵌入式学习-飞凌嵌入式ElfBoard ELF 1板卡-使用AHT20进行环境监测之AHT20传感器介绍
203 浏览 0 评论
761 浏览 0 评论
806 浏览 1 评论
基于瑞萨FPB-RA4E2智能床头灯项目——1编译环境搭建与点亮驱动ws2812全彩LED
743 浏览 0 评论
嵌入式学习-飞凌嵌入式ElfBoard ELF 1板卡-LCD显示图片编程示例之介绍mmap
1195 浏览 0 评论
【youyeetoo X1 windows 开发板体验】少儿AI智能STEAM积木平台
11811 浏览 31 评论
小黑屋| 手机版| Archiver| 电子发烧友 ( 湘ICP备2023018690号 )
GMT+8, 2024-11-28 04:07 , Processed in 0.527501 second(s), Total 73, Slave 56 queries .
Powered by 电子发烧友网
© 2015 bbs.elecfans.com
关注我们的微信
下载发烧友APP
电子发烧友观察
版权所有 © 湖南华秋数字科技有限公司
电子发烧友 (电路图) 湘公网安备 43011202000918 号 电信与信息服务业务经营许可证:合字B2-20210191 工商网监 湘ICP备2023018690号