ickey_LYL
获得 11 次赞
帮助过201人
MPU6050模块是InvenSense公司推出的一款低成本的6轴传感器模块,包括三轴加速度,三轴角速度。其体积小巧,用途非常广。但实际应用的时候,直接使用的确不是这些量,而是需要根据这些数据解算出三轴的角度数据。比如平衡小车,需要算出 模块的俯仰角,然后控制算法根据角度大小控制小车轮子的移动。四轴飞行器需要根据俯仰角度、滚转角度,和飞行指令来调节四个电机的转速。
int fFifoPush(CFifo *myfifo, UCHAR c){ UINT tmp_len; if(myfifo->fullflag != FULL){ tmp_len = myfifo->m_wp; myfifo->m_buf[tmp_len] = c; tmp_len++; if (tmp_len >= myfifo->m_bufsize) tmp_len = 0; if (tmp_len == myfifo->m_rp) { myfifo-> fullflag = FULL; } myfifo->m_wp = tmp_len; myfifo-> emptyflag = !EMPTY; return TRUE; } else return ERROR_FULL_W ; }3.从队列中读出数据int fFifoPop(CFifo *myfifo, int *data){ UINT tmp_len; UCHAR tmp_char; if(myfifo->emptyflag != EMPTY){ tmp_len = myfifo->m_rp; tmp_char = myfifo->m_buf[tmp_len]; *data = tmp_char; tmp_len++; if (tmp_len >= myfifo->m_bufsize) tmp_len = 0; myfifo->m_rp = tmp_len; if (tmp_len == myfifo->m_wp) { myfifo->emptyflag = EMPTY; } myfifo-> fullflag = !FULL; return TRUE; } else return ERROR_EMPTY_R;}4.计算队列中数据长度UINT fFifoCheckLen(CFifo *myfifo){ if(myfifo->emptyflag == EMPTY) return 0; else if(myfifo->fullflag == FULL) return myfifo->m_bufsize; if(myfifo->m_wp >= myfifo->m_rp ){ return myfifo->m_wp - myfifo->m_rp ; } else{ return myfifo->m_bufsize - myfifo->m_rp + myfifo->m_wp; }}