聚豐項(xiàng)目 > 基于AB32VG1的姿態(tài)檢測(cè)
本項(xiàng)目基于中科藍(lán)汛AB32VG1開(kāi)發(fā)板，制作了一個(gè)姿態(tài)檢測(cè)系統(tǒng)。維特智能的高精度姿態(tài)傳感器JY901S模塊，該集成高精度的陀螺儀、加速度計(jì)、地磁場(chǎng)傳感器，采用高性能的微處理器和先進(jìn)的動(dòng)力學(xué)解算與卡爾曼動(dòng)態(tài)濾波算法，能夠快速求解出模塊當(dāng)前的實(shí)時(shí)運(yùn)動(dòng)姿態(tài)。模塊支持IIC和串口兩種數(shù)字接口，本設(shè)計(jì)應(yīng)用串口接口完成姿態(tài)傳感器與開(kāi)發(fā)板的通信。顯示屏選擇0.96寸IIC通信的OLED屏，對(duì)讀取到的姿態(tài)角度進(jìn)行顯示。
ELECFANS_WYC
ELECFANS_WYC
團(tuán)隊(duì)成員
ELECFANS_WYC 電子愛(ài)好者
1、OLED：PE3--OLED_SCL、PE2--OLED_SDA、電源引腳；
2、JY901S（九軸姿態(tài)傳感器）
2.1 引腳說(shuō)明：PA3--JY_TX、PA4--JY_RX、電源引腳。
2.2、性能參數(shù)
電壓：3.3V~5V；電流：<25mA；
測(cè)量維度：加速度-3維，角速度-3維，磁場(chǎng)-3維；
量程：加速度：±2/4/8/16g，角速度：±250/500/1000/2000°/s，角度X、Z軸±180°，Y軸±90°。磁場(chǎng)：30Gauss；
穩(wěn)定性:加速度:0.01g，角速度0.05°/s。
姿態(tài)測(cè)量穩(wěn)定度:0.01°。
軟件框架：軟件部分使用到了板卡的USART1和IIC兩種通信，首先對(duì)兩個(gè)模塊初始化，然后開(kāi)啟按協(xié)議接收串口數(shù)據(jù)線程和OLED顯示線程。流程圖如下所示。
軟件流程圖
USART1配置：
協(xié)議說(shuō)明：
角度輸出
| 0x55 | 0x53 | RollL | RollH | PitchL | PitchH | YawL | YawH | VL | VH | SUM |
| 幀頭 | 功能 | data1L | data1H | data2L | data2H | data3L | data3H | VersionH | VersionL | 校驗(yàn)和 |
計(jì)算方法：
俯仰角（y軸）Pitch=((PitchH<<8)|PitchL)/32768*180(°)
Version=(VH<<8)|VL
Sum=0x55+0x53+RollH+RollL+PitchH+PitchL+YawH+YawL+VH+VL
姿態(tài)傳感器向上位機(jī)傳輸數(shù)據(jù)使用了基于串口的特定協(xié)議，一幀數(shù)據(jù)中包含幀頭、功能字節(jié)、數(shù)據(jù)字節(jié)、版本信息和校驗(yàn)字節(jié)。因此編寫串口讀取程序時(shí)應(yīng)按照如下邏輯：首先尋找?guī)^，然后匹配功能字節(jié)來(lái)判斷此幀數(shù)據(jù)為什么內(nèi)容，接著接收接下來(lái)的若干字節(jié)數(shù)據(jù)，最后根據(jù)校驗(yàn)字節(jié)校驗(yàn)數(shù)據(jù)是否接收正確。流程如下圖所示：
串口讀取協(xié)議流程圖
串口1數(shù)據(jù)讀?。ê琂Y901S串口數(shù)據(jù)讀取協(xié)議）：
//串口1數(shù)據(jù)讀取線程入口
static void serial_thread_entry(void *parameter)
{
uint8_t Res_U3;
uint8_t Sum_U3=0,i_U3;
while (1)
{
/* 從串口讀取一個(gè)字節(jié)的數(shù)據(jù)，沒(méi)有讀取到則等待接收信號(hào)量 */
while (rt_device_read(serial, -1, &Res_U3, 1) != 1)
{
/* 阻塞等待接收信號(hào)量，等到信號(hào)量后再次讀取數(shù)據(jù) */
rt_sem_take(&rx_sem, RT_WAITING_FOREVER);
}
/* 讀取到的數(shù)據(jù)輸出 */
//rt_kprintf("%c",Res_U3);
if((USART3_REC_STA&0x8000)==0)//接收未完成
{
if((USART3_REC_STA&0x00FF)>11)//數(shù)據(jù)長(zhǎng)度出錯(cuò)
{
USART3_REC_STA=0;////接收錯(cuò)誤，重新接收
//rt_kprintf("->1\n");
}
else
{
if((USART3_REC_STA&0x4000)==0)//未接收到幀頭
{
if(Res_U3==0x55) //本字節(jié)為幀頭
{
USART3_REC_STA |= 0x4000;
USART3_REC_BUF[USART3_REC_STA&0x00FF]=Res_U3;
USART3_REC_STA++;
//rt_kprintf("->2\n");
}
else {
//rt_kprintf("->3\n");
}
}
else//已接收到幀頭
{
if((USART3_REC_STA&0x2000)==0)//未接收到標(biāo)識(shí)字節(jié)
{
if((Res_U3==0x53)||(0))//本字節(jié)為標(biāo)識(shí)字節(jié) (Res_U3==0x54)
{
USART3_REC_STA |= 0x2000;
USART3_REC_BUF[USART3_REC_STA&0x00FF]=Res_U3;
USART3_REC_STA++;
//rt_kprintf("->4\n");
}
else
{
USART3_REC_STA=0;//接收錯(cuò)誤，重新接收
//rt_kprintf("->5 Res_U3=%X\n",Res_U3);
}
}
else//已接收到標(biāo)識(shí)字節(jié)
{
if((USART3_REC_STA&0x00FF)<11)//
{
USART3_REC_BUF[USART3_REC_STA&0x00FF]=Res_U3;
USART3_REC_STA++;
//rt_kprintf("->6\n");
if((USART3_REC_STA&0x00FF)==11)
{
//求和
Sum_U3=0;//清零
for(i_U3=0;i_U3<10;i_U3++)
{
Sum_U3=(uint8_t)(Sum_U3+USART3_REC_BUF[i_U3]);
}
//校驗(yàn)和
if(Sum_U3==USART3_REC_BUF[10])
{
USART3_REC_STA |= 0x8000;//校驗(yàn)和成功，接收完成
//rt_kprintf("rec success!\n");
}
else
{
USART3_REC_STA=0;//校驗(yàn)和失敗，重新接收
//rt_kprintf("->7\n");
}
}
}
else {
//rt_kprintf("->8\n");
}
}
}
}
}
}
}
姿態(tài)傳感器數(shù)據(jù)分析：
//數(shù)據(jù)分析函數(shù)
void Data_analysis(uint8_t fun_byte)
{
uint8_t DataH,DataL;
if((USART3_REC_STA&0x8000)==0) return;
switch(fun_byte)
{
case 0x53:
{
Roll_X= ((short)((USART3_REC_BUF[3]<<8)|USART3_REC_BUF[2]))/32768.0*180;
Pitch_Y=((short)((USART3_REC_BUF[5]<<8)|USART3_REC_BUF[4]))/32768.0*180;
Yaw_Z= ((short)((USART3_REC_BUF[7]<<8)|USART3_REC_BUF[6]))/32768.0*180;
}break;
default :USART3_REC_STA=0;break;
}
}
IIC OLED配置：
OLED姿態(tài)數(shù)據(jù)顯示：
static void thread_usart1_recsho(void *parameter)
{
while(1)
{
if(USART3_REC_STA&0x8000)//接收完成
{
Data_analysis(USART3_REC_BUF[1]);
sprintf((char*)print_str,"%3.1f",Roll_X);
ssd1306_SetCursor(66,8);
ssd1306_WriteString(print_str,Font_11x18,White);
sprintf((char*)print_str,"%3.1f",Pitch_Y);
ssd1306_SetCursor(66,8+18);
ssd1306_WriteString(print_str,Font_11x18,White);
sprintf((char*)print_str,"%3.1f",Yaw_Z);
ssd1306_SetCursor(66,8+18+18);
ssd1306_WriteString(print_str,Font_11x18,White);
ssd1306_UpdateScreen();
/*
sprintf((char*)print_str,"Roll_X:%3.1f\n",Roll_X);
rt_kprintf("%s",print_str);
sprintf((char*)print_str,"Pitch_Y:%3.1f\n",Pitch_Y);
rt_kprintf("%s",print_str);
sprintf((char*)print_str,"Yaw_Z:%3.1f\n",Yaw_Z);
rt_kprintf("%s",print_str);*/
USART3_REC_STA=0;
}
rt_thread_mdelay(10);
}
}
效果演示：
代碼見(jiàn)附件。
（11.74 MB）下載
幽幽鹿鳴： 代碼縮水，沒(méi)用
回復(fù)
ELECFANS_WYC： 好的親，我改改。
回復(fù)
工商網(wǎng)監(jiān)