VladislavOstapov/sdrpi-fpv-control
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases 4 Wiki Activity

фикс RXLOSS для sbus

Browse Source
This commit is contained in:
Vladislav Ostapov
2025-12-17 15:39:51 +03:00
parent 1b5bbf2755
commit 8d17d6faab
1 changed files with 65 additions and 49 deletions
Download Patch File Download Diff File Expand all files Collapse all files

114
air/main.cpp
View File

@@ -23,7 +23,7 @@ inline int64_t milliseconds() {
return ((tv.tv_sec * 1000000l) + tv.tv_usec) / 1000; return ((tv.tv_sec * 1000000l) + tv.tv_usec) / 1000;
} }
constexpr int64_t SBUS_SEND_FRAME_INTERVAL = 25; constexpr int64_t SBUS_SEND_FRAME_INTERVAL = 15;
constexpr int64_t SBUS_RXLOSS_INTERVAL = 500; constexpr int64_t SBUS_RXLOSS_INTERVAL = 500;
class UDPServer { class UDPServer {
@@ -94,9 +94,9 @@ struct SbusData {
bool lost_frame = false; bool lost_frame = false;
bool failsafe = false; bool failsafe = false;
bool ch17 = false, ch18 = false; bool ch17 = false, ch18 = false;
static constexpr size_t NUM_CH = 16; static constexpr size_t NUM_CH = 18;
static constexpr int16_t SBUS_CH_MIN = 173; static constexpr int16_t SBUS_CH_MIN = 173 + 10;
static constexpr int16_t SBUS_CH_MAX = 1812; static constexpr int16_t SBUS_CH_MAX = 1812 - 10;
int16_t ch[NUM_CH]; int16_t ch[NUM_CH];
/* Message len */ /* Message len */
@@ -110,52 +110,68 @@ struct SbusData {
static constexpr uint8_t CH18_MASK_ = 0x02; static constexpr uint8_t CH18_MASK_ = 0x02;
static constexpr uint8_t LOST_FRAME_MASK_ = 0x04; static constexpr uint8_t LOST_FRAME_MASK_ = 0x04;
static constexpr uint8_t FAILSAFE_MASK_ = 0x08; static constexpr uint8_t FAILSAFE_MASK_ = 0x08;
/* Data */ uint8_t binaryBuffer[25];
uint8_t buf_[BUF_LEN_];
void fillDataBuf() { void fillDataBuf() {
/* Assemble packet */ typedef struct sbusChannels_s {
buf_[0] = HEADER_; // 176 bits of data (11 bits per channel * 16 channels) = 22 bytes.
buf_[1] = static_cast<uint8_t>((ch[0] & 0x07FF)); unsigned int chan0 : 11;
buf_[2] = static_cast<uint8_t>((ch[0] & 0x07FF) >> 8 | unsigned int chan1 : 11;
(ch[1] & 0x07FF) << 3); unsigned int chan2 : 11;
buf_[3] = static_cast<uint8_t>((ch[1] & 0x07FF) >> 5 | unsigned int chan3 : 11;
(ch[2] & 0x07FF) << 6); unsigned int chan4 : 11;
buf_[4] = static_cast<uint8_t>((ch[2] & 0x07FF) >> 2); unsigned int chan5 : 11;
buf_[5] = static_cast<uint8_t>((ch[2] & 0x07FF) >> 10 | unsigned int chan6 : 11;
(ch[3] & 0x07FF) << 1); unsigned int chan7 : 11;
buf_[6] = static_cast<uint8_t>((ch[3] & 0x07FF) >> 7 | unsigned int chan8 : 11;
(ch[4] & 0x07FF) << 4); unsigned int chan9 : 11;
buf_[7] = static_cast<uint8_t>((ch[4] & 0x07FF) >> 4 | unsigned int chan10 : 11;
(ch[5] & 0x07FF) << 7); unsigned int chan11 : 11;
buf_[8] = static_cast<uint8_t>((ch[5] & 0x07FF) >> 1); unsigned int chan12 : 11;
buf_[9] = static_cast<uint8_t>((ch[5] & 0x07FF) >> 9 | unsigned int chan13 : 11;
(ch[6] & 0x07FF) << 2); unsigned int chan14 : 11;
buf_[10] = static_cast<uint8_t>((ch[6] & 0x07FF) >> 6 | unsigned int chan15 : 11;
(ch[7] & 0x07FF) << 5); } __attribute__((__packed__)) sbusChannels_t;
buf_[11] = static_cast<uint8_t>((ch[7] & 0x07FF) >> 3); static_assert(sizeof(sbusChannels_s) == 22);
buf_[12] = static_cast<uint8_t>((ch[8] & 0x07FF)); struct sbusFrame_s {
buf_[13] = static_cast<uint8_t>((ch[8] & 0x07FF) >> 8 | uint8_t syncByte = HEADER_;
(ch[9] & 0x07FF) << 3); sbusChannels_t channels{};
buf_[14] = static_cast<uint8_t>((ch[9] & 0x07FF) >> 5 | uint8_t flags{};
(ch[10] & 0x07FF) << 6); /**
buf_[15] = static_cast<uint8_t>((ch[10] & 0x07FF) >> 2); * The endByte is 0x00 on FrSky and some futaba RX's, on Some SBUS2 RX's the value indicates the telemetry byte that is sent after every 4th sbus frame.
buf_[16] = static_cast<uint8_t>((ch[10] & 0x07FF) >> 10 | *
(ch[11] & 0x07FF) << 1); * See https://github.com/cleanflight/cleanflight/issues/590#issuecomment-101027349
buf_[17] = static_cast<uint8_t>((ch[11] & 0x07FF) >> 7 | * and
(ch[12] & 0x07FF) << 4); * https://github.com/cleanflight/cleanflight/issues/590#issuecomment-101706023
buf_[18] = static_cast<uint8_t>((ch[12] & 0x07FF) >> 4 | */
(ch[13] & 0x07FF) << 7); uint8_t endByte = FOOTER_;
buf_[19] = static_cast<uint8_t>((ch[13] & 0x07FF) >> 1); } __attribute__ ((__packed__));
buf_[20] = static_cast<uint8_t>((ch[13] & 0x07FF) >> 9 | static_assert(sizeof(sbusFrame_s) == sizeof(binaryBuffer));
(ch[14] & 0x07FF) << 2);
buf_[21] = static_cast<uint8_t>((ch[14] & 0x07FF) >> 6 | auto* dest = reinterpret_cast<sbusFrame_s*>(binaryBuffer);
(ch[15] & 0x07FF) << 5); dest->syncByte = HEADER_;
buf_[22] = static_cast<uint8_t>((ch[15] & 0x07FF) >> 3); dest->channels.chan0 = std::min(std::max(ch[0], SBUS_CH_MIN), SBUS_CH_MAX);
buf_[23] = 0x00 | (ch17 * CH17_MASK_) | (ch18 * CH18_MASK_) | dest->channels.chan1 = std::min(std::max(ch[1], SBUS_CH_MIN), SBUS_CH_MAX);
(failsafe * FAILSAFE_MASK_) | dest->channels.chan2 = std::min(std::max(ch[2], SBUS_CH_MIN), SBUS_CH_MAX);
(lost_frame * LOST_FRAME_MASK_); dest->channels.chan3 = std::min(std::max(ch[3], SBUS_CH_MIN), SBUS_CH_MAX);
buf_[24] = FOOTER_; dest->channels.chan4 = std::min(std::max(ch[4], SBUS_CH_MIN), SBUS_CH_MAX);
dest->channels.chan5 = std::min(std::max(ch[5], SBUS_CH_MIN), SBUS_CH_MAX);
dest->channels.chan6 = std::min(std::max(ch[6], SBUS_CH_MIN), SBUS_CH_MAX);
dest->channels.chan7 = std::min(std::max(ch[7], SBUS_CH_MIN), SBUS_CH_MAX);
dest->channels.chan8 = std::min(std::max(ch[8], SBUS_CH_MIN), SBUS_CH_MAX);
dest->channels.chan9 = std::min(std::max(ch[9], SBUS_CH_MIN), SBUS_CH_MAX);
dest->channels.chan10 = std::min(std::max(ch[10], SBUS_CH_MIN), SBUS_CH_MAX);
dest->channels.chan11 = std::min(std::max(ch[11], SBUS_CH_MIN), SBUS_CH_MAX);
dest->channels.chan12 = std::min(std::max(ch[12], SBUS_CH_MIN), SBUS_CH_MAX);
dest->channels.chan13 = std::min(std::max(ch[13], SBUS_CH_MIN), SBUS_CH_MAX);
dest->channels.chan14 = std::min(std::max(ch[14], SBUS_CH_MIN), SBUS_CH_MAX);
dest->channels.chan15 = std::min(std::max(ch[15], SBUS_CH_MIN), SBUS_CH_MAX);
dest->flags = 0;
if (ch[16] >= (SBUS_CH_MAX + SBUS_CH_MAX) / 2) { dest->flags |= CH17_MASK_; }
if (ch[16] >= (SBUS_CH_MAX + SBUS_CH_MAX) / 2) { dest->flags |= CH18_MASK_; }
dest->endByte = FOOTER_;
} }
}; };
@@ -295,7 +311,7 @@ int main(int argc, char* argv[]) {
const auto now = milliseconds(); const auto now = milliseconds();
if (now - lastSbusWrite >= SBUS_SEND_FRAME_INTERVAL && now - lastSbusRecv <= SBUS_RXLOSS_INTERVAL) { if (now - lastSbusWrite >= SBUS_SEND_FRAME_INTERVAL && now - lastSbusRecv <= SBUS_RXLOSS_INTERVAL) {
lastSbusWrite = now; lastSbusWrite = now;
if (!serial.write(sb.buf_)) { if (!serial.write(sb.binaryBuffer)) {
break; break;
} }
} }