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(data[i]); - sb.ch[i] = static_cast((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(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(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);