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