фикс перекоса каналов на ардупилоте

air/main.cpp

@@ -95,8 +95,8 @@ struct SbusData {
     bool failsafe = false;
     bool ch17 = false, ch18 = false;
     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;
+    static constexpr int16_t SBUS_CH_MIN = 173 + 20;
+    static constexpr int16_t SBUS_CH_MAX = 1812 - 20;
     int16_t ch[NUM_CH];
 
     /* Message len */
@@ -112,7 +112,6 @@ struct SbusData {
     static constexpr uint8_t FAILSAFE_MASK_ = 0x08;
     uint8_t binaryBuffer[25];
 
-    void fillDataBuf() {
     typedef struct sbusChannels_s {
         // 176 bits of data (11 bits per channel * 16 channels) = 22 bytes.
         unsigned int chan0 : 11;
@@ -148,6 +147,7 @@ struct SbusData {
     } __attribute__ ((__packed__));
     static_assert(sizeof(sbusFrame_s) == sizeof(binaryBuffer));
 
+    void fillDataBuf() {
         auto* dest = reinterpret_cast<sbusFrame_s*>(binaryBuffer);
         dest->syncByte = HEADER_;
         dest->channels.chan0 = std::min(std::max(ch[0], SBUS_CH_MIN), SBUS_CH_MAX);
@@ -169,7 +169,7 @@ struct SbusData {
 
         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_; }
+        if (ch[17] >= (SBUS_CH_MAX + SBUS_CH_MAX) / 2) { dest->flags |= CH18_MASK_; }
 
         dest->endByte = FOOTER_;
     }
@@ -234,6 +234,15 @@ public:
         return fd;
     }
 
+    int drain() {
+        int ret;
+        do {
+            ret = ioctl(fd, TCSBRK, 1);
+        } while (ret < 0 && errno == EINTR);
+
+        return ret;
+    }
+
     // Метод для записи данных в порт
     bool write(std::span<const uint8_t> data) {
         if (fd < 0) return false;
@@ -245,7 +254,10 @@ public:
         }
 
         // Принудительная отправка данных
-        // tcdrain(fd);
+        if (drain() < 0) {
+            std::cerr << "Failed to flush data to serial port" << std::endl;
+            return false;
+        }
         return true;
     }
 };
@@ -289,8 +301,8 @@ int main(int argc, char* argv[]) {
                 if (!data.empty()) {
                     packetCount++;
 
-                    for (int i = 0; i < data.size() && i < SbusData::NUM_CH; ++i) {
-                        auto item = static_cast<double>(data[i]);
+                    for (int i = 0; i < SbusData::NUM_CH; ++i) {
+                        auto item = i < data.size() ? static_cast<double>(data[i]) : 1500.0;
                         item -= 1000.0;
                         item = std::min(item, 1000.0);
                         item = std::max(item, 0.0);