123 lines
2.5 KiB
C++
123 lines
2.5 KiB
C++
#include "stm32g4xx.h"
|
|
|
|
#include <string.h>
|
|
|
|
#include "uart.h"
|
|
#include "tick.h"
|
|
|
|
#include "sbus.h"
|
|
|
|
#pragma pack(push,1)
|
|
union SBUS_Struct
|
|
{
|
|
unsigned char data[25];
|
|
struct
|
|
{
|
|
unsigned start : 8;
|
|
unsigned ch1 : 11;
|
|
unsigned ch2 : 11;
|
|
unsigned ch3 : 11;
|
|
unsigned ch4 : 11;
|
|
unsigned ch5 : 11;
|
|
unsigned ch6 : 11;
|
|
unsigned ch7 : 11;
|
|
unsigned ch8 : 11;
|
|
unsigned ch9 : 11;
|
|
unsigned ch10 : 11;
|
|
unsigned ch11 : 11;
|
|
unsigned ch12 : 11;
|
|
unsigned ch13 : 11;
|
|
unsigned ch14 : 11;
|
|
unsigned ch15 : 11;
|
|
unsigned ch16 : 11;
|
|
unsigned flags : 8;
|
|
unsigned end : 8;
|
|
} bus;
|
|
};
|
|
#pragma pack(pop)
|
|
|
|
// IBUS: LEN(1)+CMD(1)+DATA(0..32)+CHSM(2)
|
|
|
|
#define SBUS_START 0x0F
|
|
|
|
#define SBUS_CH17 0x01
|
|
#define SBUS_CH18 0x02
|
|
|
|
#define SBUS_FRAMELOST 0x04
|
|
#define SBUS_FAILSAFE 0x08
|
|
|
|
#define SBUS_LENGTH 25
|
|
|
|
void SBUS_Init()
|
|
{
|
|
UART1_Init(100'000);
|
|
USART1->CR1 = USART_CR1_TE | USART_CR1_RE | USART_CR1_RXNEIE;
|
|
USART1->CR2 = USART_CR2_STOP_1 | USART_CR2_RXINV;
|
|
USART1->CR1 |= USART_CR1_PCE | USART_CR1_PS | USART_CR1_UE;
|
|
}
|
|
|
|
static const unsigned long Size = SBUS_LENGTH;
|
|
static char Buffer[Size];
|
|
|
|
static char Length = 0;
|
|
|
|
static unsigned long Time;
|
|
|
|
static bool Parse(SBUS_Data& Data, char byte)
|
|
{
|
|
unsigned long tick=TICK_GetCount();
|
|
|
|
unsigned long wait = tick - Time;
|
|
if (wait > 4) Length = 0; // Protocol synchronization lost !!!
|
|
|
|
Time=tick;
|
|
|
|
if (!Length && (byte != SBUS_START)) return false;
|
|
|
|
Buffer[Length++] = byte;
|
|
|
|
if(Length<Size) return false;
|
|
Length=0;
|
|
|
|
SBUS_Struct* frame=(SBUS_Struct*)Buffer;
|
|
if(frame->bus.end!=0) return false;
|
|
|
|
Data.X=frame->bus.ch1;
|
|
Data.Y=frame->bus.ch2;
|
|
Data.Z=frame->bus.ch3;
|
|
Data.W=frame->bus.ch4;
|
|
|
|
Data.SWA=frame->bus.ch5;
|
|
Data.SWB=frame->bus.ch6;
|
|
Data.SWC=frame->bus.ch7;
|
|
Data.SWD=frame->bus.ch8;
|
|
|
|
Data.VRA=frame->bus.ch9;
|
|
Data.VRB=frame->bus.ch10;
|
|
|
|
Data.OTHER[0]=frame->bus.ch11;
|
|
Data.OTHER[1]=frame->bus.ch12;
|
|
Data.OTHER[2]=frame->bus.ch13;
|
|
Data.OTHER[3]=frame->bus.ch14;
|
|
Data.OTHER[4]=frame->bus.ch15;
|
|
Data.OTHER[5]=frame->bus.ch16;
|
|
|
|
Data.OTHER[6]=(bool)(frame->bus.flags & SBUS_CH17);
|
|
Data.OTHER[7]=(bool)(frame->bus.flags & SBUS_CH18);
|
|
|
|
Data.FailSafe=frame->bus.flags & SBUS_FAILSAFE;
|
|
Data.FrameLost=frame->bus.flags & SBUS_FRAMELOST;
|
|
|
|
return true;
|
|
}
|
|
|
|
bool SBUS_Update(SBUS_Data& Data, bool& Off)
|
|
{
|
|
char buf[Size];
|
|
unsigned long size = UART1_Recv(buf, Size);
|
|
|
|
bool done = false;
|
|
for (long a = 0; a < size; a++) done = Parse(Data, buf[a]);
|
|
Off=Data.FailSafe;
|
|
return done;
|
|
} |