Chisel(Constructing Hardware In a Scala Embedded Language)是UC Berkeley开发的一种开源硬件构造语言。
站长xddcore有话说:在我大二的时候,因为项目需要,接触了Chisel。在体验过后,我被它深深的吸引了。我幻想着它十年后的样子,充满希望。于是创建了这个博客,让更多人的了解Chisel,学习Chisel。

深度解析FUTABA的SBUS协议(/天地飞遥控器的WBUS协议/Robomaster接收机的DBUS协议)到底是啥?

内容纲要
Reading Time: < 1 minute

写在前面:

无论是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三极管搭个非门就行了。
为什么软件手段取反不行?
因为SBUS和RS232是翻转关系。RS232的0是SBUS里面的1,因为这个翻转指得不仅是数据位的翻转,也有起始位和结束位的翻转。这个起始位和结束位是硬件串口自动处理的。你只能读到并翻转数据的buffer。无法翻转起始位和结束位(这是硬件串口自动处理,也是串口判断一帧数据是否开始/结束的依据)。

讲完了预备知识,我们现在来看软件实现(我用的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);
    }
}

如有问题欢迎私信,或email:1034029664@qq.com

Share

xddcore

xddcore www.github.com/xddcore

You may also like...

发表回复

您的电子邮箱地址不会被公开。