寫在前面:
無論是SBUS(日本FUTABA,所以航模,車模愛好者都知道的公司,一個好點遙控器近萬了),還是WBUS(天地飛遙控器接收機用),亦或者DBUS(Robomaster官方接收機用)。
劃重點:
我們可以適當的認爲,這三種協議其實全部都是由FUTABA公司最早的SBUS協議,換了個名字過來的。其協議解碼代碼目前完全兼容。
起源
樓主第一次知道SBUS協議,是因爲當時高三比較閒,然後想着用松果派ONE(swm320vet7)做一個無人機飛控。當時一開始想着做個四軸飛控的,但是對於串級PID不是很會。。。所以就轉戰固定翼了(最後就做了個角度環的單環PID,比較穩)。當然代碼也是全註釋,完全開源的。大家有興趣可以看看。
https://gitee.com/pineconepi/PineconePi_Pilot
當時,樓主用的是,天地飛7WTF-07遙控器(2018版),因爲要做開源飛控,所以自然需要研究遙控器接受的協議。當時接收機傳輸遙控器舵杆值普遍的方式有兩種:
1.通過PWM脈寬來表示舵杆值,每個通道都需要產生一路PWM。接收端(也就是負責解碼SBUS單片機)需要捕獲PWM脈寬。當時樓主一想,我遙控器8通道,要接8根線在松果派ONE飛控上,還得弄8路PWM脈寬捕獲,太累了。SBUS1根線它不香嗎?
2.通過SBUS協議(1根線,可看作Serial Tx),傳輸高達16通道的數據。以下是SBUS協議的介紹:
S.BUS可以傳輸16個比例通道和2個數字(bool)通道。其硬件上基於RS232協議,採用TTL電平,但高位取反(負邏輯,低電平爲“1”,高電平爲“0”),通信波特率爲100K(不兼容波特率115200)。
Start Byte = 0x0F。中間32個字節爲16個通道的數據,每個通道用就是一個整形數,範圍是0-2047。 高字節在前,低字節在後。
XOR(校驗碼)爲包括頭字節所有34個字節的異或校驗。
Flags的定義:
bit7 = ch17 = digital channel (0x80)
bit6 = ch18 = digital channel (0x40)
bit5 = Frame lost, equivalent red LED on receiver (0x20)
bit4 = failsafe activated (0x10)
bit3 = n/a
bit2 = n/a
bit1 = n/a
bit0 = n/a
看完之後,樓主真的是覺得SBUS協議香香的。
繼續劃重點:
問題來了,看過上述對於SBUS協議的介紹後,咦?電平是負邏輯的?
樓主一開始,想,負邏輯簡單啊,我直接把接收機傳過來的數收過來,然後按位取反就行了。(我當時覺得自己真的是個天才,軟件手段解決真的香)。 但是,經過我的實測,這樣的辦法是不行的。
那沒招了,只能老老實實用硬件手段了。就用個npn三極管搭個非門就行了。
講完了預備知識,我們現在來看軟件實現(我用的swm320,但實際上,硬件平臺是啥真的無所謂,解碼邏輯大家可以直接移植,樓主當時正好與或非玩得比較愉快,所以就有了如下代碼:
SBUS.h
#ifndef _SBUS_H_
#define _SBUS_H_
#include "swm320.h"
#define SBUS_RX_LEN 25 //25ֽ
#define StartByte 0x0f
#define EndByte 0x00
typedef struct{//25ֽSBUS洢ṹ
uint8_t Start;
uint16_t Ch1;
uint16_t Ch2;
uint16_t Ch3;
uint16_t Ch4;
uint16_t Ch5;
uint16_t Ch6;
uint16_t Ch7;
uint16_t Ch8;
uint16_t Ch9;
uint16_t Ch10;
uint16_t Ch11;
uint16_t Ch12;
uint16_t Ch13;
uint16_t Ch14;
uint16_t Ch15;
uint16_t Ch16;
uint8_t Flag;
uint8_t End;
}SBUS_Buffer;
void SBUS_Init(void);
void SBUS_Handle(void);
#endif
SBUS.c
#include "SBUS.h"
SBUS_Buffer SBUS;
uint8_t First_Byte_flag_SBUS=1; //首字節標誌
uint8_t SBUS_RX_Finish=0;
uint8_t SBUS_RXIndex = 0;//當前接收字節數
uint8_t SBUS_RXBuffer[SBUS_RX_LEN] = {0};//接收緩衝
void SBUS_Init()
{
//串口配置爲波特率100kbps,
//8位數據,偶校驗(even),2位停止位,無流控。
//25字節
UART_InitStructure UART_initStruct;
PORT_Init(PORTC, PIN4, FUNMUX0_UART1_RXD, 1); //GPIOC.4配置爲UART1輸入引腳
PORT_Init(PORTC, PIN3, FUNMUX1_UART1_TXD, 0); //GPIOC.3配置爲UART1輸出引腳
UART_initStruct.Baudrate = 100000;
UART_initStruct.DataBits = UART_DATA_8BIT;
UART_initStruct.Parity = UART_PARITY_EVEN;
UART_initStruct.StopBits = UART_STOP_2BIT;
UART_initStruct.RXThreshold = 1;
UART_initStruct.RXThresholdIEn = 1;
UART_Init(UART1, &UART_initStruct);
UART_Open(UART1);
printf("SBUS Init [OK]!\r\n");
}
void UART1_Handler(void)//中斷函數
{
uint32_t chr;
if(UART_INTRXThresholdStat(UART1) || UART_INTTimeoutStat(UART1))
{
while(UART_IsRXFIFOEmpty(UART1) == 0)
{
if(UART_ReadByte(UART1, &chr) == 0)
{
if((chr==0x0f)||(First_Byte_flag_SBUS==0))
{
First_Byte_flag_SBUS=0;//首字節已識別
if(SBUS_RXIndex < (SBUS_RX_LEN-2))
{
SBUS_RXBuffer[SBUS_RXIndex] = chr;
SBUS_RXIndex++;
}
else if(SBUS_RXIndex < (SBUS_RX_LEN-1))
{
SBUS_RXBuffer[SBUS_RX_LEN-1] = chr;
First_Byte_flag_SBUS=1;//準備再次識別首字節
SBUS_RXIndex=0; //完成一幀(25字節)數據接收,準備下一次接收
if((SBUS_RXBuffer[0]==StartByte)&&(SBUS_RXBuffer[24]==EndByte))
{
// printf("SBUS RX Success!");
SBUS_RX_Finish=1;//接收成功
}
else
{
SBUS_RX_Finish=0;//接收失敗
First_Byte_flag_SBUS=1;//準備再次識別首字節
SBUS_RXIndex=0; //接收失敗,準備下一次接收
// printf("SBUS RX Error!");
}
}
}
}
}
}
}
void SBUS_Handle()
{
if(SBUS_RX_Finish==1)
{
SBUS_RX_Finish=0;//準備下一次接收
NVIC_DisableIRQ(UART1_IRQn);//從UART_RXBuffer讀取數據過程中要關閉中斷,防止讀寫混亂
SBUS.Start=SBUS_RXBuffer[0];
SBUS.Ch1=((uint16_t)SBUS_RXBuffer[1])|((uint16_t)((SBUS_RXBuffer[2]&0x07)<<8));
SBUS.Ch2=((uint16_t)((SBUS_RXBuffer[2]&0xf8)>>3))|(((uint16_t)(SBUS_RXBuffer[3]&0x3f))<<6);
SBUS.Ch3=((uint16_t)((SBUS_RXBuffer[3]&0xc0)>>6))|((((uint16_t)SBUS_RXBuffer[4])<<2))|(((uint16_t)(SBUS_RXBuffer[5]&0x01))<<10);
SBUS.Ch4=((uint16_t)((SBUS_RXBuffer[5]&0xfe)>>1))|(((uint16_t)(SBUS_RXBuffer[6]&0x0f))<<7);
SBUS.Ch5=((uint16_t)((SBUS_RXBuffer[6]&0xf0)>>4))|(((uint16_t)(SBUS_RXBuffer[7]&0x7f))<<4);
SBUS.Ch6=((uint16_t)((SBUS_RXBuffer[7]&0x80)>>7))|(((uint16_t)SBUS_RXBuffer[8])<<1)|(((uint16_t)(SBUS_RXBuffer[9]&0x03))<<9);
SBUS.Ch7=((uint16_t)((SBUS_RXBuffer[9]&0xfc)>>2))|(((uint16_t)(SBUS_RXBuffer[10]&0x1f))<<6);
SBUS.Ch8=((uint16_t)((SBUS_RXBuffer[10]&0xe0)>>5))|(((uint16_t)(SBUS_RXBuffer[11]))<<3);
SBUS.Ch9=((uint16_t)SBUS_RXBuffer[12])|(((uint16_t)(SBUS_RXBuffer[13]&0x07))<<8);
SBUS.Ch10=((uint16_t)((SBUS_RXBuffer[13]&0xf8)>>3))|(((uint16_t)(SBUS_RXBuffer[14]&0x3f))<<5);
SBUS.Ch11=((uint16_t)((SBUS_RXBuffer[14]&0xc0)>>6))|(((uint16_t)SBUS_RXBuffer[15])<<2)|(((uint16_t)(SBUS_RXBuffer[16]&0x01))<<10);
SBUS.Ch12=((uint16_t)((SBUS_RXBuffer[16]&0xfe)>>1))|(((uint16_t)(SBUS_RXBuffer[17]&0x0f))<<7);
SBUS.Ch13=((uint16_t)((SBUS_RXBuffer[17]&0xf0)>>4))|(((uint16_t)(SBUS_RXBuffer[18]&0x7f))<<4);
SBUS.Ch14=((uint16_t)((SBUS_RXBuffer[18]&0x80)>>7))|(((uint16_t)SBUS_RXBuffer[19])<<1)|(((uint16_t)(SBUS_RXBuffer[20]&0x03))<<9);
SBUS.Ch15=((uint16_t)((SBUS_RXBuffer[20]&0xfc)>>2))|(((uint16_t)(SBUS_RXBuffer[21]&0x1f))<<6);
SBUS.Ch16=((uint16_t)((SBUS_RXBuffer[21]&0xe0)>>5))|(((uint16_t)SBUS_RXBuffer[22])<<3);
SBUS.Flag=SBUS_RXBuffer[23];
SBUS.End=SBUS_RXBuffer[24];
NVIC_EnableIRQ(UART1_IRQn);
}
}