рабочий sbus в воздухе

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) {

ground/joystick-reader.cpp

@@ -64,13 +64,12 @@ bool JoystickReader::readData(std::vector<uint16_t>& data) {
     // Читаем кнопки
     int buttons = SDL_JoystickNumButtons(joystick);
     for (int i = 0; i < buttons && i < data.size() - axes; ++i) {
-        auto buttonState = SDL_JoystickGetButton(joystick, i);
-        data[axes + i] = static_cast<uint16_t>(1000.0 + (buttonState * (1000.0 / 255.0)));
+        data[axes + i] = SDL_JoystickGetButton(joystick, i) ? 2000 : 1000;
     }
 
     for (auto& i: data) {
-        if (i < 950) i = 950;
-        if (i > 2050) i = 2050;
+        if (i < 1000) i = 1000;
+        if (i > 2000) i = 2000;
     }
     return true;
 }

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);