mirror of
https://github.com/betaflight/betaflight.git
synced 2025-07-23 08:15:30 +03:00
1f293795e7
match the comment from pullrequest about spacing remains : some hand alignment for comment and wrong /** */ usage.
76 lines
1.9 KiB
C
76 lines
1.9 KiB
C
#include "board.h"
|
|
#include "mw.h"
|
|
|
|
// driver for SUMD receiver using UART2
|
|
|
|
#define SUMD_SYNCBYTE 0xA8
|
|
#define SUMD_MAX_CHANNEL 8
|
|
#define SUMD_BUFFSIZE (SUMD_MAX_CHANNEL * 2 + 5) // 6 channels + 5 -> 17 bytes for 6 channels
|
|
|
|
static bool sumdFrameDone = false;
|
|
static void sumdDataReceive(uint16_t c);
|
|
static uint16_t sumdReadRawRC(uint8_t chan);
|
|
|
|
static uint32_t sumdChannelData[SUMD_MAX_CHANNEL];
|
|
|
|
void sumdInit(rcReadRawDataPtr *callback)
|
|
{
|
|
core.rcvrport = uartOpen(USART2, sumdDataReceive, 115200, MODE_RX);
|
|
if (callback)
|
|
*callback = sumdReadRawRC;
|
|
}
|
|
|
|
static uint8_t sumd[SUMD_BUFFSIZE] = { 0, };
|
|
static uint8_t sumdSize;
|
|
|
|
// Receive ISR callback
|
|
static void sumdDataReceive(uint16_t c)
|
|
{
|
|
uint32_t sumdTime;
|
|
static uint32_t sumdTimeLast;
|
|
static uint8_t sumdIndex;
|
|
|
|
sumdTime = micros();
|
|
if ((sumdTime - sumdTimeLast) > 4000)
|
|
sumdIndex = 0;
|
|
sumdTimeLast = sumdTime;
|
|
|
|
if (sumdIndex == 0) {
|
|
if (c != SUMD_SYNCBYTE)
|
|
return;
|
|
else
|
|
sumdFrameDone = false; // lazy main loop didnt fetch the stuff
|
|
}
|
|
if (sumdIndex == 2)
|
|
sumdSize = (uint8_t)c;
|
|
if (sumdIndex < SUMD_BUFFSIZE)
|
|
sumd[sumdIndex] = (uint8_t)c;
|
|
sumdIndex++;
|
|
if (sumdIndex == sumdSize * 2 + 5) {
|
|
sumdIndex = 0;
|
|
sumdFrameDone = true;
|
|
}
|
|
}
|
|
|
|
bool sumdFrameComplete(void)
|
|
{
|
|
uint8_t b;
|
|
|
|
if (sumdFrameDone) {
|
|
sumdFrameDone = false;
|
|
if (sumd[1] == 0x01) {
|
|
failsafeCnt = 0;
|
|
if (sumdSize > SUMD_MAX_CHANNEL)
|
|
sumdSize = SUMD_MAX_CHANNEL;
|
|
for (b = 0; b < sumdSize; b++)
|
|
sumdChannelData[b] = ((sumd[2 * b + 3] << 8) | sumd[2 * b + 4]);
|
|
return true;
|
|
}
|
|
}
|
|
return false;
|
|
}
|
|
|
|
static uint16_t sumdReadRawRC(uint8_t chan)
|
|
{
|
|
return sumdChannelData[mcfg.rcmap[chan]] / 8;
|
|
}
|