VladislavOstapov/sdrpi-fpv-control
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases 4 Wiki Activity

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

Browse Source
This commit is contained in:
Vladislav Ostapov
2026-01-10 16:41:59 +03:00
parent 8d17d6faab
commit 5bd75b9d5d
1 changed files with 53 additions and 41 deletions
Download Patch File Download Diff File Expand all files Collapse all files

94
air/main.cpp
View File

@@ -95,8 +95,8 @@ struct SbusData {
bool failsafe = false; bool failsafe = false;
bool ch17 = false, ch18 = false; bool ch17 = false, ch18 = false;
static constexpr size_t NUM_CH = 18; static constexpr size_t NUM_CH = 18;
static constexpr int16_t SBUS_CH_MIN = 173 + 10; static constexpr int16_t SBUS_CH_MIN = 173 + 20;
static constexpr int16_t SBUS_CH_MAX = 1812 - 10; static constexpr int16_t SBUS_CH_MAX = 1812 - 20;
int16_t ch[NUM_CH]; int16_t ch[NUM_CH];
/* Message len */ /* Message len */
@@ -112,42 +112,42 @@ struct SbusData {
static constexpr uint8_t FAILSAFE_MASK_ = 0x08; static constexpr uint8_t FAILSAFE_MASK_ = 0x08;
uint8_t binaryBuffer[25]; uint8_t binaryBuffer[25];
void fillDataBuf() { typedef struct sbusChannels_s {
typedef struct sbusChannels_s { // 176 bits of data (11 bits per channel * 16 channels) = 22 bytes.
// 176 bits of data (11 bits per channel * 16 channels) = 22 bytes. unsigned int chan0 : 11;
unsigned int chan0 : 11; unsigned int chan1 : 11;
unsigned int chan1 : 11; unsigned int chan2 : 11;
unsigned int chan2 : 11; unsigned int chan3 : 11;
unsigned int chan3 : 11; unsigned int chan4 : 11;
unsigned int chan4 : 11; unsigned int chan5 : 11;
unsigned int chan5 : 11; unsigned int chan6 : 11;
unsigned int chan6 : 11; unsigned int chan7 : 11;
unsigned int chan7 : 11; unsigned int chan8 : 11;
unsigned int chan8 : 11; unsigned int chan9 : 11;
unsigned int chan9 : 11; unsigned int chan10 : 11;
unsigned int chan10 : 11; unsigned int chan11 : 11;
unsigned int chan11 : 11; unsigned int chan12 : 11;
unsigned int chan12 : 11; unsigned int chan13 : 11;
unsigned int chan13 : 11; unsigned int chan14 : 11;
unsigned int chan14 : 11; unsigned int chan15 : 11;
unsigned int chan15 : 11; } __attribute__((__packed__)) sbusChannels_t;
} __attribute__((__packed__)) sbusChannels_t; static_assert(sizeof(sbusChannels_s) == 22);
static_assert(sizeof(sbusChannels_s) == 22); struct sbusFrame_s {
struct sbusFrame_s { uint8_t syncByte = HEADER_;
uint8_t syncByte = HEADER_; sbusChannels_t channels{};
sbusChannels_t channels{}; uint8_t flags{};
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.
* 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
* See https://github.com/cleanflight/cleanflight/issues/590#issuecomment-101027349 * and
* and * https://github.com/cleanflight/cleanflight/issues/590#issuecomment-101706023
* https://github.com/cleanflight/cleanflight/issues/590#issuecomment-101706023 */
*/ uint8_t endByte = FOOTER_;
uint8_t endByte = FOOTER_; } __attribute__ ((__packed__));
} __attribute__ ((__packed__)); static_assert(sizeof(sbusFrame_s) == sizeof(binaryBuffer));
static_assert(sizeof(sbusFrame_s) == sizeof(binaryBuffer));
void fillDataBuf() {
auto* dest = reinterpret_cast<sbusFrame_s*>(binaryBuffer); auto* dest = reinterpret_cast<sbusFrame_s*>(binaryBuffer);
dest->syncByte = HEADER_; dest->syncByte = HEADER_;
dest->channels.chan0 = std::min(std::max(ch[0], SBUS_CH_MIN), SBUS_CH_MAX); dest->channels.chan0 = std::min(std::max(ch[0], SBUS_CH_MIN), SBUS_CH_MAX);
@@ -169,7 +169,7 @@ struct SbusData {
dest->flags = 0; 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 |= 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_; dest->endByte = FOOTER_;
} }
@@ -234,6 +234,15 @@ public:
return fd; 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) { bool write(std::span<const uint8_t> data) {
if (fd < 0) return false; 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; return true;
} }
}; };
@@ -289,8 +301,8 @@ int main(int argc, char* argv[]) {
if (!data.empty()) { if (!data.empty()) {
packetCount++; packetCount++;
for (int i = 0; i < data.size() && i < SbusData::NUM_CH; ++i) { for (int i = 0; i < SbusData::NUM_CH; ++i) {
auto item = static_cast<double>(data[i]); auto item = i < data.size() ? static_cast<double>(data[i]) : 1500.0;
item -= 1000.0; item -= 1000.0;
item = std::min(item, 1000.0); item = std::min(item, 1000.0);
item = std::max(item, 0.0); item = std::max(item, 0.0);