From 8aab304e5f2693ff069bc13f1a391c402aecfdc7 Mon Sep 17 00:00:00 2001
From: Vladislav Ostapov <Vlados.ost.31@yandex.ru>
Date: Mon, 24 Nov 2025 19:31:25 +0300
Subject: [PATCH] =?UTF-8?q?=D1=80=D0=B0=D0=B1=D0=BE=D1=87=D0=B8=D0=B9=20sb?=
 =?UTF-8?q?us=20=D0=B2=20=D0=B2=D0=BE=D0=B7=D0=B4=D1=83=D1=85=D0=B5?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 air/main.cpp    | 24 +++++++++++++++++-------
 ground/main.cpp |  4 ++--
 2 files changed, 19 insertions(+), 9 deletions(-)

diff --git a/air/main.cpp b/air/main.cpp
index f2aead8..a3bbcb5 100644
--- a/air/main.cpp
+++ b/air/main.cpp
@@ -85,6 +85,8 @@ struct SbusData {
     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;
     int16_t ch[NUM_CH];
 
     /* Message len */
@@ -253,16 +255,24 @@ int main(int argc, char* argv[]) {
                 packet_count++;
 
                 for (int i = 0; i < data.size() && i < SbusData::NUM_CH; ++i) {
-                    auto item = static_cast<int16_t>(data[i]);
-                    sb.ch[i] = static_cast<int16_t>((item - 1000.0) * 2);
-                    if (sb.ch[i] < 50) {
-                        sb.ch[i] = 50; // минимальное число
-                    } else if (sb.ch[i] > 1900) {
-                        sb.ch[i] = 1900; // максимальное число
+                    auto item = static_cast<double>(data[i]);
+                    item -= 1000.0;
+                    item = std::min(item, 1000.0);
+                    item = std::max(item, 0.0);
+                    item *= (SbusData::SBUS_CH_MAX - SbusData::SBUS_CH_MIN) / 1000.0;
+                    item += SbusData::SBUS_CH_MIN;
+
+                    sb.ch[i] = static_cast<int16_t>(item);
+                    if (sb.ch[i] < SbusData::SBUS_CH_MIN) {
+                        sb.ch[i] = SbusData::SBUS_CH_MIN; // минимальное число
+                    } else if (sb.ch[i] > SbusData::SBUS_CH_MAX) {
+                        sb.ch[i] = SbusData::SBUS_CH_MAX; // максимальное число
                     }
                 }
                 sb.fillDataBuf();
-                serial.write(sb.buf_);
+                if (!serial.write(sb.buf_)) {
+                    break;
+                }
 
                 // Выводим статистику каждые 100 пакетов
                 if (packet_count % 100 == 0) {
diff --git a/ground/main.cpp b/ground/main.cpp
index c20fd4c..c3d8ed1 100644
--- a/ground/main.cpp
+++ b/ground/main.cpp
@@ -27,8 +27,8 @@ int main(int argc, char* argv[]) {
 
     JoystickReader reader;
     const int64_t timeInterval = 1000 / frequency;
-    while (!reader.initialize()) {
-        std::this_thread::sleep_for(std::chrono::milliseconds(1000));
+    if (!reader.initialize()) {
+        return 1;
     }
 
     UDPSocket udp(sendAddress, 1066);