From 8d17d6faabb709bcd44a1c8398118550128117f4 Mon Sep 17 00:00:00 2001 From: Vladislav Ostapov Date: Wed, 17 Dec 2025 15:39:51 +0300 Subject: [PATCH] =?UTF-8?q?=D1=84=D0=B8=D0=BA=D1=81=20RXLOSS=20=D0=B4?= =?UTF-8?q?=D0=BB=D1=8F=20sbus?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- air/main.cpp | 114 +++++++++++++++++++++++++++++---------------------- 1 file changed, 65 insertions(+), 49 deletions(-) diff --git a/air/main.cpp b/air/main.cpp index bd6c48b..70b1ad4 100644 --- a/air/main.cpp +++ b/air/main.cpp @@ -23,7 +23,7 @@ inline int64_t milliseconds() { 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; class UDPServer { @@ -94,9 +94,9 @@ struct SbusData { bool lost_frame = false; bool failsafe = false; bool ch17 = false, ch18 = false; - static constexpr size_t NUM_CH = 16; - static constexpr int16_t SBUS_CH_MIN = 173; - static constexpr int16_t SBUS_CH_MAX = 1812; + static constexpr size_t NUM_CH = 18; + static constexpr int16_t SBUS_CH_MIN = 173 + 10; + static constexpr int16_t SBUS_CH_MAX = 1812 - 10; int16_t ch[NUM_CH]; /* Message len */ @@ -110,52 +110,68 @@ struct SbusData { static constexpr uint8_t CH18_MASK_ = 0x02; static constexpr uint8_t LOST_FRAME_MASK_ = 0x04; static constexpr uint8_t FAILSAFE_MASK_ = 0x08; - /* Data */ - uint8_t buf_[BUF_LEN_]; + uint8_t binaryBuffer[25]; void fillDataBuf() { - /* Assemble packet */ - buf_[0] = HEADER_; - buf_[1] = static_cast((ch[0] & 0x07FF)); - buf_[2] = static_cast((ch[0] & 0x07FF) >> 8 | - (ch[1] & 0x07FF) << 3); - buf_[3] = static_cast((ch[1] & 0x07FF) >> 5 | - (ch[2] & 0x07FF) << 6); - buf_[4] = static_cast((ch[2] & 0x07FF) >> 2); - buf_[5] = static_cast((ch[2] & 0x07FF) >> 10 | - (ch[3] & 0x07FF) << 1); - buf_[6] = static_cast((ch[3] & 0x07FF) >> 7 | - (ch[4] & 0x07FF) << 4); - buf_[7] = static_cast((ch[4] & 0x07FF) >> 4 | - (ch[5] & 0x07FF) << 7); - buf_[8] = static_cast((ch[5] & 0x07FF) >> 1); - buf_[9] = static_cast((ch[5] & 0x07FF) >> 9 | - (ch[6] & 0x07FF) << 2); - buf_[10] = static_cast((ch[6] & 0x07FF) >> 6 | - (ch[7] & 0x07FF) << 5); - buf_[11] = static_cast((ch[7] & 0x07FF) >> 3); - buf_[12] = static_cast((ch[8] & 0x07FF)); - buf_[13] = static_cast((ch[8] & 0x07FF) >> 8 | - (ch[9] & 0x07FF) << 3); - buf_[14] = static_cast((ch[9] & 0x07FF) >> 5 | - (ch[10] & 0x07FF) << 6); - buf_[15] = static_cast((ch[10] & 0x07FF) >> 2); - buf_[16] = static_cast((ch[10] & 0x07FF) >> 10 | - (ch[11] & 0x07FF) << 1); - buf_[17] = static_cast((ch[11] & 0x07FF) >> 7 | - (ch[12] & 0x07FF) << 4); - buf_[18] = static_cast((ch[12] & 0x07FF) >> 4 | - (ch[13] & 0x07FF) << 7); - buf_[19] = static_cast((ch[13] & 0x07FF) >> 1); - buf_[20] = static_cast((ch[13] & 0x07FF) >> 9 | - (ch[14] & 0x07FF) << 2); - buf_[21] = static_cast((ch[14] & 0x07FF) >> 6 | - (ch[15] & 0x07FF) << 5); - buf_[22] = static_cast((ch[15] & 0x07FF) >> 3); - buf_[23] = 0x00 | (ch17 * CH17_MASK_) | (ch18 * CH18_MASK_) | - (failsafe * FAILSAFE_MASK_) | - (lost_frame * LOST_FRAME_MASK_); - buf_[24] = FOOTER_; + typedef struct sbusChannels_s { + // 176 bits of data (11 bits per channel * 16 channels) = 22 bytes. + unsigned int chan0 : 11; + unsigned int chan1 : 11; + unsigned int chan2 : 11; + unsigned int chan3 : 11; + unsigned int chan4 : 11; + unsigned int chan5 : 11; + unsigned int chan6 : 11; + unsigned int chan7 : 11; + unsigned int chan8 : 11; + unsigned int chan9 : 11; + unsigned int chan10 : 11; + unsigned int chan11 : 11; + unsigned int chan12 : 11; + unsigned int chan13 : 11; + unsigned int chan14 : 11; + unsigned int chan15 : 11; + } __attribute__((__packed__)) sbusChannels_t; + static_assert(sizeof(sbusChannels_s) == 22); + struct sbusFrame_s { + uint8_t syncByte = HEADER_; + sbusChannels_t channels{}; + uint8_t flags{}; + /** + * 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. + * + * See https://github.com/cleanflight/cleanflight/issues/590#issuecomment-101027349 + * and + * https://github.com/cleanflight/cleanflight/issues/590#issuecomment-101706023 + */ + uint8_t endByte = FOOTER_; + } __attribute__ ((__packed__)); + static_assert(sizeof(sbusFrame_s) == sizeof(binaryBuffer)); + + auto* dest = reinterpret_cast(binaryBuffer); + dest->syncByte = HEADER_; + dest->channels.chan0 = std::min(std::max(ch[0], SBUS_CH_MIN), SBUS_CH_MAX); + dest->channels.chan1 = std::min(std::max(ch[1], SBUS_CH_MIN), SBUS_CH_MAX); + dest->channels.chan2 = std::min(std::max(ch[2], SBUS_CH_MIN), SBUS_CH_MAX); + dest->channels.chan3 = std::min(std::max(ch[3], SBUS_CH_MIN), SBUS_CH_MAX); + 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(); if (now - lastSbusWrite >= SBUS_SEND_FRAME_INTERVAL && now - lastSbusRecv <= SBUS_RXLOSS_INTERVAL) { lastSbusWrite = now; - if (!serial.write(sb.buf_)) { + if (!serial.write(sb.binaryBuffer)) { break; } }