From 5df98067a242486ce771f37ee37b1cbbf77d201a Mon Sep 17 00:00:00 2001 From: wimalopaan Date: Mon, 17 Jan 2022 12:42:20 +0100 Subject: [PATCH 01/31] first version missing translation strings for all but german --- radio/src/dataconstants.h | 1 + radio/src/ibus.cpp | 112 ++++++++++++++++++++++++++++++++ radio/src/ibus.h | 7 ++ radio/src/opentx.h | 1 + radio/src/tasks.cpp | 7 +- radio/src/translations/de.h.txt | 2 + 6 files changed, 129 insertions(+), 1 deletion(-) create mode 100644 radio/src/ibus.cpp create mode 100644 radio/src/ibus.h diff --git a/radio/src/dataconstants.h b/radio/src/dataconstants.h index 2b33855b9c9..0b18f45302a 100644 --- a/radio/src/dataconstants.h +++ b/radio/src/dataconstants.h @@ -230,6 +230,7 @@ enum UartModes { UART_MODE_TELEMETRY_MIRROR, UART_MODE_TELEMETRY, UART_MODE_SBUS_TRAINER, + UART_MODE_IBUS_TRAINER, UART_MODE_LUA, UART_MODE_CLI, UART_MODE_GPS, diff --git a/radio/src/ibus.cpp b/radio/src/ibus.cpp new file mode 100644 index 00000000000..faf5e70ef9c --- /dev/null +++ b/radio/src/ibus.cpp @@ -0,0 +1,112 @@ +#include "ibus.h" +#include "opentx.h" + +#include +#include + +#define IBUS_FRAME_GAP_DELAY 1000 // 500uS + +#define IBUS_VALUE_MIN 988 +#define IBUS_VALUE_MAX 2011 +#define IBUS_VALUE_CENTER 1500 + +//namespace { + struct CheckSum final { + inline void reset() { + mSum = std::numeric_limits::max(); + } + inline uint8_t operator+=(const uint8_t b) { + mSum -= static_cast(b); + return b; + } + inline uint8_t highByte() const { + return uint8_t(mSum >> 8); + } + inline uint8_t lowByte() const { + return uint8_t(mSum); + } + inline void highByte(const uint8_t hb) { + mH = hb; + } + inline void lowByte(const uint8_t lb){ + mL = lb; + } + inline explicit operator bool() const { + return ((mSum & 0xff) == mL) && (((mSum >> 8) & 0xff) == mH); + } + private: + uint8_t mH{}; + uint8_t mL{}; + uint16_t mSum = std::numeric_limits::max(); + }; + + uint16_t clamp(uint16_t v, uint16_t lower, uint16_t upper) { + return (v < lower) ? lower : ((v > upper) ? upper: v); + } + + int16_t convertIbusToPuls(uint16_t const ibusValue) { + const uint16_t clamped = clamp(ibusValue, IBUS_VALUE_MIN, IBUS_VALUE_MAX); + return (clamped - IBUS_VALUE_CENTER); + } + + void processIbusFrame(const uint8_t* const ibusFrame, int16_t* const pulses, uint8_t size) { + if (size != IBUS_FRAME_SIZE) return; + if (ibusFrame[0] != 0x20) return; + if (ibusFrame[1] != 0x40) return; + CheckSum cs; + cs += 0x20; + cs += 0x40; + for(size_t i = 0; i < 28; ++i) { + cs += ibusFrame[2 + i]; + } + cs.lowByte(ibusFrame[2 + 28]); + cs.highByte(ibusFrame[2 + 28 + 1]); + if (!cs) return; + + for (size_t chi{0}; chi < MAX_TRAINER_CHANNELS; chi++) { + if (chi < 14) { + const uint8_t h = ibusFrame[2 * chi + 1 + 2] & 0x0f; + const uint8_t l = ibusFrame[2 * chi + 2]; + const uint16_t v = (uint16_t(h) << 8) + uint8_t(l); + pulses[chi] = convertIbusToPuls(v); + } + else if (chi < 18) { + const uint8_t h1 = ibusFrame[6 * (chi - 14) + 1 + 2] & 0xf0; + const uint8_t h2 = ibusFrame[6 * (chi - 14) + 3 + 2] & 0xf0; + const uint8_t h3 = ibusFrame[6 * (chi - 14) + 5 + 2] & 0xf0; + const uint16_t v = (uint8_t(h1) >> 4) + uint8_t(h2) + (uint16_t(h3) << 4); + pulses[chi] = convertIbusToPuls(v); + } + } + ppmInputValidityTimer = PPM_IN_VALID_TIMEOUT; + } +//} + +void processIbusInput() { +#if !defined(SIMU) + uint8_t rxchar; + bool active = false; + static uint8_t IbusIndex = 0; + static uint16_t IbusTimer; + static uint8_t IbusFrame[IBUS_FRAME_SIZE]; + + while (sbusGetByte(&rxchar)) { + active = true; + IbusIndex = std::min(IbusIndex, (uint8_t)(IBUS_FRAME_SIZE - 1)); + IbusFrame[IbusIndex++] = rxchar; + } + if (active) { + IbusTimer = getTmr2MHz(); + return; + } + else { + if (IbusIndex) { + if ((uint16_t) (getTmr2MHz() - IbusTimer) > IBUS_FRAME_GAP_DELAY) { + processIbusFrame(IbusFrame, ppmInput, IbusIndex); + IbusIndex = 0; + } + } + } +#endif + +} diff --git a/radio/src/ibus.h b/radio/src/ibus.h new file mode 100644 index 00000000000..e2dcedd11c0 --- /dev/null +++ b/radio/src/ibus.h @@ -0,0 +1,7 @@ +#pragma once + +#define IBUS_BAUDRATE 115200 +#define IBUS_FRAME_SIZE 32 + +void processIbusInput(); + diff --git a/radio/src/opentx.h b/radio/src/opentx.h index 52d8ec9f208..741f79e8c2a 100644 --- a/radio/src/opentx.h +++ b/radio/src/opentx.h @@ -609,6 +609,7 @@ static inline void GET_ADC_IF_MIXER_NOT_RUNNING() } #include "sbus.h" +#include "ibus.h" void resetBacklightTimeout(); void checkBacklight(); diff --git a/radio/src/tasks.cpp b/radio/src/tasks.cpp index 495a1171843..3edcd1c2824 100644 --- a/radio/src/tasks.cpp +++ b/radio/src/tasks.cpp @@ -90,7 +90,12 @@ void execMixerFrequentActions() { #if defined(SBUS_TRAINER) // SBUS trainer - processSbusInput(); + if ((g_eeGeneral.auxSerialMode == UART_MODE_SBUS_TRAINER) || (g_eeGeneral.auxSerialMode == UART_MODE_SBUS_TRAINER)) { + processSbusInput(); + } + if ((g_eeGeneral.auxSerialMode == UART_MODE_IBUS_TRAINER) || (g_eeGeneral.auxSerialMode == UART_MODE_IBUS_TRAINER)) { + processIbusInput(); + } #endif #if defined(GYRO) diff --git a/radio/src/translations/de.h.txt b/radio/src/translations/de.h.txt index 0d80f27a183..57c7d863885 100644 --- a/radio/src/translations/de.h.txt +++ b/radio/src/translations/de.h.txt @@ -63,6 +63,7 @@ #define LEN_AUX_SERIAL_MODES "\015" #define TR_AUX_SERIAL_MODES "AUS\0 ""Telem Mirror\0""Telemetry In\0""SBUS Eingang\0""LUA\0 ""CLI\0 ""GPS\0 ""Debug\0 " +//#define TR_AUX_SERIAL_MODES "AUS\0 ""Telem Mirror\0""Telemetry In\0""SBUS Eingang\0""IBUS Eingang\0""LUA\0 " #define LEN_SWTYPES "\006" #define TR_SWTYPES "Kein\0 ""Taster""2POS\0 ""3POS\0" @@ -329,6 +330,7 @@ #define TR_VTRAINER_MASTER_JACK "Lehrer/Buchse\0 " #define TR_VTRAINER_SLAVE_JACK "Schüler/Buchse\0 " #define TR_VTRAINER_MASTER_SBUS_MODULE "Lehrer/SBUS Modul\0" +#define TR_VTRAINER_MASTER_IBUS_MODULE "Lehrer/IBUS Modul\0" #define TR_VTRAINER_MASTER_CPPM_MODULE "Lehrer/CPPM Modul\0" #define TR_VTRAINER_MASTER_BATTERY "Lehrer/Serial\0 " #define TR_VTRAINER_BLUETOOTH TR("Lehrer/BT\0 ""Schüler/BT\0 ", "Lehrer/Bluetooth\0 ""Schüler/Bluetooth\0") From 67d24dbfb8ebf27cfcea2b719aaa6c1216f4097d Mon Sep 17 00:00:00 2001 From: wimalopaan Date: Tue, 18 Jan 2022 07:30:56 +0100 Subject: [PATCH 02/31] WIP: intermediate commit (still missing other translations: strings/enum correspondance) --- radio/src/ibus.cpp | 135 ++++++---- radio/src/sbus.cpp | 235 ++++++++++++++---- radio/src/sbus.h | 1 + .../common/arm/stm32/timers_driver.cpp | 3 + radio/src/tasks.cpp | 4 +- 5 files changed, 281 insertions(+), 97 deletions(-) diff --git a/radio/src/ibus.cpp b/radio/src/ibus.cpp index faf5e70ef9c..5d97d970115 100644 --- a/radio/src/ibus.cpp +++ b/radio/src/ibus.cpp @@ -4,13 +4,13 @@ #include #include -#define IBUS_FRAME_GAP_DELAY 1000 // 500uS +//#define IBUS_FRAME_GAP_DELAY 2000 // 1ms #define IBUS_VALUE_MIN 988 #define IBUS_VALUE_MAX 2011 #define IBUS_VALUE_CENTER 1500 -//namespace { +namespace IBus { struct CheckSum final { inline void reset() { mSum = std::numeric_limits::max(); @@ -48,65 +48,94 @@ const uint16_t clamped = clamp(ibusValue, IBUS_VALUE_MIN, IBUS_VALUE_MAX); return (clamped - IBUS_VALUE_CENTER); } - - void processIbusFrame(const uint8_t* const ibusFrame, int16_t* const pulses, uint8_t size) { - if (size != IBUS_FRAME_SIZE) return; - if (ibusFrame[0] != 0x20) return; - if (ibusFrame[1] != 0x40) return; - CheckSum cs; - cs += 0x20; - cs += 0x40; - for(size_t i = 0; i < 28; ++i) { - cs += ibusFrame[2 + i]; - } - cs.lowByte(ibusFrame[2 + 28]); - cs.highByte(ibusFrame[2 + 28 + 1]); - if (!cs) return; - - for (size_t chi{0}; chi < MAX_TRAINER_CHANNELS; chi++) { - if (chi < 14) { - const uint8_t h = ibusFrame[2 * chi + 1 + 2] & 0x0f; - const uint8_t l = ibusFrame[2 * chi + 2]; - const uint16_t v = (uint16_t(h) << 8) + uint8_t(l); - pulses[chi] = convertIbusToPuls(v); - } - else if (chi < 18) { - const uint8_t h1 = ibusFrame[6 * (chi - 14) + 1 + 2] & 0xf0; - const uint8_t h2 = ibusFrame[6 * (chi - 14) + 3 + 2] & 0xf0; - const uint8_t h3 = ibusFrame[6 * (chi - 14) + 5 + 2] & 0xf0; - const uint16_t v = (uint8_t(h1) >> 4) + uint8_t(h2) + (uint16_t(h3) << 4); - pulses[chi] = convertIbusToPuls(v); + struct Servo { + enum class State : uint8_t {Undefined, GotStart20, Data, CheckL, CheckH}; + static inline void process(const uint8_t b, std::function f) { + switch(mState) { + case State::Undefined: + csum.reset(); + if (b == 0x20) { + csum += b; + mState = State::GotStart20; + } + break; + case State::GotStart20: + if (b == 0x40) { + csum += b; + mState = State::Data; + mIndex = 0; + } + else { + mState = State::Undefined; + } + break; + case State::Data: + ibusFrame[mIndex] = b; + csum += b; + if (mIndex >= (ibusFrame.size() - 1)) { + mState = State::CheckL; + } + else { + ++mIndex; + } + break; + case State::CheckL: + csum.lowByte(b); + mState = State::CheckH; + break; + case State::CheckH: + csum.highByte(b); + mState = State::Undefined; + if (csum) { + ++mPackagesCounter; + f(); + } + break; + } + } + static inline void convert(int16_t* pulses) { + for (size_t chi{0}; chi < MAX_TRAINER_CHANNELS; chi++) { + if (chi < 14) { + const uint8_t h = ibusFrame[2 * chi + 1] & 0x0f; + const uint8_t l = ibusFrame[2 * chi]; + const uint16_t v = (uint16_t(h) << 8) + uint8_t(l); + pulses[chi] = convertIbusToPuls(v); + } + else if (chi < 18) { + const uint8_t h1 = ibusFrame[6 * (chi - 14) + 1] & 0xf0; + const uint8_t h2 = ibusFrame[6 * (chi - 14) + 3] & 0xf0; + const uint8_t h3 = ibusFrame[6 * (chi - 14) + 5] & 0xf0; + const uint16_t v = (uint8_t(h1) >> 4) + uint8_t(h2) + (uint16_t(h3) << 4); + pulses[chi] = convertIbusToPuls(v); + } } } - ppmInputValidityTimer = PPM_IN_VALID_TIMEOUT; - } -//} + private: + using MesgType = std::array; // 0x20, 0x40 , 28 Bytes, checkH, checkL + static CheckSum csum; + static State mState; + static MesgType ibusFrame; + static uint8_t mIndex; + static uint16_t mPackagesCounter; + }; + + CheckSum Servo::csum; + Servo::State Servo::mState{Servo::State::Undefined}; + Servo::MesgType Servo::ibusFrame; + uint8_t Servo::mIndex{}; + uint16_t Servo::mPackagesCounter{}; +} void processIbusInput() { #if !defined(SIMU) uint8_t rxchar; - bool active = false; - static uint8_t IbusIndex = 0; - static uint16_t IbusTimer; - static uint8_t IbusFrame[IBUS_FRAME_SIZE]; while (sbusGetByte(&rxchar)) { - active = true; - IbusIndex = std::min(IbusIndex, (uint8_t)(IBUS_FRAME_SIZE - 1)); - IbusFrame[IbusIndex++] = rxchar; - } - if (active) { - IbusTimer = getTmr2MHz(); - return; - } - else { - if (IbusIndex) { - if ((uint16_t) (getTmr2MHz() - IbusTimer) > IBUS_FRAME_GAP_DELAY) { - processIbusFrame(IbusFrame, ppmInput, IbusIndex); - IbusIndex = 0; - } - } + IBus::Servo::process(rxchar, [&](){ + IBus::Servo::convert(ppmInput); + ppmInputValidityTimer = PPM_IN_VALID_TIMEOUT; + }); } #endif - } + diff --git a/radio/src/sbus.cpp b/radio/src/sbus.cpp index 096bca02d4a..cefb66c1b84 100644 --- a/radio/src/sbus.cpp +++ b/radio/src/sbus.cpp @@ -77,55 +77,206 @@ void processSbusFrame(uint8_t * sbus, int16_t * pulses, uint32_t size) return; // SBUS invalid frame or failsafe mode } - sbus++; // skip start byte - - uint32_t inputbitsavailable = 0; - uint32_t inputbits = 0; - for (uint32_t i=0; i>= SBUS_CH_BITS; - } +// sbus++; // skip start byte + +// uint32_t inputbitsavailable = 0; +// uint32_t inputbits = 0; +// for (uint32_t i=0; i>= SBUS_CH_BITS; +// } + +// ppmInputValidityTimer = PPM_IN_VALID_TIMEOUT; +//} + +//void processSbusInput() +//{ +//#if !defined(SIMU) +// uint8_t rxchar; +// uint32_t active = 0; +// static uint8_t SbusIndex = 0; +// static uint16_t SbusTimer; +// static uint8_t SbusFrame[SBUS_FRAME_SIZE]; - ppmInputValidityTimer = PPM_IN_VALID_TIMEOUT; +// while (sbusGetByte(&rxchar)) { +// active = 1; +// if (SbusIndex > SBUS_FRAME_SIZE-1) { +// SbusIndex = SBUS_FRAME_SIZE-1; +// } +// SbusFrame[SbusIndex++] = rxchar; +// } +// if (active) { +// SbusTimer = getTmr2MHz(); +// return; +// } +// else { +// if (SbusIndex) { +// if ((uint16_t) (getTmr2MHz() - SbusTimer) > SBUS_FRAME_GAP_DELAY) { +// processSbusFrame(SbusFrame, ppmInput, SbusIndex); +// SbusIndex = 0; +// } +// } +// } +//#endif +//} + +namespace SBus { + struct Servo { + enum class State : uint8_t {Undefined, Data, GotEnd, WaitEnd}; + + static constexpr uint8_t mPauseCount{2}; // 2ms + + static constexpr uint8_t mFrameLostMask{1 << SBUS_FRAMELOST_BIT}; + static constexpr uint8_t mFailSafeMask{1 << SBUS_FAILSAFE_BIT}; + + static inline void tick1ms() { + if (mPauseCounter > 0) { + --mPauseCounter; + } + else { + mState = State::Undefined; + } + } + + static inline void process(const uint8_t b, std::function f) { + mPauseCounter = mPauseCount; + switch(mState) { + case State::Undefined: + if (b == 0x00) { + mState = State::GotEnd; + } + else if (b == 0x0f) { + mState = State::Data; + mIndex = 0; + } + break; + case State::GotEnd: + if (b == 0x0f) { + mState = State::Data; + mIndex = 0; + } + else if (b == 0x00) { + mState = State::GotEnd; + } + else { + mState = State::Undefined; + } + break; + case State::Data: + mData[mIndex] = b; + if (mIndex >= (mData.size() - 1)) { + mState = State::WaitEnd; + } + else { + ++mIndex; + } + break; + case State::WaitEnd: + if (b == 0x00) { + mState = State::GotEnd; + if (!((mData[mData.size() - 1] & mFrameLostMask) || (mData[mData.size() - 1] & mFailSafeMask))) { + f(); + ++mPackages; + } + } + else { + mState = State::Undefined; + } + break; + } + } + static inline void convert(int16_t* pulses) { + pulses[0] = (uint16_t) (((mData[0] | mData[1] << 8)) & 0x07FF); + pulses[1] = (uint16_t) ((mData[1]>>3 | mData[2] <<5) & 0x07FF); + pulses[2] = (uint16_t) ((mData[2]>>6 | mData[3] <<2 |mData[4]<<10) & 0x07FF); + pulses[3] = (uint16_t) ((mData[4]>>1 | mData[5] <<7) & 0x07FF); + pulses[4] = (uint16_t) ((mData[5]>>4 | mData[6] <<4) & 0x07FF); + pulses[5] = (uint16_t) ((mData[6]>>7 | mData[7] <<1 |mData[8]<<9) & 0x07FF); + pulses[6] = (uint16_t) ((mData[8]>>2 | mData[9] <<6) & 0x07FF); + pulses[7] = (uint16_t) ((mData[9]>>5 | mData[10]<<3) & 0x07FF); + pulses[8] = (uint16_t) ((mData[11] | mData[12]<<8) & 0x07FF); + pulses[9] = (uint16_t) ((mData[12]>>3 | mData[13]<<5) & 0x07FF); + pulses[10] = (uint16_t) ((mData[13]>>6 | mData[14]<<2 |mData[15]<<10) & 0x07FF); + pulses[11] = (uint16_t) ((mData[15]>>1 | mData[16]<<7) & 0x07FF); + pulses[12] = (uint16_t) ((mData[16]>>4 | mData[17]<<4) & 0x07FF); + pulses[13] = (uint16_t) ((mData[17]>>7 | mData[18]<<1 |mData[19]<<9) & 0x07FF); + pulses[14] = (uint16_t) ((mData[19]>>2 | mData[20]<<6) & 0x07FF); + pulses[15] = (uint16_t) ((mData[20]>>5 | mData[21]<<3) & 0x07FF); + + for(size_t i = 0; i < 16; ++i) { + pulses[i] -= SBUS_CH_CENTER; + pulses[i] *= 5; + pulses[i] /= 8; + } + } + using MesgType = std::array; + static State mState; + static MesgType mData; + static uint8_t mIndex; + static uint16_t mPackages; + static uint8_t mPauseCounter; + }; + Servo::State Servo::mState{State::Undefined}; + Servo::MesgType Servo::mData; + uint8_t Servo::mIndex{}; + uint16_t Servo::mPackages{}; + uint8_t Servo::mPauseCounter{Servo::mPauseCount}; // 2 ms +} +void sbusTrainerPauseCheck() { +#if !defined(SIMU) + SBus::Servo::tick1ms(); +#endif } -void processSbusInput() -{ +//void processSbusInput() +//{ - // TODO: place this outside of the function - static uint8_t SbusIndex = 0; - static uint16_t SbusTimer; - static uint8_t SbusFrame[SBUS_FRAME_SIZE]; +// // TODO: place this outside of the function +// static uint8_t SbusIndex = 0; +// static uint16_t SbusTimer; +// static uint8_t SbusFrame[SBUS_FRAME_SIZE]; - uint32_t active = 0; +// uint32_t active = 0; - // Drain input first (if existing) - uint8_t rxchar; - auto _getByte = sbusGetByte; - while (_getByte && (_getByte(&rxchar) > 0)) { - active = 1; - if (SbusIndex > SBUS_FRAME_SIZE - 1) { - SbusIndex = SBUS_FRAME_SIZE - 1; - } - SbusFrame[SbusIndex++] = rxchar; - } +// // Drain input first (if existing) +// uint8_t rxchar; +// auto _getByte = sbusGetByte; +// while (_getByte && (_getByte(&rxchar) > 0)) { +// active = 1; +// if (SbusIndex > SBUS_FRAME_SIZE - 1) { +// SbusIndex = SBUS_FRAME_SIZE - 1; +// } +// SbusFrame[SbusIndex++] = rxchar; +// } - // Data has been received - if (active) { - SbusTimer = getTmr2MHz(); - return; - } +// // Data has been received +// if (active) { +// SbusTimer = getTmr2MHz(); +// return; +// } + +// // Check if end-of-frame is detected +// if (SbusIndex) { +// if ((uint16_t)(getTmr2MHz() - SbusTimer) > SBUS_FRAME_GAP_DELAY) { +// processSbusFrame(SbusFrame, ppmInput, SbusIndex); +// SbusIndex = 0; +// } +// } + +void processSbusInput() { +#if !defined(SIMU) + uint8_t rxchar; - // Check if end-of-frame is detected - if (SbusIndex) { - if ((uint16_t)(getTmr2MHz() - SbusTimer) > SBUS_FRAME_GAP_DELAY) { - processSbusFrame(SbusFrame, ppmInput, SbusIndex); - SbusIndex = 0; - } + while (sbusGetByte(&rxchar)) { + SBus::Servo::process(rxchar, [&](){ + SBus::Servo::convert(ppmInput); + ppmInputValidityTimer = PPM_IN_VALID_TIMEOUT; + }); } +#endif } diff --git a/radio/src/sbus.h b/radio/src/sbus.h index 981a7c7ed06..c26cd02be1b 100644 --- a/radio/src/sbus.h +++ b/radio/src/sbus.h @@ -37,5 +37,6 @@ int sbusAuxGetByte(uint8_t* byte); void sbusSetGetByte(int (*fct)(uint8_t*)); void processSbusInput(); +void sbusTrainerPauseCheck(); #endif // _SBUS_H_ diff --git a/radio/src/targets/common/arm/stm32/timers_driver.cpp b/radio/src/targets/common/arm/stm32/timers_driver.cpp index 1fe4644c31c..02faffd9486 100644 --- a/radio/src/targets/common/arm/stm32/timers_driver.cpp +++ b/radio/src/targets/common/arm/stm32/timers_driver.cpp @@ -65,6 +65,9 @@ static void interrupt1ms() flysky_hall_stick_loop(); } #endif +#if defined(SBUS_TRAINER) + sbusTrainerPauseCheck(); +#endif // 5ms loop if (pre_scale == 5 || pre_scale == 10) { diff --git a/radio/src/tasks.cpp b/radio/src/tasks.cpp index 3edcd1c2824..d6cc0569b27 100644 --- a/radio/src/tasks.cpp +++ b/radio/src/tasks.cpp @@ -90,10 +90,10 @@ void execMixerFrequentActions() { #if defined(SBUS_TRAINER) // SBUS trainer - if ((g_eeGeneral.auxSerialMode == UART_MODE_SBUS_TRAINER) || (g_eeGeneral.auxSerialMode == UART_MODE_SBUS_TRAINER)) { + if ((g_eeGeneral.auxSerialMode == UART_MODE_SBUS_TRAINER) || (g_eeGeneral.aux2SerialMode == UART_MODE_SBUS_TRAINER)) { processSbusInput(); } - if ((g_eeGeneral.auxSerialMode == UART_MODE_IBUS_TRAINER) || (g_eeGeneral.auxSerialMode == UART_MODE_IBUS_TRAINER)) { + else if ((g_eeGeneral.auxSerialMode == UART_MODE_IBUS_TRAINER) || (g_eeGeneral.aux2SerialMode == UART_MODE_IBUS_TRAINER)) { processIbusInput(); } #endif From 886a06ab9e480d3a9107997191fe57879366de22 Mon Sep 17 00:00:00 2001 From: wimalopaan Date: Tue, 18 Jan 2022 08:38:55 +0100 Subject: [PATCH 03/31] fix taranis --- radio/src/tasks.cpp | 12 ++++++++++-- 1 file changed, 10 insertions(+), 2 deletions(-) diff --git a/radio/src/tasks.cpp b/radio/src/tasks.cpp index d6cc0569b27..ba55b8f681d 100644 --- a/radio/src/tasks.cpp +++ b/radio/src/tasks.cpp @@ -90,10 +90,18 @@ void execMixerFrequentActions() { #if defined(SBUS_TRAINER) // SBUS trainer - if ((g_eeGeneral.auxSerialMode == UART_MODE_SBUS_TRAINER) || (g_eeGeneral.aux2SerialMode == UART_MODE_SBUS_TRAINER)) { + if ((g_eeGeneral.auxSerialMode == UART_MODE_SBUS_TRAINER) + #ifdef AUX2_SERIAL + || (g_eeGeneral.aux2SerialMode == UART_MODE_SBUS_TRAINER) + #endif + ) { processSbusInput(); } - else if ((g_eeGeneral.auxSerialMode == UART_MODE_IBUS_TRAINER) || (g_eeGeneral.aux2SerialMode == UART_MODE_IBUS_TRAINER)) { + else if ((g_eeGeneral.auxSerialMode == UART_MODE_IBUS_TRAINER) + #ifdef AUX2_SERIAL + || (g_eeGeneral.aux2SerialMode == UART_MODE_IBUS_TRAINER) + #endif + ) { processIbusInput(); } #endif From 51dbaaec2f9d1026ef07e97f412b22aad275e12a Mon Sep 17 00:00:00 2001 From: wimalopaan Date: Tue, 18 Jan 2022 08:39:11 +0100 Subject: [PATCH 04/31] fix translations --- radio/src/translations/cn.h.txt | 2 ++ radio/src/translations/cz.h.txt | 2 ++ radio/src/translations/en.h.txt | 2 ++ radio/src/translations/es.h.txt | 2 ++ radio/src/translations/fi.h.txt | 2 ++ radio/src/translations/fr.h.txt | 2 ++ radio/src/translations/it.h.txt | 2 ++ radio/src/translations/nl.h.txt | 2 ++ radio/src/translations/pl.h.txt | 2 ++ radio/src/translations/pt.h.txt | 2 ++ radio/src/translations/se.h.txt | 2 ++ 11 files changed, 22 insertions(+) diff --git a/radio/src/translations/cn.h.txt b/radio/src/translations/cn.h.txt index c199fbcc4de..d4bcc68c2b2 100644 --- a/radio/src/translations/cn.h.txt +++ b/radio/src/translations/cn.h.txt @@ -64,6 +64,7 @@ #define LEN_AUX_SERIAL_MODES "\010" #define TR_AUX_SERIAL_MODES "调试\0 " "回传镜像" "回传输入" "SBUS教练" "LUA脚本\0""CLI\0 ""GPS\0 ""Debug\0 " +//#define TR_AUX_SERIAL_MODES "调试\0 " "回传镜像" "回传输入" "SBUS教练" "IBUS教练" "LUA脚本\0" #define LEN_SWTYPES "\004" #define TR_SWTYPES "无\0 " "回弹" "2段\0""3段\0" @@ -311,6 +312,7 @@ #define TR_VTRAINER_MASTER_JACK "教练主机/教练口\0 " #define TR_VTRAINER_SLAVE_JACK "学生从机/教练口\0 " #define TR_VTRAINER_MASTER_SBUS_MODULE "教练主机/SBUS模块\0" +#define TR_VTRAINER_MASTER_IBUS_MODULE "教练主机/IBUS模块\0" #define TR_VTRAINER_MASTER_CPPM_MODULE "教练从机/CPPM模块\0" #define TR_VTRAINER_MASTER_BATTERY "教练主机/串口\0 " #define TR_VTRAINER_BLUETOOTH TR("Master/BT\0 ""Slave/BT\0 ", "教练主机/蓝牙\0 " "教练从机/蓝牙\0 ") diff --git a/radio/src/translations/cz.h.txt b/radio/src/translations/cz.h.txt index 440ff9fea69..d8befdc7b16 100644 --- a/radio/src/translations/cz.h.txt +++ b/radio/src/translations/cz.h.txt @@ -62,6 +62,7 @@ #define LEN_AUX_SERIAL_MODES "\015" #define TR_AUX_SERIAL_MODES "VYP\0 ""Telem Mirror\0""Telemetry In\0""SBUS Trenér\0 ""LUA\0 ""CLI\0 ""GPS\0 ""Debug\0 " +//#define TR_AUX_SERIAL_MODES "Debug\0 ""Telem Mirror\0""Telemetry In\0""SBUS Trenér\0 ""IBUS Trenér\0 ""LUA\0 " #define LEN_SWTYPES "\013" #define TR_SWTYPES "Žádný\0 ""Bez aretace""2-polohový\0""3-polohový\0" @@ -326,6 +327,7 @@ #define TR_VTRAINER_MASTER_JACK "Učitel/Jack\0 " #define TR_VTRAINER_SLAVE_JACK "Žák/Jack\0 " #define TR_VTRAINER_MASTER_SBUS_MODULE "Učitel/SBUS Modul\0" +#define TR_VTRAINER_MASTER_IBUS_MODULE "Učitel/IBUS Modul\0" #define TR_VTRAINER_MASTER_CPPM_MODULE "Učitel/CPPM Modul\0" #define TR_VTRAINER_MASTER_BATTERY "Učitel/Serial\0 " #define TR_VTRAINER_BLUETOOTH TR("Master/BT\0 ""Slave/BT\0 ", "Master/Bluetooth\0 ""Slave/Bluetooth\0 ") diff --git a/radio/src/translations/en.h.txt b/radio/src/translations/en.h.txt index db7355305d3..f8972cfdd58 100644 --- a/radio/src/translations/en.h.txt +++ b/radio/src/translations/en.h.txt @@ -63,6 +63,7 @@ #define LEN_AUX_SERIAL_MODES "\015" #define TR_AUX_SERIAL_MODES "OFF\0 ""Telem Mirror\0""Telemetry In\0""SBUS Trainer\0""LUA\0 ""CLI\0 ""GPS\0 ""Debug\0 " +//#define TR_AUX_SERIAL_MODES "Debug\0 ""Telem Mirror\0""Telemetry In\0""SBUS Trainer\0""IBUS Trainer\0""LUA\0 " #define LEN_SWTYPES "\006" #define TR_SWTYPES "None\0 ""Toggle""2POS\0 ""3POS\0" @@ -329,6 +330,7 @@ #define TR_VTRAINER_MASTER_JACK "Master/Jack\0 " #define TR_VTRAINER_SLAVE_JACK "Slave/Jack\0 " #define TR_VTRAINER_MASTER_SBUS_MODULE "Master/SBUS Module" +#define TR_VTRAINER_MASTER_IBUS_MODULE "Master/IBUS Module" #define TR_VTRAINER_MASTER_CPPM_MODULE "Master/CPPM Module" #define TR_VTRAINER_MASTER_BATTERY "Master/Serial\0 " #define TR_VTRAINER_BLUETOOTH TR("Master/BT\0 ""Slave/BT\0 ", "Master/Bluetooth\0 ""Slave/Bluetooth\0 ") diff --git a/radio/src/translations/es.h.txt b/radio/src/translations/es.h.txt index c582b2e5ed5..566e1c24e1c 100644 --- a/radio/src/translations/es.h.txt +++ b/radio/src/translations/es.h.txt @@ -63,6 +63,7 @@ #define LEN_AUX_SERIAL_MODES "\015" #define TR_AUX_SERIAL_MODES "OFF\0 ""Telem Mirror\0""Telemetría\0 ""Entrenador SBUS""LUA\0 ""CLI\0 ""GPS\0 ""Debug\0 " +//#define TR_AUX_SERIAL_MODES "Debug\0 ""Telem Mirror\0""Telemetría\0 ""Entrenador SBUS""LUA\0 ""Entrenador IBUS""LUA\0 " #define LEN_SWTYPES "\007" #define TR_SWTYPES "Nada\0 ""Palanca""2POS\0 ""3POS\0 " @@ -326,6 +327,7 @@ #define TR_VTRAINER_MASTER_JACK "Master/Jack\0 " #define TR_VTRAINER_SLAVE_JACK "Esclav/Jack\0 " #define TR_VTRAINER_MASTER_SBUS_MODULE "Master/Módulo SBUS" +#define TR_VTRAINER_MASTER_IBUS_MODULE "Master/Módulo IBUS" #define TR_VTRAINER_MASTER_CPPM_MODULE "Master/Módulo CPPM" #define TR_VTRAINER_MASTER_BATTERY "Master/Serie\0 " #define TR_VTRAINER_BLUETOOTH TR("Master/BT\0 ""Esclavo/BT\0 ", "Master/Bluetooth\0 ""Esclavo/Bluetooth\0") diff --git a/radio/src/translations/fi.h.txt b/radio/src/translations/fi.h.txt index d7ca2e3fa47..34ee91d4166 100644 --- a/radio/src/translations/fi.h.txt +++ b/radio/src/translations/fi.h.txt @@ -63,6 +63,7 @@ #define LEN_AUX_SERIAL_MODES "\015" #define TR_AUX_SERIAL_MODES "POIS\0 ""S-Port Pelik\0""Telemetry In\0""SBUS Trainer\0""LUA\0 ""CLI\0 ""GPS\0 ""Debug\0 " +//#define TR_AUX_SERIAL_MODES "Debug\0 ""S-Port Pelik\0""Telemetry In\0""SBUS Trainer\0""IBUS Trainer\0""LUA\0 " #define LEN_SWTYPES "\006" #define TR_SWTYPES "None\0 ""Toggle""2POS\0 ""3POS\0" @@ -347,6 +348,7 @@ #define TR_VTRAINER_MASTER_JACK "Master/Jack\0 " #define TR_VTRAINER_SLAVE_JACK "Slave/Jack\0 " #define TR_VTRAINER_MASTER_SBUS_MODULE "Master/SBUS Module" +#define TR_VTRAINER_MASTER_IBUS_MODULE "Master/IBUS Module" #define TR_VTRAINER_MASTER_CPPM_MODULE "Master/CPPM Module" #define TR_VTRAINER_MASTER_BATTERY "Master/Serial\0 " #define TR_VTRAINER_BLUETOOTH TR("Master/BT\0 ""Slave/BT\0 ", "Master/Bluetooth\0 ""Slave/Bluetooth\0 ") diff --git a/radio/src/translations/fr.h.txt b/radio/src/translations/fr.h.txt index 5278a52ddf8..6ffae9b42ec 100644 --- a/radio/src/translations/fr.h.txt +++ b/radio/src/translations/fr.h.txt @@ -66,6 +66,7 @@ #define LEN_AUX_SERIAL_MODES "\016" #define TR_AUX_SERIAL_MODES "OFF\0 ""Recopie Telem\0""Télémétrie In\0""Ecolage SBUS\0 ""LUA\0 ""CLI\0 ""GPS\0 ""Debug\0 " +//#define TR_AUX_SERIAL_MODES "Debug\0 ""Recopie Telem\0""Télémétrie In\0""Ecolage SBUS\0 ""Ecolage IBUS\0 ""LUA\0 " #define LEN_SWTYPES "\006" #define TR_SWTYPES "Rien\0 ""Levier""2-POS\0""3-POS\0" @@ -349,6 +350,7 @@ #define TR_VTRAINER_MASTER_JACK "Maître/Jack\0 " #define TR_VTRAINER_SLAVE_JACK "Elève/Jack\0 " #define TR_VTRAINER_MASTER_SBUS_MODULE "Maître/SBUS Module" +#define TR_VTRAINER_MASTER_IBUS_MODULE "Maître/IBUS Module" #define TR_VTRAINER_MASTER_CPPM_MODULE "Maître/CPPM Module" #define TR_VTRAINER_MASTER_BATTERY "Maître/Série\0 " #define TR_VTRAINER_BLUETOOTH TR("Maître/BT\0 ""Elève/BT\0 ", "Maître/Bluetooth\0 ""Elève/Bluetooth\0 ") diff --git a/radio/src/translations/it.h.txt b/radio/src/translations/it.h.txt index eddb06528d9..e10d7ac47fb 100644 --- a/radio/src/translations/it.h.txt +++ b/radio/src/translations/it.h.txt @@ -64,6 +64,7 @@ #define LEN_AUX_SERIAL_MODES "\016" #define TR_AUX_SERIAL_MODES "OFF\0 ""Replica S-Port""Telemetria\0 ""SBUS Trainer\0 ""LUA\0 ""CLI\0 ""GPS\0 ""Debug\0 " +//#define TR_AUX_SERIAL_MODES "Debug\0 ""Replica S-Port""Telemetria\0 ""SBUS Trainer\0 ""IBUS Trainer\0 ""LUA\0 " #define LEN_SWTYPES "\006" #define TR_SWTYPES "Dis.\0 ""Toggle""2POS\0 ""3POS\0" @@ -354,6 +355,7 @@ #define TR_VTRAINER_MASTER_JACK "Maestro/Jack\0 " #define TR_VTRAINER_SLAVE_JACK "Allievo/Jack\0 " #define TR_VTRAINER_MASTER_SBUS_MODULE "Master/Modulo SBUS" +#define TR_VTRAINER_MASTER_IBUS_MODULE "Master/Modulo IBUS" #define TR_VTRAINER_MASTER_CPPM_MODULE "Master/Modulo CPPM" #define TR_VTRAINER_MASTER_BATTERY "Master/Seriale\0 " #define TR_VTRAINER_BLUETOOTH TR("Master/BT\0 ""Slave/BT\0 ", "Master/Bluetooth\0 ""Slave/Bluetooth\0 ") diff --git a/radio/src/translations/nl.h.txt b/radio/src/translations/nl.h.txt index 7dc5949eb2d..475c26d6cad 100644 --- a/radio/src/translations/nl.h.txt +++ b/radio/src/translations/nl.h.txt @@ -63,6 +63,7 @@ #define LEN_AUX_SERIAL_MODES "\015" #define TR_AUX_SERIAL_MODES "UIT\0 ""Telem Mirror\0""Telemetry In\0""SBUS Leerling""LUA\0 ""CLI\0 ""GPS\0 ""Debug\0 " +//#define TR_AUX_SERIAL_MODES "Debug\0 ""Telem Mirror\0""Telemetry In\0""SBUS Leerling""IBUS Leerling""LUA\0 " #define LEN_SWTYPES "\006" #define TR_SWTYPES "Geen\0 ""Wissel""2POS\0 ""3POS\0" @@ -328,6 +329,7 @@ #define TR_VTRAINER_MASTER_JACK "Master/Jack\0 " #define TR_VTRAINER_SLAVE_JACK "Slave/Jack\0 " #define TR_VTRAINER_MASTER_SBUS_MODULE "Master/SBUS Module" +#define TR_VTRAINER_MASTER_IBUS_MODULE "Master/IBUS Module" #define TR_VTRAINER_MASTER_CPPM_MODULE "Master/CPPM Module" #define TR_VTRAINER_MASTER_BATTERY "Master/Serial\0 " #define TR_VTRAINER_BLUETOOTH TR("Master/BT\0 ""Slave/BT\0 ", "Master/Bluetooth\0 ""Slave/Bluetooth\0 ") diff --git a/radio/src/translations/pl.h.txt b/radio/src/translations/pl.h.txt index ccdc2f6d459..aa883fa6969 100644 --- a/radio/src/translations/pl.h.txt +++ b/radio/src/translations/pl.h.txt @@ -63,6 +63,7 @@ #define LEN_AUX_SERIAL_MODES "\015" /*13 decimal*/ #define TR_AUX_SERIAL_MODES "Wyłącz\0 ""S-Port Kopia ""Telemetria\0 ""Trener SBUS\0 ""LUA\0 ""CLI\0 ""GPS\0 ""Debug\0 " +//#define TR_AUX_SERIAL_MODES "Debug\0 ""S-Port Kopia ""Telemetria\0 ""Trener SBUS\0 ""Trener IBUS\0 ""LUA\0 " #define LEN_SWTYPES "\006" #define TR_SWTYPES "Brak\0 ""Chwil.""2POZ\0 ""3POZ\0" @@ -347,6 +348,7 @@ #define TR_VTRAINER_MASTER_JACK "Trener/Jack\0 " #define TR_VTRAINER_SLAVE_JACK "Uczeń/Jack\0 " #define TR_VTRAINER_MASTER_SBUS_MODULE "Trener/SBUS Moduł " +#define TR_VTRAINER_MASTER_IBUS_MODULE "Trener/IBUS Moduł " #define TR_VTRAINER_MASTER_CPPM_MODULE "Trener/CPPM Moduł " #define TR_VTRAINER_MASTER_BATTERY "Trener/Serial\0 " #define TR_VTRAINER_BLUETOOTH TR("Master/BT\0 ""Slave/BT\0", "Master/Bluetooth\0 ""Slave/Bluetooth\0 ") diff --git a/radio/src/translations/pt.h.txt b/radio/src/translations/pt.h.txt index f46c80427ef..07cf176f8eb 100644 --- a/radio/src/translations/pt.h.txt +++ b/radio/src/translations/pt.h.txt @@ -63,6 +63,7 @@ #define LEN_AUX_SERIAL_MODES "\017" #define TR_AUX_SERIAL_MODES "OFF\0 ""S-Port Mirror\0 ""Telemetry\0 ""SBUS Trainer\0 ""LUA\0 ""CLI\0 ""GPS\0 ""Debug\0 " +//#define TR_AUX_SERIAL_MODES "Debug\0 ""S-Port Mirror\0 ""Telemetry\0 ""SBUS Trainer\0 ""IBUS Trainer\0 ""LUA\0 " #define LEN_SWTYPES "\006" #define TR_SWTYPES "None\0 ""Toggle""2POS\0 ""3POS\0" @@ -347,6 +348,7 @@ #define TR_VTRAINER_MASTER_JACK "Master/Jack\0 " #define TR_VTRAINER_SLAVE_JACK "Slave/Jack\0 " #define TR_VTRAINER_MASTER_SBUS_MODULE "Master/SBUS Module" +#define TR_VTRAINER_MASTER_IBUS_MODULE "Master/IBUS Module" #define TR_VTRAINER_MASTER_CPPM_MODULE "Master/CPPM Module" #define TR_VTRAINER_MASTER_BATTERY "Master/Serial\0 " #define TR_VTRAINER_BLUETOOTH TR("Master/BT\0 ""Slave/BT\0", "Master/Bluetooth\0 ""Slave/Bluetooth\0 ") diff --git a/radio/src/translations/se.h.txt b/radio/src/translations/se.h.txt index 21bda0abace..0c4ba7b5b0a 100644 --- a/radio/src/translations/se.h.txt +++ b/radio/src/translations/se.h.txt @@ -69,6 +69,7 @@ #define LEN_AUX_SERIAL_MODES "\022" #define TR_AUX_SERIAL_MODES "Av\0 ""Spegling av S-Port""Telemetri\0 ""SBUS Trainer\0 ""LUA\0 ""CLI\0 ""GPS\0 ""Debug\0 " +//#define TR_AUX_SERIAL_MODES "Debug\0 ""Spegling av S-Port""Telemetri\0 ""SBUS Trainer\0 ""IBUS Trainer\0 ""LUA\0 " #define LEN_SWTYPES "\006" #define TR_SWTYPES "Ingen\0""Flipp\0""2Pos\0 ""3Pos\0" @@ -350,6 +351,7 @@ #define TR_VTRAINER_MASTER_JACK "Lärare/Uttag\0 " #define TR_VTRAINER_SLAVE_JACK "Elev./Uttag\0 " #define TR_VTRAINER_MASTER_SBUS_MODULE "Lärare/SBUS-Modul\0" +#define TR_VTRAINER_MASTER_IBUS_MODULE "Lärare/IBUS-Modul\0" #define TR_VTRAINER_MASTER_CPPM_MODULE "Lärare/CPPM-Modul\0" #define TR_VTRAINER_MASTER_BATTERY "Lärare/Serial\0 " #define TR_VTRAINER_BLUETOOTH TR("Master/BT\0 ""Slave/BT\0", "Master/Bluetooth\0 ""Slave/Bluetooth\0 ") From e7bfec0121cf5d325c0b0d4203a622600f978962 Mon Sep 17 00:00:00 2001 From: wimalopaan Date: Tue, 18 Jan 2022 12:32:24 +0100 Subject: [PATCH 05/31] fixed use of 1ms tick --- radio/src/sbus.cpp | 46 +++++++++++++++++++++++++++------------------- 1 file changed, 27 insertions(+), 19 deletions(-) diff --git a/radio/src/sbus.cpp b/radio/src/sbus.cpp index cefb66c1b84..fc0775743ca 100644 --- a/radio/src/sbus.cpp +++ b/radio/src/sbus.cpp @@ -25,14 +25,14 @@ #define SBUS_FRAME_GAP_DELAY 1000 // 500uS -#define SBUS_START_BYTE 0x0F -#define SBUS_END_BYTE 0x00 -#define SBUS_FLAGS_IDX 23 +//#define SBUS_START_BYTE 0x0F +//#define SBUS_END_BYTE 0x00 +//#define SBUS_FLAGS_IDX 23 #define SBUS_FRAMELOST_BIT 2 #define SBUS_FAILSAFE_BIT 3 -#define SBUS_CH_BITS 11 -#define SBUS_CH_MASK ((1<>= SBUS_CH_BITS; -// } + uint32_t inputbitsavailable = 0; + uint32_t inputbits = 0; + for (uint32_t i=0; i>= SBUS_CH_BITS; + } -// ppmInputValidityTimer = PPM_IN_VALID_TIMEOUT; -//} + ppmInputValidityTimer = PPM_IN_VALID_TIMEOUT; +} //void processSbusInput() //{ @@ -229,7 +229,10 @@ namespace SBus { } void sbusTrainerPauseCheck() { #if !defined(SIMU) +// GPIO_SetBits(EXTMODULE_TX_GPIO, EXTMODULE_TX_GPIO_PIN); SBus::Servo::tick1ms(); + processSbusInput(); +// GPIO_ResetBits(EXTMODULE_TX_GPIO, EXTMODULE_TX_GPIO_PIN); #endif } @@ -272,10 +275,15 @@ void processSbusInput() { #if !defined(SIMU) uint8_t rxchar; +// GPIO_SetBits(EXTMODULE_TX_GPIO, EXTMODULE_TX_GPIO_PIN); +// GPIO_ResetBits(EXTMODULE_TX_GPIO, EXTMODULE_TX_GPIO_PIN); + while (sbusGetByte(&rxchar)) { SBus::Servo::process(rxchar, [&](){ +// GPIO_SetBits(EXTMODULE_TX_GPIO, EXTMODULE_TX_GPIO_PIN); SBus::Servo::convert(ppmInput); ppmInputValidityTimer = PPM_IN_VALID_TIMEOUT; +// GPIO_ResetBits(EXTMODULE_TX_GPIO, EXTMODULE_TX_GPIO_PIN); }); } #endif From e416f16621d3bac23aae8b19f86af2b304c4f3b4 Mon Sep 17 00:00:00 2001 From: wimalopaan Date: Tue, 18 Jan 2022 12:55:11 +0100 Subject: [PATCH 06/31] do not call SBus::tick1ms() if not SBUS enabled --- radio/src/sbus.cpp | 11 +++++++++-- 1 file changed, 9 insertions(+), 2 deletions(-) diff --git a/radio/src/sbus.cpp b/radio/src/sbus.cpp index fc0775743ca..44dda778295 100644 --- a/radio/src/sbus.cpp +++ b/radio/src/sbus.cpp @@ -230,8 +230,15 @@ namespace SBus { void sbusTrainerPauseCheck() { #if !defined(SIMU) // GPIO_SetBits(EXTMODULE_TX_GPIO, EXTMODULE_TX_GPIO_PIN); - SBus::Servo::tick1ms(); - processSbusInput(); + + if ((g_eeGeneral.auxSerialMode == UART_MODE_SBUS_TRAINER) + #ifdef AUX2_SERIAL + || (g_eeGeneral.aux2SerialMode == UART_MODE_SBUS_TRAINER) + #endif + ) { + SBus::Servo::tick1ms(); + processSbusInput(); + } // GPIO_ResetBits(EXTMODULE_TX_GPIO, EXTMODULE_TX_GPIO_PIN); #endif } From c0cbf9a8e4b50490c8f7c01ff39a0dc52e3dfb9b Mon Sep 17 00:00:00 2001 From: wimalopaan Date: Tue, 18 Jan 2022 19:28:04 +0100 Subject: [PATCH 07/31] restored switches.cpp --- radio/src/switches.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/radio/src/switches.cpp b/radio/src/switches.cpp index 8d7708d733d..33ee1080a36 100644 --- a/radio/src/switches.cpp +++ b/radio/src/switches.cpp @@ -919,6 +919,7 @@ void logicalSwitchesTimerTick() } } } + } } else { if (ls->v1 != SWSRC_NONE) { // only if used / source set @@ -930,6 +931,7 @@ void logicalSwitchesTimerTick() } } } + } } } else if (ls->func == LS_FUNC_EDGE) { From 119d5a95bbb2c9b37e598f34f1f027165032fd9b Mon Sep 17 00:00:00 2001 From: wimalopaan Date: Wed, 19 Jan 2022 12:00:49 +0100 Subject: [PATCH 08/31] - cleaned code - fixed taranis radios --- radio/src/dataconstants.h | 1 + radio/src/ibus.cpp | 42 ++++---- radio/src/ibus.h | 1 - radio/src/sbus.cpp | 106 ++++++++++--------- radio/src/targets/taranis/board.h | 18 ++++ radio/src/targets/taranis/trainer_driver.cpp | 53 +++++++++- radio/src/tasks.cpp | 7 +- radio/src/trainer.cpp | 3 + radio/src/trainer.h | 36 +++++++ radio/src/translations/untranslated.h | 2 + 10 files changed, 193 insertions(+), 76 deletions(-) diff --git a/radio/src/dataconstants.h b/radio/src/dataconstants.h index 0b18f45302a..abbb2897f6d 100644 --- a/radio/src/dataconstants.h +++ b/radio/src/dataconstants.h @@ -188,6 +188,7 @@ enum TrainerMode { TRAINER_MODE_MASTER_TRAINER_JACK, TRAINER_MODE_SLAVE, TRAINER_MODE_MASTER_SBUS_EXTERNAL_MODULE, + TRAINER_MODE_MASTER_IBUS_EXTERNAL_MODULE, TRAINER_MODE_MASTER_CPPM_EXTERNAL_MODULE, TRAINER_MODE_MASTER_SERIAL, TRAINER_MODE_MASTER_BLUETOOTH, diff --git a/radio/src/ibus.cpp b/radio/src/ibus.cpp index 5d97d970115..ddb9086377f 100644 --- a/radio/src/ibus.cpp +++ b/radio/src/ibus.cpp @@ -1,14 +1,14 @@ #include "ibus.h" #include "opentx.h" +#include "trainer.h" #include #include -//#define IBUS_FRAME_GAP_DELAY 2000 // 1ms - -#define IBUS_VALUE_MIN 988 -#define IBUS_VALUE_MAX 2011 -#define IBUS_VALUE_CENTER 1500 +#ifdef __GNUC__ +# pragma GCC diagnostic push +# pragma GCC diagnostic error "-Wswitch" // unfortunately the project uses -Wnoswitch +#endif namespace IBus { struct CheckSum final { @@ -16,7 +16,7 @@ namespace IBus { mSum = std::numeric_limits::max(); } inline uint8_t operator+=(const uint8_t b) { - mSum -= static_cast(b); + mSum -= b; return b; } inline uint8_t highByte() const { @@ -40,27 +40,27 @@ namespace IBus { uint16_t mSum = std::numeric_limits::max(); }; - uint16_t clamp(uint16_t v, uint16_t lower, uint16_t upper) { - return (v < lower) ? lower : ((v > upper) ? upper: v); - } - - int16_t convertIbusToPuls(uint16_t const ibusValue) { - const uint16_t clamped = clamp(ibusValue, IBUS_VALUE_MIN, IBUS_VALUE_MAX); - return (clamped - IBUS_VALUE_CENTER); - } struct Servo { + using IBus = Trainer::Protocol::IBus; + using MesgType = IBus::MesgType; + enum class State : uint8_t {Undefined, GotStart20, Data, CheckL, CheckH}; - static inline void process(const uint8_t b, std::function f) { - switch(mState) { + + static inline int16_t convertIbusToPuls(uint16_t const ibusValue) { + return Trainer::clamp(ibusValue - IBus::CenterValue); + } + + static inline void process(const uint8_t b, const std::function f) { + switch(mState) { // enum-switch -> no default (intentional) case State::Undefined: csum.reset(); - if (b == 0x20) { + if (b == IBus::StartByte1) { csum += b; mState = State::GotStart20; } break; case State::GotStart20: - if (b == 0x40) { + if (b == IBus::StartByte2) { csum += b; mState = State::Data; mIndex = 0; @@ -93,7 +93,7 @@ namespace IBus { break; } } - static inline void convert(int16_t* pulses) { + static inline void convert(int16_t* const pulses) { for (size_t chi{0}; chi < MAX_TRAINER_CHANNELS; chi++) { if (chi < 14) { const uint8_t h = ibusFrame[2 * chi + 1] & 0x0f; @@ -111,7 +111,6 @@ namespace IBus { } } private: - using MesgType = std::array; // 0x20, 0x40 , 28 Bytes, checkH, checkL static CheckSum csum; static State mState; static MesgType ibusFrame; @@ -139,3 +138,6 @@ void processIbusInput() { #endif } +#ifdef __GNUC__ +# pragma GCC diagnostic pop +#endif diff --git a/radio/src/ibus.h b/radio/src/ibus.h index e2dcedd11c0..cd8493f828a 100644 --- a/radio/src/ibus.h +++ b/radio/src/ibus.h @@ -1,7 +1,6 @@ #pragma once #define IBUS_BAUDRATE 115200 -#define IBUS_FRAME_SIZE 32 void processIbusInput(); diff --git a/radio/src/sbus.cpp b/radio/src/sbus.cpp index 44dda778295..8f16c3f6d09 100644 --- a/radio/src/sbus.cpp +++ b/radio/src/sbus.cpp @@ -127,13 +127,14 @@ void processSbusFrame(uint8_t * sbus, int16_t * pulses, uint32_t size) namespace SBus { struct Servo { + using SBus = Trainer::Protocol::SBus; + using MesgType = SBus::MesgType; + static_assert(std::tuple_size::value == (SBUS_FRAME_SIZE - 2), "consistency check"); + enum class State : uint8_t {Undefined, Data, GotEnd, WaitEnd}; static constexpr uint8_t mPauseCount{2}; // 2ms - - static constexpr uint8_t mFrameLostMask{1 << SBUS_FRAMELOST_BIT}; - static constexpr uint8_t mFailSafeMask{1 << SBUS_FAILSAFE_BIT}; - + static inline void tick1ms() { if (mPauseCounter > 0) { --mPauseCounter; @@ -142,25 +143,30 @@ namespace SBus { mState = State::Undefined; } } + + static inline int16_t convertSbusToPuls(uint16_t const sbusValue) { + const int16_t centered = sbusValue - SBus::CenterValue; + return Trainer::clamp((centered * 5) / 8); + } - static inline void process(const uint8_t b, std::function f) { + static inline void process(const uint8_t b, const std::function f) { mPauseCounter = mPauseCount; - switch(mState) { + switch(mState) { // enum-switch -> no default (intentional) case State::Undefined: - if (b == 0x00) { + if (b == SBus::EndByte) { mState = State::GotEnd; } - else if (b == 0x0f) { + else if (b == SBus::StartByte) { mState = State::Data; mIndex = 0; } break; case State::GotEnd: - if (b == 0x0f) { + if (b == SBus::StartByte) { mState = State::Data; mIndex = 0; } - else if (b == 0x00) { + else if (b == SBus::EndByte) { mState = State::GotEnd; } else { @@ -169,7 +175,7 @@ namespace SBus { break; case State::Data: mData[mIndex] = b; - if (mIndex >= (mData.size() - 1)) { + if (mIndex >= (mData.size() - 1)) { // got last byte mState = State::WaitEnd; } else { @@ -177,9 +183,10 @@ namespace SBus { } break; case State::WaitEnd: - if (b == 0x00) { + if (b == SBus::EndByte) { mState = State::GotEnd; - if (!((mData[mData.size() - 1] & mFrameLostMask) || (mData[mData.size() - 1] & mFailSafeMask))) { + uint8_t& statusByte = mData[mData.size() - 1]; // last byte + if (!((statusByte & SBus::FrameLostMask) || (statusByte & SBus::FailSafeMask))) { f(); ++mPackages; } @@ -189,57 +196,59 @@ namespace SBus { } break; } - } - static inline void convert(int16_t* pulses) { - pulses[0] = (uint16_t) (((mData[0] | mData[1] << 8)) & 0x07FF); - pulses[1] = (uint16_t) ((mData[1]>>3 | mData[2] <<5) & 0x07FF); - pulses[2] = (uint16_t) ((mData[2]>>6 | mData[3] <<2 |mData[4]<<10) & 0x07FF); - pulses[3] = (uint16_t) ((mData[4]>>1 | mData[5] <<7) & 0x07FF); - pulses[4] = (uint16_t) ((mData[5]>>4 | mData[6] <<4) & 0x07FF); - pulses[5] = (uint16_t) ((mData[6]>>7 | mData[7] <<1 |mData[8]<<9) & 0x07FF); - pulses[6] = (uint16_t) ((mData[8]>>2 | mData[9] <<6) & 0x07FF); - pulses[7] = (uint16_t) ((mData[9]>>5 | mData[10]<<3) & 0x07FF); - pulses[8] = (uint16_t) ((mData[11] | mData[12]<<8) & 0x07FF); - pulses[9] = (uint16_t) ((mData[12]>>3 | mData[13]<<5) & 0x07FF); - pulses[10] = (uint16_t) ((mData[13]>>6 | mData[14]<<2 |mData[15]<<10) & 0x07FF); - pulses[11] = (uint16_t) ((mData[15]>>1 | mData[16]<<7) & 0x07FF); - pulses[12] = (uint16_t) ((mData[16]>>4 | mData[17]<<4) & 0x07FF); - pulses[13] = (uint16_t) ((mData[17]>>7 | mData[18]<<1 |mData[19]<<9) & 0x07FF); - pulses[14] = (uint16_t) ((mData[19]>>2 | mData[20]<<6) & 0x07FF); - pulses[15] = (uint16_t) ((mData[20]>>5 | mData[21]<<3) & 0x07FF); + } + static inline void convert(int16_t* const pulses) { + static_assert(MAX_TRAINER_CHANNELS == 16); + pulses[0] = (uint16_t) (((mData[0] | mData[1] << 8)) & SBus::ValueMask); + pulses[1] = (uint16_t) ((mData[1]>>3 | mData[2] <<5) & SBus::ValueMask); + pulses[2] = (uint16_t) ((mData[2]>>6 | mData[3] <<2 | mData[4]<<10) & SBus::ValueMask); + pulses[3] = (uint16_t) ((mData[4]>>1 | mData[5] <<7) & SBus::ValueMask); + pulses[4] = (uint16_t) ((mData[5]>>4 | mData[6] <<4) & SBus::ValueMask); + pulses[5] = (uint16_t) ((mData[6]>>7 | mData[7] <<1 | mData[8]<<9) & SBus::ValueMask); + pulses[6] = (uint16_t) ((mData[8]>>2 | mData[9] <<6) & SBus::ValueMask); + pulses[7] = (uint16_t) ((mData[9]>>5 | mData[10]<<3) & SBus::ValueMask); + pulses[8] = (uint16_t) ((mData[11] | mData[12]<<8) & SBus::ValueMask); + pulses[9] = (uint16_t) ((mData[12]>>3 | mData[13]<<5) & SBus::ValueMask); + pulses[10] = (uint16_t) ((mData[13]>>6 | mData[14]<<2 | mData[15]<<10) & SBus::ValueMask); + pulses[11] = (uint16_t) ((mData[15]>>1 | mData[16]<<7) & SBus::ValueMask); + pulses[12] = (uint16_t) ((mData[16]>>4 | mData[17]<<4) & SBus::ValueMask); + pulses[13] = (uint16_t) ((mData[17]>>7 | mData[18]<<1 | mData[19]<<9) & SBus::ValueMask); + pulses[14] = (uint16_t) ((mData[19]>>2 | mData[20]<<6) & SBus::ValueMask); + pulses[15] = (uint16_t) ((mData[20]>>5 | mData[21]<<3) & SBus::ValueMask); - for(size_t i = 0; i < 16; ++i) { - pulses[i] -= SBUS_CH_CENTER; - pulses[i] *= 5; - pulses[i] /= 8; + for(size_t i = 0; i < MAX_TRAINER_CHANNELS; ++i) { + pulses[i] = convertSbusToPuls(pulses[i]); } } - using MesgType = std::array; + private: static State mState; static MesgType mData; static uint8_t mIndex; static uint16_t mPackages; static uint8_t mPauseCounter; }; - Servo::State Servo::mState{State::Undefined}; + Servo::State Servo::mState{Servo::State::Undefined}; Servo::MesgType Servo::mData; - uint8_t Servo::mIndex{}; - uint16_t Servo::mPackages{}; + uint8_t Servo::mIndex{0}; + uint16_t Servo::mPackages{0}; uint8_t Servo::mPauseCounter{Servo::mPauseCount}; // 2 ms } + void sbusTrainerPauseCheck() { #if !defined(SIMU) -// GPIO_SetBits(EXTMODULE_TX_GPIO, EXTMODULE_TX_GPIO_PIN); - +# if defined(AUX_SERIAL) || defined(AUX2_SERIAL) || defined(TRAINER_MODULE_SBUS) if ((g_eeGeneral.auxSerialMode == UART_MODE_SBUS_TRAINER) - #ifdef AUX2_SERIAL + #if defined(AUX2_SERIAL) || (g_eeGeneral.aux2SerialMode == UART_MODE_SBUS_TRAINER) + #endif + #if defined(TRAINER_MODULE_SBUS) + || (g_model.trainerData.mode == TRAINER_MODE_MASTER_SBUS_EXTERNAL_MODULE) #endif ) { SBus::Servo::tick1ms(); processSbusInput(); } -// GPIO_ResetBits(EXTMODULE_TX_GPIO, EXTMODULE_TX_GPIO_PIN); +# endif #endif } @@ -281,17 +290,16 @@ void sbusTrainerPauseCheck() { void processSbusInput() { #if !defined(SIMU) uint8_t rxchar; - -// GPIO_SetBits(EXTMODULE_TX_GPIO, EXTMODULE_TX_GPIO_PIN); -// GPIO_ResetBits(EXTMODULE_TX_GPIO, EXTMODULE_TX_GPIO_PIN); - + while (sbusGetByte(&rxchar)) { SBus::Servo::process(rxchar, [&](){ -// GPIO_SetBits(EXTMODULE_TX_GPIO, EXTMODULE_TX_GPIO_PIN); SBus::Servo::convert(ppmInput); ppmInputValidityTimer = PPM_IN_VALID_TIMEOUT; -// GPIO_ResetBits(EXTMODULE_TX_GPIO, EXTMODULE_TX_GPIO_PIN); }); } #endif } + +#ifdef __GNUC__ +# pragma GCC diagnostic pop +#endif diff --git a/radio/src/targets/taranis/board.h b/radio/src/targets/taranis/board.h index d2c77e8bf17..03c33ae7fff 100644 --- a/radio/src/targets/taranis/board.h +++ b/radio/src/targets/taranis/board.h @@ -169,8 +169,26 @@ uint32_t isBootloaderStart(const uint8_t * buffer); #if defined(TRAINER_MODULE_SBUS) void init_trainer_module_sbus(); + void init_trainer_module_ibus(); void stop_trainer_module_sbus(); +<<<<<<< HEAD int trainerModuleSbusGetByte(uint8_t* byte); +======= +#else + #define init_trainer_module_sbus() + #define init_trainer_module_ibus() + #define stop_trainer_module_sbus() +#endif + +#if defined(INTMODULE_HEARTBEAT_GPIO) +void init_intmodule_heartbeat(); +void stop_intmodule_heartbeat(); +void check_intmodule_heartbeat(); +#else +#define init_intmodule_heartbeat() +#define stop_intmodule_heartbeat() +#define check_intmodule_heartbeat() +>>>>>>> 87eb2196d (- cleaned code) #endif void check_telemetry_exti(); diff --git a/radio/src/targets/taranis/trainer_driver.cpp b/radio/src/targets/taranis/trainer_driver.cpp index 919c382402c..34858ae3602 100644 --- a/radio/src/targets/taranis/trainer_driver.cpp +++ b/radio/src/targets/taranis/trainer_driver.cpp @@ -284,9 +284,9 @@ void init_trainer_module_sbus() GPIO_InitStructure.GPIO_Speed = GPIO_Speed_2MHz; GPIO_Init(TRAINER_MODULE_SBUS_GPIO, &GPIO_InitStructure); - USART_InitStructure.USART_BaudRate = 100000; + USART_InitStructure.USART_BaudRate = SBUS_BAUDRATE; USART_InitStructure.USART_WordLength = USART_WordLength_9b; - USART_InitStructure.USART_StopBits = USART_StopBits_1; + USART_InitStructure.USART_StopBits = USART_StopBits_1; // this should be 2 stop bits USART_InitStructure.USART_Parity = USART_Parity_Even; USART_InitStructure.USART_HardwareFlowControl = USART_HardwareFlowControl_None; USART_InitStructure.USART_Mode = USART_Mode_Rx; @@ -317,6 +317,55 @@ void init_trainer_module_sbus() DMA_Cmd(TRAINER_MODULE_SBUS_DMA_STREAM, ENABLE); } +void init_trainer_module_ibus() +{ + EXTERNAL_MODULE_ON(); + + USART_InitTypeDef USART_InitStructure; + GPIO_InitTypeDef GPIO_InitStructure; + + GPIO_PinAFConfig(TRAINER_MODULE_SBUS_GPIO, TRAINER_MODULE_SBUS_GPIO_PinSource, TRAINER_MODULE_SBUS_GPIO_AF); + + GPIO_InitStructure.GPIO_Pin = TRAINER_MODULE_SBUS_GPIO_PIN; + GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF; + GPIO_InitStructure.GPIO_OType = GPIO_OType_PP; + GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_UP; + GPIO_InitStructure.GPIO_Speed = GPIO_Speed_2MHz; + GPIO_Init(TRAINER_MODULE_SBUS_GPIO, &GPIO_InitStructure); + + USART_InitStructure.USART_BaudRate = IBUS_BAUDRATE; + USART_InitStructure.USART_WordLength = USART_WordLength_8b; + USART_InitStructure.USART_StopBits = USART_StopBits_1; + USART_InitStructure.USART_Parity = USART_Parity_No; + USART_InitStructure.USART_HardwareFlowControl = USART_HardwareFlowControl_None; + USART_InitStructure.USART_Mode = USART_Mode_Rx; + USART_Init(TRAINER_MODULE_SBUS_USART, &USART_InitStructure); + + DMA_InitTypeDef DMA_InitStructure; + trainerSbusFifo.clear(); + USART_ITConfig(TRAINER_MODULE_SBUS_USART, USART_IT_RXNE, DISABLE); + USART_ITConfig(TRAINER_MODULE_SBUS_USART, USART_IT_TXE, DISABLE); + DMA_InitStructure.DMA_Channel = TRAINER_MODULE_SBUS_DMA_CHANNEL; + DMA_InitStructure.DMA_PeripheralBaseAddr = CONVERT_PTR_UINT(&TRAINER_MODULE_SBUS_USART->DR); + DMA_InitStructure.DMA_Memory0BaseAddr = CONVERT_PTR_UINT(trainerSbusFifo.buffer()); + DMA_InitStructure.DMA_DIR = DMA_DIR_PeripheralToMemory; + DMA_InitStructure.DMA_BufferSize = trainerSbusFifo.size(); + DMA_InitStructure.DMA_PeripheralInc = DMA_PeripheralInc_Disable; + DMA_InitStructure.DMA_MemoryInc = DMA_MemoryInc_Enable; + DMA_InitStructure.DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte; + DMA_InitStructure.DMA_MemoryDataSize = DMA_MemoryDataSize_Byte; + DMA_InitStructure.DMA_Mode = DMA_Mode_Circular; + DMA_InitStructure.DMA_Priority = DMA_Priority_Low; + DMA_InitStructure.DMA_FIFOMode = DMA_FIFOMode_Disable; + DMA_InitStructure.DMA_FIFOThreshold = DMA_FIFOThreshold_Full; + DMA_InitStructure.DMA_MemoryBurst = DMA_MemoryBurst_Single; + DMA_InitStructure.DMA_PeripheralBurst = DMA_PeripheralBurst_Single; + DMA_Init(TRAINER_MODULE_SBUS_DMA_STREAM, &DMA_InitStructure); + USART_DMACmd(TRAINER_MODULE_SBUS_USART, USART_DMAReq_Rx, ENABLE); + USART_Cmd(TRAINER_MODULE_SBUS_USART, ENABLE); + DMA_Cmd(TRAINER_MODULE_SBUS_DMA_STREAM, ENABLE); +} + void stop_trainer_module_sbus() { DMA_Cmd(TRAINER_MODULE_SBUS_DMA_STREAM, DISABLE); diff --git a/radio/src/tasks.cpp b/radio/src/tasks.cpp index ba55b8f681d..6f43fd7fd90 100644 --- a/radio/src/tasks.cpp +++ b/radio/src/tasks.cpp @@ -88,17 +88,16 @@ constexpr uint8_t MIXER_MAX_PERIOD = MAX_REFRESH_RATE / 1000 /*ms*/; void execMixerFrequentActions() { -#if defined(SBUS_TRAINER) - // SBUS trainer +#if defined(SBUS_TRAINER) && (defined(AUX_SERIAL) || defined(AUX2_SERIAL)) if ((g_eeGeneral.auxSerialMode == UART_MODE_SBUS_TRAINER) - #ifdef AUX2_SERIAL + #if defined(AUX2_SERIAL) || (g_eeGeneral.aux2SerialMode == UART_MODE_SBUS_TRAINER) #endif ) { processSbusInput(); } else if ((g_eeGeneral.auxSerialMode == UART_MODE_IBUS_TRAINER) - #ifdef AUX2_SERIAL + #if defined(AUX2_SERIAL) || (g_eeGeneral.aux2SerialMode == UART_MODE_IBUS_TRAINER) #endif ) { diff --git a/radio/src/trainer.cpp b/radio/src/trainer.cpp index 632b1f18385..9340193c9c8 100644 --- a/radio/src/trainer.cpp +++ b/radio/src/trainer.cpp @@ -130,6 +130,9 @@ void checkTrainerSettings() init_trainer_module_sbus(); sbusSetGetByte(trainerModuleSbusGetByte); break; + case TRAINER_MODE_MASTER_IBUS_EXTERNAL_MODULE: + init_trainer_module_ibus(); + break; #endif } diff --git a/radio/src/trainer.h b/radio/src/trainer.h index 9a81901a5cc..d1b6648b841 100644 --- a/radio/src/trainer.h +++ b/radio/src/trainer.h @@ -24,6 +24,42 @@ #include "dataconstants.h" +namespace Trainer { + static constexpr int16_t MaxValue = +512; + static constexpr int16_t MinValue = -512; + + static inline int16_t clamp(int16_t const v) { + return (v < MinValue) ? MinValue : ((v > MaxValue) ? MaxValue : v); + } + + namespace Protocol { + struct SBus { + using MesgType = std::array; + + static constexpr uint8_t ValueBits = 11; + static constexpr uint16_t ValueMask = ((1 << ValueBits) - 1); + + static constexpr uint8_t FrameLostBit = 2; + static constexpr uint8_t FailSafeBit = 3; + static constexpr uint8_t StartByte = 0x0f; + static constexpr uint8_t EndByte = 0x00; + static constexpr uint8_t FrameLostMask{1 << FrameLostBit}; + static constexpr uint8_t FailSafeMask{1 << FailSafeBit}; + + static constexpr uint16_t CenterValue = 0x3e0; + }; + struct IBus { + using MesgType = std::array; // 0x20, 0x40 , 28 Bytes, checkH, checkL + + static constexpr uint8_t StartByte1 = 0x20; + static constexpr uint8_t StartByte2 = 0x40; + static constexpr uint16_t MaxValue = 988; + static constexpr uint16_t MinValue = 2011; + static constexpr uint16_t CenterValue = (MaxValue + MinValue) / 2; + }; + } +} + // Trainer input channels extern int16_t ppmInput[MAX_TRAINER_CHANNELS]; diff --git a/radio/src/translations/untranslated.h b/radio/src/translations/untranslated.h index dd6aad31f6a..4c4dca3ca63 100644 --- a/radio/src/translations/untranslated.h +++ b/radio/src/translations/untranslated.h @@ -87,6 +87,8 @@ TR_VTRAINER_MASTER_SBUS_MODULE TR_VTRAINER_MASTER_CPPM_MODULE \ TR_VTRAINER_MASTER_BATTERY TR_VTRAINER_BLUETOOTH TR_VTRAINER_MULTI +//#define TR_VTRAINERMODES TR_VTRAINER_MASTER_JACK TR_VTRAINER_SLAVE_JACK TR_VTRAINER_MASTER_BATTERY TR_VTRAINER_BLUETOOTH TR_VTRAINER_MULTI + #if defined(PCBHORUS) || defined(PCBNV14) #define LEN_VKEYS "\005" #define TR_VKEYS "PGUP\0""PGDN\0""ENTER""MDL\0 ""RTN\0 ""TELE\0""SYS\0 " From c7b6173ed834d3f67a92730602c32fa5a3102bd2 Mon Sep 17 00:00:00 2001 From: wimalopaan Date: Mon, 21 Mar 2022 14:50:49 +0100 Subject: [PATCH 09/31] wip: just compiles after rebase --- radio/src/CMakeLists.txt | 1 + radio/src/ibus.cpp | 2 +- radio/src/sbus.cpp | 59 ++++++++++++--------------- radio/src/switches.cpp | 2 - radio/src/tasks.cpp | 31 +++++++++----- radio/src/trainer.cpp | 1 + radio/src/translations/cn.h.txt | 3 +- radio/src/translations/cz.h.txt | 3 +- radio/src/translations/de.h.txt | 3 +- radio/src/translations/en.h.txt | 3 +- radio/src/translations/es.h.txt | 3 +- radio/src/translations/fi.h.txt | 3 +- radio/src/translations/fr.h.txt | 3 +- radio/src/translations/it.h.txt | 3 +- radio/src/translations/nl.h.txt | 3 +- radio/src/translations/pl.h.txt | 3 +- radio/src/translations/pt.h.txt | 3 +- radio/src/translations/se.h.txt | 3 +- radio/src/translations/untranslated.h | 4 +- 19 files changed, 62 insertions(+), 74 deletions(-) diff --git a/radio/src/CMakeLists.txt b/radio/src/CMakeLists.txt index 63dddacd851..21f9b42527a 100644 --- a/radio/src/CMakeLists.txt +++ b/radio/src/CMakeLists.txt @@ -476,6 +476,7 @@ set(SRC model_init.cpp serial.cpp sbus.cpp + ibus.cpp ) if(GUI) diff --git a/radio/src/ibus.cpp b/radio/src/ibus.cpp index ddb9086377f..963f205f227 100644 --- a/radio/src/ibus.cpp +++ b/radio/src/ibus.cpp @@ -129,7 +129,7 @@ void processIbusInput() { #if !defined(SIMU) uint8_t rxchar; - while (sbusGetByte(&rxchar)) { + while (sbusAuxGetByte(&rxchar)) { IBus::Servo::process(rxchar, [&](){ IBus::Servo::convert(ppmInput); ppmInputValidityTimer = PPM_IN_VALID_TIMEOUT; diff --git a/radio/src/sbus.cpp b/radio/src/sbus.cpp index 8f16c3f6d09..f61b6c70357 100644 --- a/radio/src/sbus.cpp +++ b/radio/src/sbus.cpp @@ -66,33 +66,33 @@ void sbusSetGetByte(int (*fct)(uint8_t*)) } // Range for pulses (ppm input) is [-512:+512] -void processSbusFrame(uint8_t * sbus, int16_t * pulses, uint32_t size) -{ - if (size != SBUS_FRAME_SIZE || sbus[0] != SBUS_START_BYTE || - sbus[SBUS_FRAME_SIZE - 1] != SBUS_END_BYTE) { - return; // not a valid SBUS frame - } - if ((sbus[SBUS_FLAGS_IDX] & (1 << SBUS_FAILSAFE_BIT)) || - (sbus[SBUS_FLAGS_IDX] & (1 << SBUS_FRAMELOST_BIT))) { - return; // SBUS invalid frame or failsafe mode - } +//void processSbusFrame(uint8_t * sbus, int16_t * pulses, uint32_t size) +//{ +// if (size != SBUS_FRAME_SIZE || sbus[0] != SBUS_START_BYTE || +// sbus[SBUS_FRAME_SIZE - 1] != SBUS_END_BYTE) { +// return; // not a valid SBUS frame +// } +// if ((sbus[SBUS_FLAGS_IDX] & (1 << SBUS_FAILSAFE_BIT)) || +// (sbus[SBUS_FLAGS_IDX] & (1 << SBUS_FRAMELOST_BIT))) { +// return; // SBUS invalid frame or failsafe mode +// } - sbus++; // skip start byte +// sbus++; // skip start byte - uint32_t inputbitsavailable = 0; - uint32_t inputbits = 0; - for (uint32_t i=0; i>= SBUS_CH_BITS; - } +// uint32_t inputbitsavailable = 0; +// uint32_t inputbits = 0; +// for (uint32_t i=0; i>= SBUS_CH_BITS; +// } - ppmInputValidityTimer = PPM_IN_VALID_TIMEOUT; -} +// ppmInputValidityTimer = PPM_IN_VALID_TIMEOUT; +//} //void processSbusInput() //{ @@ -237,14 +237,7 @@ namespace SBus { void sbusTrainerPauseCheck() { #if !defined(SIMU) # if defined(AUX_SERIAL) || defined(AUX2_SERIAL) || defined(TRAINER_MODULE_SBUS) - if ((g_eeGeneral.auxSerialMode == UART_MODE_SBUS_TRAINER) - #if defined(AUX2_SERIAL) - || (g_eeGeneral.aux2SerialMode == UART_MODE_SBUS_TRAINER) - #endif - #if defined(TRAINER_MODULE_SBUS) - || (g_model.trainerData.mode == TRAINER_MODE_MASTER_SBUS_EXTERNAL_MODULE) - #endif - ) { + if (hasSerialMode(UART_MODE_SBUS_TRAINER) >= 0) { SBus::Servo::tick1ms(); processSbusInput(); } @@ -291,7 +284,7 @@ void processSbusInput() { #if !defined(SIMU) uint8_t rxchar; - while (sbusGetByte(&rxchar)) { + while (sbusAuxGetByte(&rxchar)) { SBus::Servo::process(rxchar, [&](){ SBus::Servo::convert(ppmInput); ppmInputValidityTimer = PPM_IN_VALID_TIMEOUT; diff --git a/radio/src/switches.cpp b/radio/src/switches.cpp index 33ee1080a36..0cb43ae76d1 100644 --- a/radio/src/switches.cpp +++ b/radio/src/switches.cpp @@ -919,7 +919,6 @@ void logicalSwitchesTimerTick() } } } - } } else { if (ls->v1 != SWSRC_NONE) { // only if used / source set @@ -930,7 +929,6 @@ void logicalSwitchesTimerTick() lastValue.state = 1; } } - } } } } diff --git a/radio/src/tasks.cpp b/radio/src/tasks.cpp index 6f43fd7fd90..33cc2fc5412 100644 --- a/radio/src/tasks.cpp +++ b/radio/src/tasks.cpp @@ -88,22 +88,31 @@ constexpr uint8_t MIXER_MAX_PERIOD = MAX_REFRESH_RATE / 1000 /*ms*/; void execMixerFrequentActions() { -#if defined(SBUS_TRAINER) && (defined(AUX_SERIAL) || defined(AUX2_SERIAL)) - if ((g_eeGeneral.auxSerialMode == UART_MODE_SBUS_TRAINER) - #if defined(AUX2_SERIAL) - || (g_eeGeneral.aux2SerialMode == UART_MODE_SBUS_TRAINER) - #endif - ) { +#if defined(SBUS_TRAINER) + // SBUS trainer + if (hasSerialMode(UART_MODE_SBUS_TRAINER) >= 0) { processSbusInput(); } - else if ((g_eeGeneral.auxSerialMode == UART_MODE_IBUS_TRAINER) - #if defined(AUX2_SERIAL) - || (g_eeGeneral.aux2SerialMode == UART_MODE_IBUS_TRAINER) - #endif - ) { + else if (hasSerialMode(UART_MODE_IBUS_TRAINER) >= 0) { processIbusInput(); } #endif +//#if defined(SBUS_TRAINER) && (defined(AUX_SERIAL) || defined(AUX2_SERIAL)) +// if ((g_eeGeneral.auxSerialMode == UART_MODE_SBUS_TRAINER) +// #if defined(AUX2_SERIAL) +// || (g_eeGeneral.aux2SerialMode == UART_MODE_SBUS_TRAINER) +// #endif +// ) { +// processSbusInput(); +// } +// else if ((g_eeGeneral.auxSerialMode == UART_MODE_IBUS_TRAINER) +// #if defined(AUX2_SERIAL) +// || (g_eeGeneral.aux2SerialMode == UART_MODE_IBUS_TRAINER) +// #endif +// ) { +// processIbusInput(); +// } +//#endif #if defined(GYRO) gyro.wakeup(); diff --git a/radio/src/trainer.cpp b/radio/src/trainer.cpp index 9340193c9c8..a1faf287282 100644 --- a/radio/src/trainer.cpp +++ b/radio/src/trainer.cpp @@ -132,6 +132,7 @@ void checkTrainerSettings() break; case TRAINER_MODE_MASTER_IBUS_EXTERNAL_MODULE: init_trainer_module_ibus(); + sbusSetGetByte(trainerModuleSbusGetByte); break; #endif } diff --git a/radio/src/translations/cn.h.txt b/radio/src/translations/cn.h.txt index d4bcc68c2b2..6e31a41e9eb 100644 --- a/radio/src/translations/cn.h.txt +++ b/radio/src/translations/cn.h.txt @@ -63,8 +63,7 @@ #define TR_TRNCHN "CH1CH2CH3CH4" #define LEN_AUX_SERIAL_MODES "\010" -#define TR_AUX_SERIAL_MODES "调试\0 " "回传镜像" "回传输入" "SBUS教练" "LUA脚本\0""CLI\0 ""GPS\0 ""Debug\0 " -//#define TR_AUX_SERIAL_MODES "调试\0 " "回传镜像" "回传输入" "SBUS教练" "IBUS教练" "LUA脚本\0" +#define TR_AUX_SERIAL_MODES "调试\0 " "回传镜像" "回传输入" "SBUS教练" "IBUS教练" "LUA脚本\0""CLI\0 ""GPS\0 ""Debug\0 " #define LEN_SWTYPES "\004" #define TR_SWTYPES "无\0 " "回弹" "2段\0""3段\0" diff --git a/radio/src/translations/cz.h.txt b/radio/src/translations/cz.h.txt index d8befdc7b16..cde711d82b0 100644 --- a/radio/src/translations/cz.h.txt +++ b/radio/src/translations/cz.h.txt @@ -61,8 +61,7 @@ #define TR_TRNCHN "CH1CH2CH3CH4" #define LEN_AUX_SERIAL_MODES "\015" -#define TR_AUX_SERIAL_MODES "VYP\0 ""Telem Mirror\0""Telemetry In\0""SBUS Trenér\0 ""LUA\0 ""CLI\0 ""GPS\0 ""Debug\0 " -//#define TR_AUX_SERIAL_MODES "Debug\0 ""Telem Mirror\0""Telemetry In\0""SBUS Trenér\0 ""IBUS Trenér\0 ""LUA\0 " +#define TR_AUX_SERIAL_MODES "VYP\0 ""Telem Mirror\0""Telemetry In\0""SBUS Trenér\0 ""IBUS Trenér\0 ""LUA\0 ""CLI\0 ""GPS\0 ""Debug\0 " #define LEN_SWTYPES "\013" #define TR_SWTYPES "Žádný\0 ""Bez aretace""2-polohový\0""3-polohový\0" diff --git a/radio/src/translations/de.h.txt b/radio/src/translations/de.h.txt index 57c7d863885..4f9df6a0c86 100644 --- a/radio/src/translations/de.h.txt +++ b/radio/src/translations/de.h.txt @@ -62,8 +62,7 @@ #define TR_TRNCHN "CH1CH2CH3CH4" #define LEN_AUX_SERIAL_MODES "\015" -#define TR_AUX_SERIAL_MODES "AUS\0 ""Telem Mirror\0""Telemetry In\0""SBUS Eingang\0""LUA\0 ""CLI\0 ""GPS\0 ""Debug\0 " -//#define TR_AUX_SERIAL_MODES "AUS\0 ""Telem Mirror\0""Telemetry In\0""SBUS Eingang\0""IBUS Eingang\0""LUA\0 " +#define TR_AUX_SERIAL_MODES "AUS\0 ""Telem Mirror\0""Telemetry In\0""SBUS Eingang\0""IBUS Eingang\0""LUA\0 ""CLI\0 ""GPS\0 ""Debug\0 " #define LEN_SWTYPES "\006" #define TR_SWTYPES "Kein\0 ""Taster""2POS\0 ""3POS\0" diff --git a/radio/src/translations/en.h.txt b/radio/src/translations/en.h.txt index f8972cfdd58..b091b451cc4 100644 --- a/radio/src/translations/en.h.txt +++ b/radio/src/translations/en.h.txt @@ -62,8 +62,7 @@ #define TR_TRNCHN "CH1CH2CH3CH4" #define LEN_AUX_SERIAL_MODES "\015" -#define TR_AUX_SERIAL_MODES "OFF\0 ""Telem Mirror\0""Telemetry In\0""SBUS Trainer\0""LUA\0 ""CLI\0 ""GPS\0 ""Debug\0 " -//#define TR_AUX_SERIAL_MODES "Debug\0 ""Telem Mirror\0""Telemetry In\0""SBUS Trainer\0""IBUS Trainer\0""LUA\0 " +#define TR_AUX_SERIAL_MODES "OFF\0 ""Telem Mirror\0""Telemetry In\0""SBUS Trainer\0""IBUS Trainer\0""LUA\0 ""CLI\0 ""GPS\0 ""Debug\0 " #define LEN_SWTYPES "\006" #define TR_SWTYPES "None\0 ""Toggle""2POS\0 ""3POS\0" diff --git a/radio/src/translations/es.h.txt b/radio/src/translations/es.h.txt index 566e1c24e1c..08e7b640f43 100644 --- a/radio/src/translations/es.h.txt +++ b/radio/src/translations/es.h.txt @@ -62,8 +62,7 @@ #define TR_TRNCHN "CH1CH2CH3CH4" #define LEN_AUX_SERIAL_MODES "\015" -#define TR_AUX_SERIAL_MODES "OFF\0 ""Telem Mirror\0""Telemetría\0 ""Entrenador SBUS""LUA\0 ""CLI\0 ""GPS\0 ""Debug\0 " -//#define TR_AUX_SERIAL_MODES "Debug\0 ""Telem Mirror\0""Telemetría\0 ""Entrenador SBUS""LUA\0 ""Entrenador IBUS""LUA\0 " +#define TR_AUX_SERIAL_MODES "OFF\0 ""Telem Mirror\0""Telemetría\0 ""Entrenador SBUS""Entrenador IBUS""LUA\0 ""CLI\0 ""GPS\0 ""Debug\0 " #define LEN_SWTYPES "\007" #define TR_SWTYPES "Nada\0 ""Palanca""2POS\0 ""3POS\0 " diff --git a/radio/src/translations/fi.h.txt b/radio/src/translations/fi.h.txt index 34ee91d4166..f9b36dbe009 100644 --- a/radio/src/translations/fi.h.txt +++ b/radio/src/translations/fi.h.txt @@ -62,8 +62,7 @@ #define TR_TRNCHN "CH1CH2CH3CH4" #define LEN_AUX_SERIAL_MODES "\015" -#define TR_AUX_SERIAL_MODES "POIS\0 ""S-Port Pelik\0""Telemetry In\0""SBUS Trainer\0""LUA\0 ""CLI\0 ""GPS\0 ""Debug\0 " -//#define TR_AUX_SERIAL_MODES "Debug\0 ""S-Port Pelik\0""Telemetry In\0""SBUS Trainer\0""IBUS Trainer\0""LUA\0 " +#define TR_AUX_SERIAL_MODES "POIS\0 ""S-Port Pelik\0""Telemetry In\0""SBUS Trainer\0""IBUS Trainer\0""LUA\0 ""CLI\0 ""GPS\0 ""Debug\0 " #define LEN_SWTYPES "\006" #define TR_SWTYPES "None\0 ""Toggle""2POS\0 ""3POS\0" diff --git a/radio/src/translations/fr.h.txt b/radio/src/translations/fr.h.txt index 6ffae9b42ec..b2bb23afeb2 100644 --- a/radio/src/translations/fr.h.txt +++ b/radio/src/translations/fr.h.txt @@ -65,8 +65,7 @@ #define TR_TRNCHN "CH1CH2CH3CH4" #define LEN_AUX_SERIAL_MODES "\016" -#define TR_AUX_SERIAL_MODES "OFF\0 ""Recopie Telem\0""Télémétrie In\0""Ecolage SBUS\0 ""LUA\0 ""CLI\0 ""GPS\0 ""Debug\0 " -//#define TR_AUX_SERIAL_MODES "Debug\0 ""Recopie Telem\0""Télémétrie In\0""Ecolage SBUS\0 ""Ecolage IBUS\0 ""LUA\0 " +#define TR_AUX_SERIAL_MODES "OFF\0 ""Recopie Telem\0""Télémétrie In\0""Ecolage SBUS\0 ""Ecolage IBUS\0 ""LUA\0 ""CLI\0 ""GPS\0 ""Debug\0 " #define LEN_SWTYPES "\006" #define TR_SWTYPES "Rien\0 ""Levier""2-POS\0""3-POS\0" diff --git a/radio/src/translations/it.h.txt b/radio/src/translations/it.h.txt index e10d7ac47fb..11f09b52f42 100644 --- a/radio/src/translations/it.h.txt +++ b/radio/src/translations/it.h.txt @@ -63,8 +63,7 @@ #define TR_TRNCHN "ch1ch2ch3ch4" #define LEN_AUX_SERIAL_MODES "\016" -#define TR_AUX_SERIAL_MODES "OFF\0 ""Replica S-Port""Telemetria\0 ""SBUS Trainer\0 ""LUA\0 ""CLI\0 ""GPS\0 ""Debug\0 " -//#define TR_AUX_SERIAL_MODES "Debug\0 ""Replica S-Port""Telemetria\0 ""SBUS Trainer\0 ""IBUS Trainer\0 ""LUA\0 " +#define TR_AUX_SERIAL_MODES "OFF\0 ""Replica S-Port""Telemetria\0 ""SBUS Trainer\0 ""IBUS Trainer\0 ""LUA\0 ""CLI\0 ""GPS\0 ""Debug\0 " #define LEN_SWTYPES "\006" #define TR_SWTYPES "Dis.\0 ""Toggle""2POS\0 ""3POS\0" diff --git a/radio/src/translations/nl.h.txt b/radio/src/translations/nl.h.txt index 475c26d6cad..1cdcce81c14 100644 --- a/radio/src/translations/nl.h.txt +++ b/radio/src/translations/nl.h.txt @@ -62,8 +62,7 @@ #define TR_TRNCHN "CH1CH2CH3CH4" #define LEN_AUX_SERIAL_MODES "\015" -#define TR_AUX_SERIAL_MODES "UIT\0 ""Telem Mirror\0""Telemetry In\0""SBUS Leerling""LUA\0 ""CLI\0 ""GPS\0 ""Debug\0 " -//#define TR_AUX_SERIAL_MODES "Debug\0 ""Telem Mirror\0""Telemetry In\0""SBUS Leerling""IBUS Leerling""LUA\0 " +#define TR_AUX_SERIAL_MODES "UIT\0 ""Telem Mirror\0""Telemetry In\0""SBUS Leerling""IBUS Leerling""LUA\0 ""CLI\0 ""GPS\0 ""Debug\0 " #define LEN_SWTYPES "\006" #define TR_SWTYPES "Geen\0 ""Wissel""2POS\0 ""3POS\0" diff --git a/radio/src/translations/pl.h.txt b/radio/src/translations/pl.h.txt index aa883fa6969..afd63aff9a3 100644 --- a/radio/src/translations/pl.h.txt +++ b/radio/src/translations/pl.h.txt @@ -62,8 +62,7 @@ #define TR_TRNCHN "KN1KN2KN3KN4" #define LEN_AUX_SERIAL_MODES "\015" /*13 decimal*/ -#define TR_AUX_SERIAL_MODES "Wyłącz\0 ""S-Port Kopia ""Telemetria\0 ""Trener SBUS\0 ""LUA\0 ""CLI\0 ""GPS\0 ""Debug\0 " -//#define TR_AUX_SERIAL_MODES "Debug\0 ""S-Port Kopia ""Telemetria\0 ""Trener SBUS\0 ""Trener IBUS\0 ""LUA\0 " +#define TR_AUX_SERIAL_MODES "Wyłącz\0 ""S-Port Kopia ""Telemetria\0 ""Trener SBUS\0 ""Trener IBUS\0 ""LUA\0 ""CLI\0 ""GPS\0 ""Debug\0 " #define LEN_SWTYPES "\006" #define TR_SWTYPES "Brak\0 ""Chwil.""2POZ\0 ""3POZ\0" diff --git a/radio/src/translations/pt.h.txt b/radio/src/translations/pt.h.txt index 07cf176f8eb..64d2cb7de0d 100644 --- a/radio/src/translations/pt.h.txt +++ b/radio/src/translations/pt.h.txt @@ -62,8 +62,7 @@ #define TR_TRNCHN "CH1CH2CH3CH4" #define LEN_AUX_SERIAL_MODES "\017" -#define TR_AUX_SERIAL_MODES "OFF\0 ""S-Port Mirror\0 ""Telemetry\0 ""SBUS Trainer\0 ""LUA\0 ""CLI\0 ""GPS\0 ""Debug\0 " -//#define TR_AUX_SERIAL_MODES "Debug\0 ""S-Port Mirror\0 ""Telemetry\0 ""SBUS Trainer\0 ""IBUS Trainer\0 ""LUA\0 " +#define TR_AUX_SERIAL_MODES "OFF\0 ""S-Port Mirror\0 ""Telemetry\0 ""SBUS Trainer\0 ""IBUS Trainer\0 ""LUA\0 ""CLI\0 ""GPS\0 ""Debug\0 " #define LEN_SWTYPES "\006" #define TR_SWTYPES "None\0 ""Toggle""2POS\0 ""3POS\0" diff --git a/radio/src/translations/se.h.txt b/radio/src/translations/se.h.txt index 0c4ba7b5b0a..a89161d9e4b 100644 --- a/radio/src/translations/se.h.txt +++ b/radio/src/translations/se.h.txt @@ -68,8 +68,7 @@ #define TR_TRNCHN "KN1KN2KN3KN4" #define LEN_AUX_SERIAL_MODES "\022" -#define TR_AUX_SERIAL_MODES "Av\0 ""Spegling av S-Port""Telemetri\0 ""SBUS Trainer\0 ""LUA\0 ""CLI\0 ""GPS\0 ""Debug\0 " -//#define TR_AUX_SERIAL_MODES "Debug\0 ""Spegling av S-Port""Telemetri\0 ""SBUS Trainer\0 ""IBUS Trainer\0 ""LUA\0 " +#define TR_AUX_SERIAL_MODES "Av\0 ""Spegling av S-Port""Telemetri\0 ""SBUS Trainer\0 ""IBUS Trainer\0 ""LUA\0 ""CLI\0 ""GPS\0 ""Debug\0 " #define LEN_SWTYPES "\006" #define TR_SWTYPES "Ingen\0""Flipp\0""2Pos\0 ""3Pos\0" diff --git a/radio/src/translations/untranslated.h b/radio/src/translations/untranslated.h index 4c4dca3ca63..bb90bc0f8b9 100644 --- a/radio/src/translations/untranslated.h +++ b/radio/src/translations/untranslated.h @@ -84,11 +84,9 @@ #define TR_VTRAINERMODES \ TR_VTRAINER_MASTER_OFF TR_VTRAINER_MASTER_JACK TR_VTRAINER_SLAVE_JACK \ - TR_VTRAINER_MASTER_SBUS_MODULE TR_VTRAINER_MASTER_CPPM_MODULE \ + TR_VTRAINER_MASTER_SBUS_MODULE TR_VTRAINER_MASTER_IBUS_MODULE TR_VTRAINER_MASTER_CPPM_MODULE \ TR_VTRAINER_MASTER_BATTERY TR_VTRAINER_BLUETOOTH TR_VTRAINER_MULTI -//#define TR_VTRAINERMODES TR_VTRAINER_MASTER_JACK TR_VTRAINER_SLAVE_JACK TR_VTRAINER_MASTER_BATTERY TR_VTRAINER_BLUETOOTH TR_VTRAINER_MULTI - #if defined(PCBHORUS) || defined(PCBNV14) #define LEN_VKEYS "\005" #define TR_VKEYS "PGUP\0""PGDN\0""ENTER""MDL\0 ""RTN\0 ""TELE\0""SYS\0 " From 8a912d96ae30856aac9014ed630ce3e70fc8a0ba Mon Sep 17 00:00:00 2001 From: wimalopaan Date: Tue, 22 Mar 2022 13:17:41 +0100 Subject: [PATCH 10/31] only allow one trainer-input via serial at a time --- radio/src/gui/gui_common.cpp | 33 ++++++++++++++++++++++++++++++--- 1 file changed, 30 insertions(+), 3 deletions(-) diff --git a/radio/src/gui/gui_common.cpp b/radio/src/gui/gui_common.cpp index 1b67c1330c7..9ea8d0fc15f 100644 --- a/radio/src/gui/gui_common.cpp +++ b/radio/src/gui/gui_common.cpp @@ -380,7 +380,23 @@ int hasSerialMode(int mode) return -1; } -bool isSerialModeAvailable(uint8_t port_nr, int mode) +bool isTrainerInputMode(const int mode) +{ + return (mode == UART_MODE_IBUS_TRAINER) || (mode == UART_MODE_SBUS_TRAINER); +} + +int trainerInputActive() +{ + for(int p = 0; p < MAX_SERIAL_PORTS; ++p) { + const int mode = serialGetMode(p); + if (isTrainerInputMode(mode)) { + return p; + } + } + return -1; +} + +bool isSerialModeAvailable(const uint8_t port_nr, const int mode) { if (mode == UART_MODE_NONE) return true; @@ -416,8 +432,19 @@ bool isSerialModeAvailable(uint8_t port_nr, int mode) return false; #endif - auto p = hasSerialMode(mode); - if (p >= 0 && p != port_nr) return false; + { + const int p = hasSerialMode(mode); + if ((p >= 0) && (p != port_nr)) { + return false; + } + } + { + const int p = trainerInputActive(); + if (isTrainerInputMode(mode) && (p >= 0) && (p != port_nr)) { + return false; + } + } + return true; } From 4c4c6c15e8ddfb4e655bd34bf59126add89e0c43 Mon Sep 17 00:00:00 2001 From: wimalopaan Date: Tue, 22 Mar 2022 13:54:31 +0100 Subject: [PATCH 11/31] fixed board.h (removed merge marker) --- radio/src/targets/taranis/board.h | 17 ----------------- 1 file changed, 17 deletions(-) diff --git a/radio/src/targets/taranis/board.h b/radio/src/targets/taranis/board.h index 03c33ae7fff..55938380234 100644 --- a/radio/src/targets/taranis/board.h +++ b/radio/src/targets/taranis/board.h @@ -171,24 +171,7 @@ uint32_t isBootloaderStart(const uint8_t * buffer); void init_trainer_module_sbus(); void init_trainer_module_ibus(); void stop_trainer_module_sbus(); -<<<<<<< HEAD int trainerModuleSbusGetByte(uint8_t* byte); -======= -#else - #define init_trainer_module_sbus() - #define init_trainer_module_ibus() - #define stop_trainer_module_sbus() -#endif - -#if defined(INTMODULE_HEARTBEAT_GPIO) -void init_intmodule_heartbeat(); -void stop_intmodule_heartbeat(); -void check_intmodule_heartbeat(); -#else -#define init_intmodule_heartbeat() -#define stop_intmodule_heartbeat() -#define check_intmodule_heartbeat() ->>>>>>> 87eb2196d (- cleaned code) #endif void check_telemetry_exti(); From a4fc8510f7c3e521156b4ca91ffccb79de071f60 Mon Sep 17 00:00:00 2001 From: wimalopaan Date: Tue, 22 Mar 2022 19:26:04 +0100 Subject: [PATCH 12/31] * removed IBUS trainer via ext module bay * let only one of the SBUS/IBUS chooseable (only one can fill the trainer inputs) --- radio/src/dataconstants.h | 1 - radio/src/gui/gui_common.cpp | 2 +- radio/src/sbus.cpp | 60 --------------------------- radio/src/serial.cpp | 9 ++++ radio/src/trainer.cpp | 4 -- radio/src/translations/cn.h.txt | 1 - radio/src/translations/cz.h.txt | 1 - radio/src/translations/de.h.txt | 1 - radio/src/translations/en.h.txt | 1 - radio/src/translations/es.h.txt | 1 - radio/src/translations/fi.h.txt | 1 - radio/src/translations/fr.h.txt | 1 - radio/src/translations/it.h.txt | 1 - radio/src/translations/nl.h.txt | 1 - radio/src/translations/pl.h.txt | 1 - radio/src/translations/pt.h.txt | 1 - radio/src/translations/se.h.txt | 1 - radio/src/translations/untranslated.h | 2 +- 18 files changed, 11 insertions(+), 79 deletions(-) diff --git a/radio/src/dataconstants.h b/radio/src/dataconstants.h index abbb2897f6d..0b18f45302a 100644 --- a/radio/src/dataconstants.h +++ b/radio/src/dataconstants.h @@ -188,7 +188,6 @@ enum TrainerMode { TRAINER_MODE_MASTER_TRAINER_JACK, TRAINER_MODE_SLAVE, TRAINER_MODE_MASTER_SBUS_EXTERNAL_MODULE, - TRAINER_MODE_MASTER_IBUS_EXTERNAL_MODULE, TRAINER_MODE_MASTER_CPPM_EXTERNAL_MODULE, TRAINER_MODE_MASTER_SERIAL, TRAINER_MODE_MASTER_BLUETOOTH, diff --git a/radio/src/gui/gui_common.cpp b/radio/src/gui/gui_common.cpp index 9ea8d0fc15f..3e5146b3ff7 100644 --- a/radio/src/gui/gui_common.cpp +++ b/radio/src/gui/gui_common.cpp @@ -857,7 +857,7 @@ bool isTrainerModeAvailable(int mode) if (mode == TRAINER_MODE_MASTER_SERIAL) { #if defined(SBUS_TRAINER) - return hasSerialMode(UART_MODE_SBUS_TRAINER) >= 0; + return (hasSerialMode(UART_MODE_SBUS_TRAINER) >= 0) || (hasSerialMode(UART_MODE_IBUS_TRAINER) >= 0); #else return false; #endif diff --git a/radio/src/sbus.cpp b/radio/src/sbus.cpp index f61b6c70357..cde34d74b30 100644 --- a/radio/src/sbus.cpp +++ b/radio/src/sbus.cpp @@ -65,66 +65,6 @@ void sbusSetGetByte(int (*fct)(uint8_t*)) sbusGetByte = fct; } -// Range for pulses (ppm input) is [-512:+512] -//void processSbusFrame(uint8_t * sbus, int16_t * pulses, uint32_t size) -//{ -// if (size != SBUS_FRAME_SIZE || sbus[0] != SBUS_START_BYTE || -// sbus[SBUS_FRAME_SIZE - 1] != SBUS_END_BYTE) { -// return; // not a valid SBUS frame -// } -// if ((sbus[SBUS_FLAGS_IDX] & (1 << SBUS_FAILSAFE_BIT)) || -// (sbus[SBUS_FLAGS_IDX] & (1 << SBUS_FRAMELOST_BIT))) { -// return; // SBUS invalid frame or failsafe mode -// } - -// sbus++; // skip start byte - -// uint32_t inputbitsavailable = 0; -// uint32_t inputbits = 0; -// for (uint32_t i=0; i>= SBUS_CH_BITS; -// } - -// ppmInputValidityTimer = PPM_IN_VALID_TIMEOUT; -//} - -//void processSbusInput() -//{ -//#if !defined(SIMU) -// uint8_t rxchar; -// uint32_t active = 0; -// static uint8_t SbusIndex = 0; -// static uint16_t SbusTimer; -// static uint8_t SbusFrame[SBUS_FRAME_SIZE]; - -// while (sbusGetByte(&rxchar)) { -// active = 1; -// if (SbusIndex > SBUS_FRAME_SIZE-1) { -// SbusIndex = SBUS_FRAME_SIZE-1; -// } -// SbusFrame[SbusIndex++] = rxchar; -// } -// if (active) { -// SbusTimer = getTmr2MHz(); -// return; -// } -// else { -// if (SbusIndex) { -// if ((uint16_t) (getTmr2MHz() - SbusTimer) > SBUS_FRAME_GAP_DELAY) { -// processSbusFrame(SbusFrame, ppmInput, SbusIndex); -// SbusIndex = 0; -// } -// } -// } -//#endif -//} - namespace SBus { struct Servo { using SBus = Trainer::Protocol::SBus; diff --git a/radio/src/serial.cpp b/radio/src/serial.cpp index 0d4448796cf..540318c37ef 100644 --- a/radio/src/serial.cpp +++ b/radio/src/serial.cpp @@ -240,6 +240,15 @@ static void serialSetupPort(int mode, etx_serial_init& params) params.rx_enable = true; break; + case UART_MODE_IBUS_TRAINER: + params.baudrate = IBUS_BAUDRATE; + params.word_length = ETX_WordLength_8; + params.parity = ETX_Parity_None; + params.stop_bits = ETX_StopBits_One; + params.rx_enable = true; + power_required = true; + break; + #if defined(LUA) case UART_MODE_LUA: params.baudrate = LUA_DEFAULT_BAUDRATE; diff --git a/radio/src/trainer.cpp b/radio/src/trainer.cpp index a1faf287282..632b1f18385 100644 --- a/radio/src/trainer.cpp +++ b/radio/src/trainer.cpp @@ -130,10 +130,6 @@ void checkTrainerSettings() init_trainer_module_sbus(); sbusSetGetByte(trainerModuleSbusGetByte); break; - case TRAINER_MODE_MASTER_IBUS_EXTERNAL_MODULE: - init_trainer_module_ibus(); - sbusSetGetByte(trainerModuleSbusGetByte); - break; #endif } diff --git a/radio/src/translations/cn.h.txt b/radio/src/translations/cn.h.txt index 6e31a41e9eb..74fd6e9957d 100644 --- a/radio/src/translations/cn.h.txt +++ b/radio/src/translations/cn.h.txt @@ -311,7 +311,6 @@ #define TR_VTRAINER_MASTER_JACK "教练主机/教练口\0 " #define TR_VTRAINER_SLAVE_JACK "学生从机/教练口\0 " #define TR_VTRAINER_MASTER_SBUS_MODULE "教练主机/SBUS模块\0" -#define TR_VTRAINER_MASTER_IBUS_MODULE "教练主机/IBUS模块\0" #define TR_VTRAINER_MASTER_CPPM_MODULE "教练从机/CPPM模块\0" #define TR_VTRAINER_MASTER_BATTERY "教练主机/串口\0 " #define TR_VTRAINER_BLUETOOTH TR("Master/BT\0 ""Slave/BT\0 ", "教练主机/蓝牙\0 " "教练从机/蓝牙\0 ") diff --git a/radio/src/translations/cz.h.txt b/radio/src/translations/cz.h.txt index cde711d82b0..6c4e3b295db 100644 --- a/radio/src/translations/cz.h.txt +++ b/radio/src/translations/cz.h.txt @@ -326,7 +326,6 @@ #define TR_VTRAINER_MASTER_JACK "Učitel/Jack\0 " #define TR_VTRAINER_SLAVE_JACK "Žák/Jack\0 " #define TR_VTRAINER_MASTER_SBUS_MODULE "Učitel/SBUS Modul\0" -#define TR_VTRAINER_MASTER_IBUS_MODULE "Učitel/IBUS Modul\0" #define TR_VTRAINER_MASTER_CPPM_MODULE "Učitel/CPPM Modul\0" #define TR_VTRAINER_MASTER_BATTERY "Učitel/Serial\0 " #define TR_VTRAINER_BLUETOOTH TR("Master/BT\0 ""Slave/BT\0 ", "Master/Bluetooth\0 ""Slave/Bluetooth\0 ") diff --git a/radio/src/translations/de.h.txt b/radio/src/translations/de.h.txt index 4f9df6a0c86..799aac2eeec 100644 --- a/radio/src/translations/de.h.txt +++ b/radio/src/translations/de.h.txt @@ -329,7 +329,6 @@ #define TR_VTRAINER_MASTER_JACK "Lehrer/Buchse\0 " #define TR_VTRAINER_SLAVE_JACK "Schüler/Buchse\0 " #define TR_VTRAINER_MASTER_SBUS_MODULE "Lehrer/SBUS Modul\0" -#define TR_VTRAINER_MASTER_IBUS_MODULE "Lehrer/IBUS Modul\0" #define TR_VTRAINER_MASTER_CPPM_MODULE "Lehrer/CPPM Modul\0" #define TR_VTRAINER_MASTER_BATTERY "Lehrer/Serial\0 " #define TR_VTRAINER_BLUETOOTH TR("Lehrer/BT\0 ""Schüler/BT\0 ", "Lehrer/Bluetooth\0 ""Schüler/Bluetooth\0") diff --git a/radio/src/translations/en.h.txt b/radio/src/translations/en.h.txt index b091b451cc4..f3f74095bfd 100644 --- a/radio/src/translations/en.h.txt +++ b/radio/src/translations/en.h.txt @@ -329,7 +329,6 @@ #define TR_VTRAINER_MASTER_JACK "Master/Jack\0 " #define TR_VTRAINER_SLAVE_JACK "Slave/Jack\0 " #define TR_VTRAINER_MASTER_SBUS_MODULE "Master/SBUS Module" -#define TR_VTRAINER_MASTER_IBUS_MODULE "Master/IBUS Module" #define TR_VTRAINER_MASTER_CPPM_MODULE "Master/CPPM Module" #define TR_VTRAINER_MASTER_BATTERY "Master/Serial\0 " #define TR_VTRAINER_BLUETOOTH TR("Master/BT\0 ""Slave/BT\0 ", "Master/Bluetooth\0 ""Slave/Bluetooth\0 ") diff --git a/radio/src/translations/es.h.txt b/radio/src/translations/es.h.txt index 08e7b640f43..a4e94daae63 100644 --- a/radio/src/translations/es.h.txt +++ b/radio/src/translations/es.h.txt @@ -326,7 +326,6 @@ #define TR_VTRAINER_MASTER_JACK "Master/Jack\0 " #define TR_VTRAINER_SLAVE_JACK "Esclav/Jack\0 " #define TR_VTRAINER_MASTER_SBUS_MODULE "Master/Módulo SBUS" -#define TR_VTRAINER_MASTER_IBUS_MODULE "Master/Módulo IBUS" #define TR_VTRAINER_MASTER_CPPM_MODULE "Master/Módulo CPPM" #define TR_VTRAINER_MASTER_BATTERY "Master/Serie\0 " #define TR_VTRAINER_BLUETOOTH TR("Master/BT\0 ""Esclavo/BT\0 ", "Master/Bluetooth\0 ""Esclavo/Bluetooth\0") diff --git a/radio/src/translations/fi.h.txt b/radio/src/translations/fi.h.txt index f9b36dbe009..0885ed24e9f 100644 --- a/radio/src/translations/fi.h.txt +++ b/radio/src/translations/fi.h.txt @@ -347,7 +347,6 @@ #define TR_VTRAINER_MASTER_JACK "Master/Jack\0 " #define TR_VTRAINER_SLAVE_JACK "Slave/Jack\0 " #define TR_VTRAINER_MASTER_SBUS_MODULE "Master/SBUS Module" -#define TR_VTRAINER_MASTER_IBUS_MODULE "Master/IBUS Module" #define TR_VTRAINER_MASTER_CPPM_MODULE "Master/CPPM Module" #define TR_VTRAINER_MASTER_BATTERY "Master/Serial\0 " #define TR_VTRAINER_BLUETOOTH TR("Master/BT\0 ""Slave/BT\0 ", "Master/Bluetooth\0 ""Slave/Bluetooth\0 ") diff --git a/radio/src/translations/fr.h.txt b/radio/src/translations/fr.h.txt index b2bb23afeb2..b77f8e141cc 100644 --- a/radio/src/translations/fr.h.txt +++ b/radio/src/translations/fr.h.txt @@ -349,7 +349,6 @@ #define TR_VTRAINER_MASTER_JACK "Maître/Jack\0 " #define TR_VTRAINER_SLAVE_JACK "Elève/Jack\0 " #define TR_VTRAINER_MASTER_SBUS_MODULE "Maître/SBUS Module" -#define TR_VTRAINER_MASTER_IBUS_MODULE "Maître/IBUS Module" #define TR_VTRAINER_MASTER_CPPM_MODULE "Maître/CPPM Module" #define TR_VTRAINER_MASTER_BATTERY "Maître/Série\0 " #define TR_VTRAINER_BLUETOOTH TR("Maître/BT\0 ""Elève/BT\0 ", "Maître/Bluetooth\0 ""Elève/Bluetooth\0 ") diff --git a/radio/src/translations/it.h.txt b/radio/src/translations/it.h.txt index 11f09b52f42..c56a1ffb610 100644 --- a/radio/src/translations/it.h.txt +++ b/radio/src/translations/it.h.txt @@ -354,7 +354,6 @@ #define TR_VTRAINER_MASTER_JACK "Maestro/Jack\0 " #define TR_VTRAINER_SLAVE_JACK "Allievo/Jack\0 " #define TR_VTRAINER_MASTER_SBUS_MODULE "Master/Modulo SBUS" -#define TR_VTRAINER_MASTER_IBUS_MODULE "Master/Modulo IBUS" #define TR_VTRAINER_MASTER_CPPM_MODULE "Master/Modulo CPPM" #define TR_VTRAINER_MASTER_BATTERY "Master/Seriale\0 " #define TR_VTRAINER_BLUETOOTH TR("Master/BT\0 ""Slave/BT\0 ", "Master/Bluetooth\0 ""Slave/Bluetooth\0 ") diff --git a/radio/src/translations/nl.h.txt b/radio/src/translations/nl.h.txt index 1cdcce81c14..0af125a4b82 100644 --- a/radio/src/translations/nl.h.txt +++ b/radio/src/translations/nl.h.txt @@ -328,7 +328,6 @@ #define TR_VTRAINER_MASTER_JACK "Master/Jack\0 " #define TR_VTRAINER_SLAVE_JACK "Slave/Jack\0 " #define TR_VTRAINER_MASTER_SBUS_MODULE "Master/SBUS Module" -#define TR_VTRAINER_MASTER_IBUS_MODULE "Master/IBUS Module" #define TR_VTRAINER_MASTER_CPPM_MODULE "Master/CPPM Module" #define TR_VTRAINER_MASTER_BATTERY "Master/Serial\0 " #define TR_VTRAINER_BLUETOOTH TR("Master/BT\0 ""Slave/BT\0 ", "Master/Bluetooth\0 ""Slave/Bluetooth\0 ") diff --git a/radio/src/translations/pl.h.txt b/radio/src/translations/pl.h.txt index afd63aff9a3..ef1cd10d70e 100644 --- a/radio/src/translations/pl.h.txt +++ b/radio/src/translations/pl.h.txt @@ -347,7 +347,6 @@ #define TR_VTRAINER_MASTER_JACK "Trener/Jack\0 " #define TR_VTRAINER_SLAVE_JACK "Uczeń/Jack\0 " #define TR_VTRAINER_MASTER_SBUS_MODULE "Trener/SBUS Moduł " -#define TR_VTRAINER_MASTER_IBUS_MODULE "Trener/IBUS Moduł " #define TR_VTRAINER_MASTER_CPPM_MODULE "Trener/CPPM Moduł " #define TR_VTRAINER_MASTER_BATTERY "Trener/Serial\0 " #define TR_VTRAINER_BLUETOOTH TR("Master/BT\0 ""Slave/BT\0", "Master/Bluetooth\0 ""Slave/Bluetooth\0 ") diff --git a/radio/src/translations/pt.h.txt b/radio/src/translations/pt.h.txt index 64d2cb7de0d..0ecb86e6c5b 100644 --- a/radio/src/translations/pt.h.txt +++ b/radio/src/translations/pt.h.txt @@ -347,7 +347,6 @@ #define TR_VTRAINER_MASTER_JACK "Master/Jack\0 " #define TR_VTRAINER_SLAVE_JACK "Slave/Jack\0 " #define TR_VTRAINER_MASTER_SBUS_MODULE "Master/SBUS Module" -#define TR_VTRAINER_MASTER_IBUS_MODULE "Master/IBUS Module" #define TR_VTRAINER_MASTER_CPPM_MODULE "Master/CPPM Module" #define TR_VTRAINER_MASTER_BATTERY "Master/Serial\0 " #define TR_VTRAINER_BLUETOOTH TR("Master/BT\0 ""Slave/BT\0", "Master/Bluetooth\0 ""Slave/Bluetooth\0 ") diff --git a/radio/src/translations/se.h.txt b/radio/src/translations/se.h.txt index a89161d9e4b..58fb790a56d 100644 --- a/radio/src/translations/se.h.txt +++ b/radio/src/translations/se.h.txt @@ -350,7 +350,6 @@ #define TR_VTRAINER_MASTER_JACK "Lärare/Uttag\0 " #define TR_VTRAINER_SLAVE_JACK "Elev./Uttag\0 " #define TR_VTRAINER_MASTER_SBUS_MODULE "Lärare/SBUS-Modul\0" -#define TR_VTRAINER_MASTER_IBUS_MODULE "Lärare/IBUS-Modul\0" #define TR_VTRAINER_MASTER_CPPM_MODULE "Lärare/CPPM-Modul\0" #define TR_VTRAINER_MASTER_BATTERY "Lärare/Serial\0 " #define TR_VTRAINER_BLUETOOTH TR("Master/BT\0 ""Slave/BT\0", "Master/Bluetooth\0 ""Slave/Bluetooth\0 ") diff --git a/radio/src/translations/untranslated.h b/radio/src/translations/untranslated.h index bb90bc0f8b9..dd6aad31f6a 100644 --- a/radio/src/translations/untranslated.h +++ b/radio/src/translations/untranslated.h @@ -84,7 +84,7 @@ #define TR_VTRAINERMODES \ TR_VTRAINER_MASTER_OFF TR_VTRAINER_MASTER_JACK TR_VTRAINER_SLAVE_JACK \ - TR_VTRAINER_MASTER_SBUS_MODULE TR_VTRAINER_MASTER_IBUS_MODULE TR_VTRAINER_MASTER_CPPM_MODULE \ + TR_VTRAINER_MASTER_SBUS_MODULE TR_VTRAINER_MASTER_CPPM_MODULE \ TR_VTRAINER_MASTER_BATTERY TR_VTRAINER_BLUETOOTH TR_VTRAINER_MULTI #if defined(PCBHORUS) || defined(PCBNV14) From a9c6615a81304503ca3b01daa4e5672d6dba700e Mon Sep 17 00:00:00 2001 From: wimalopaan Date: Wed, 23 Mar 2022 19:11:39 +0100 Subject: [PATCH 13/31] after serial refactor get it working again --- radio/src/gui/gui_common.cpp | 2 +- radio/src/serial.cpp | 3 +++ radio/src/storage/yaml/yaml_datastructs_funcs.cpp | 1 + 3 files changed, 5 insertions(+), 1 deletion(-) diff --git a/radio/src/gui/gui_common.cpp b/radio/src/gui/gui_common.cpp index 3e5146b3ff7..26ff1210591 100644 --- a/radio/src/gui/gui_common.cpp +++ b/radio/src/gui/gui_common.cpp @@ -428,7 +428,7 @@ bool isSerialModeAvailable(const uint8_t port_nr, const int mode) #if defined(USB_SERIAL) // Telemetry input & SBUS trainer on VCP is not yet supported if (port_nr == SP_VCP && - (mode == UART_MODE_TELEMETRY || mode == UART_MODE_SBUS_TRAINER)) + (mode == UART_MODE_TELEMETRY || mode == UART_MODE_SBUS_TRAINER || mode == UART_MODE_IBUS_TRAINER)) return false; #endif diff --git a/radio/src/serial.cpp b/radio/src/serial.cpp index 540318c37ef..bc80db6cce1 100644 --- a/radio/src/serial.cpp +++ b/radio/src/serial.cpp @@ -174,6 +174,9 @@ static void serialSetCallBacks(int mode, void* ctx, const etx_serial_port_t* por sbusSetAuxGetByte(ctx, getByte); // TODO: setRxCb (see MODE_LUA) break; + case UART_MODE_IBUS_TRAINER: + sbusSetAuxGetByte(ctx, getByte); + break; #endif case UART_MODE_TELEMETRY: diff --git a/radio/src/storage/yaml/yaml_datastructs_funcs.cpp b/radio/src/storage/yaml/yaml_datastructs_funcs.cpp index 88fe64468ac..b7086c6cf42 100644 --- a/radio/src/storage/yaml/yaml_datastructs_funcs.cpp +++ b/radio/src/storage/yaml/yaml_datastructs_funcs.cpp @@ -1826,6 +1826,7 @@ static const struct YamlIdStr enum_UartModes[] = { { UART_MODE_TELEMETRY_MIRROR, "TELEMETRY_MIRROR" }, { UART_MODE_TELEMETRY, "TELEMETRY_IN" }, { UART_MODE_SBUS_TRAINER, "SBUS_TRAINER" }, + { UART_MODE_IBUS_TRAINER, "IBUS_TRAINER" }, { UART_MODE_LUA, "LUA" }, { UART_MODE_CLI, "CLI" }, { UART_MODE_GPS, "GPS" }, From fce5bd37843d57f66f2c7347acf6029e25b120ce Mon Sep 17 00:00:00 2001 From: wimalopaan Date: Fri, 25 Mar 2022 16:06:28 +0100 Subject: [PATCH 14/31] added CRSF trainer input SUMD is still only stub --- radio/src/CMakeLists.txt | 2 + radio/src/crc.cpp | 11 ++ radio/src/crc.h | 8 + radio/src/crsf.cpp | 150 ++++++++++++++++++ radio/src/crsf.h | 10 ++ radio/src/dataconstants.h | 2 + radio/src/gui/gui_common.cpp | 7 +- radio/src/opentx.h | 2 + radio/src/serial.cpp | 23 ++- .../storage/yaml/yaml_datastructs_funcs.cpp | 2 + radio/src/sumd.cpp | 1 + radio/src/sumd.h | 6 + radio/src/tasks.cpp | 22 +-- radio/src/telemetry/crossfire.h | 1 + radio/src/trainer.h | 21 ++- radio/src/translations/cn.h.txt | 2 +- radio/src/translations/cz.h.txt | 2 +- radio/src/translations/de.h.txt | 2 +- radio/src/translations/en.h.txt | 2 +- radio/src/translations/es.h.txt | 2 +- radio/src/translations/fi.h.txt | 2 +- radio/src/translations/fr.h.txt | 2 +- radio/src/translations/it.h.txt | 2 +- radio/src/translations/nl.h.txt | 2 +- radio/src/translations/pl.h.txt | 2 +- radio/src/translations/pt.h.txt | 2 +- radio/src/translations/se.h.txt | 2 +- 27 files changed, 258 insertions(+), 34 deletions(-) create mode 100644 radio/src/crsf.cpp create mode 100644 radio/src/crsf.h create mode 100644 radio/src/sumd.cpp create mode 100644 radio/src/sumd.h diff --git a/radio/src/CMakeLists.txt b/radio/src/CMakeLists.txt index 21f9b42527a..6708875328b 100644 --- a/radio/src/CMakeLists.txt +++ b/radio/src/CMakeLists.txt @@ -477,6 +477,8 @@ set(SRC serial.cpp sbus.cpp ibus.cpp + crsf.cpp + sumd.cpp ) if(GUI) diff --git a/radio/src/crc.cpp b/radio/src/crc.cpp index 485851b2d43..1b62c2a4bfa 100644 --- a/radio/src/crc.cpp +++ b/radio/src/crc.cpp @@ -81,6 +81,17 @@ uint8_t crc8(const uint8_t * ptr, uint32_t len) return crc; } +void CRC8::reset() { + mSum = 0; +} +CRC8& CRC8::operator+=(const uint8_t b) { + mSum = crc8tab[mSum ^ b]; + return *this; +} +CRC8::operator uint8_t() const { + return mSum; +} + // CRC8 implementation with polynom = 0xBA const unsigned char crc8tab_BA[256] = { 0x00, 0xBA, 0xCE, 0x74, 0x26, 0x9C, 0xE8, 0x52, diff --git a/radio/src/crc.h b/radio/src/crc.h index f79a56ece6a..0ea85ec163b 100644 --- a/radio/src/crc.h +++ b/radio/src/crc.h @@ -104,4 +104,12 @@ static const unsigned short crc16tab_1189[256] = { 0x7bc7,0x6a4e,0x58d5,0x495c,0x3de3,0x2c6a,0x1ef1,0x0f78 }; +struct CRC8 final { + void reset(); + CRC8& operator+=(const uint8_t b); + operator uint8_t() const; +private: + uint8_t mSum = 0; +}; + #endif diff --git a/radio/src/crsf.cpp b/radio/src/crsf.cpp new file mode 100644 index 00000000000..577ca5f046f --- /dev/null +++ b/radio/src/crsf.cpp @@ -0,0 +1,150 @@ +#include "crsf.h" +#include "opentx.h" +#include "trainer.h" + +#include +#include + +#ifdef __GNUC__ +# pragma GCC diagnostic push +# pragma GCC diagnostic error "-Wswitch" // unfortunately the project uses -Wnoswitch +#endif + +namespace CRSF { + struct Servo { + using Crsf = Trainer::Protocol::Crsf; + using MesgType = Crsf::MesgType; + + static constexpr uint8_t mPauseCount{2}; // 2ms + + enum class State : uint8_t {Undefined, GotFCAddress, GotLength, Channels, Data, AwaitCRC, AwaitCRCAndDecode}; + + static inline int16_t convertCrsfToPuls(uint16_t const value) { + const int16_t centered = value - Crsf::CenterValue; + return Trainer::clamp((centered * 5) / 8); + } + + static inline void tick1ms() { + if (mPauseCounter > 0) { + --mPauseCounter; + } + else { + mState = State::Undefined; + } + } + + static inline void process(const uint8_t b, const std::function f) { + mPauseCounter = mPauseCount; + switch(mState) { // enum-switch -> no default (intentional) + case State::Undefined: + csum.reset(); + if (b == Crsf::FcAddress) { + mState = State::GotFCAddress; + } + break; + case State::GotFCAddress: + if ((b > 2) && (b <= mData.size())) { + mLength = b - 2; // only payload (not including type and crc) + mIndex = 0; + mState = State::GotLength; + } + else { + mState = State::Undefined; + } + break; + case State::GotLength: + csum += b; + if ((b == Crsf::FrametypeChannels) && (mLength == 22)) { + mState = State::Channels; + } + else { + mState = State::Data; + } + break; + case State::Data: + csum += b; + if (++mIndex == mLength) { + mState = State::AwaitCRC; + } + break; + case State::Channels: + csum += b; + break; + case State::AwaitCRC: + if (csum == b) { + mState = State::Undefined; + } + else { + mState = State::Undefined; + } + break; + case State::AwaitCRCAndDecode: + if (csum == b) { + ++mPackages; + f(); + mState = State::Undefined; + } + else { + mState = State::Undefined; + } + break; + } + } + static inline void convert(int16_t* const pulses) { + static_assert(MAX_TRAINER_CHANNELS == 16); + pulses[0] = (uint16_t) (((mData[0] | mData[1] << 8)) & Crsf::ValueMask); + pulses[1] = (uint16_t) ((mData[1]>>3 | mData[2] <<5) & Crsf::ValueMask); + pulses[2] = (uint16_t) ((mData[2]>>6 | mData[3] <<2 | mData[4]<<10) & Crsf::ValueMask); + pulses[3] = (uint16_t) ((mData[4]>>1 | mData[5] <<7) & Crsf::ValueMask); + pulses[4] = (uint16_t) ((mData[5]>>4 | mData[6] <<4) & Crsf::ValueMask); + pulses[5] = (uint16_t) ((mData[6]>>7 | mData[7] <<1 | mData[8]<<9) & Crsf::ValueMask); + pulses[6] = (uint16_t) ((mData[8]>>2 | mData[9] <<6) & Crsf::ValueMask); + pulses[7] = (uint16_t) ((mData[9]>>5 | mData[10]<<3) & Crsf::ValueMask); + pulses[8] = (uint16_t) ((mData[11] | mData[12]<<8) & Crsf::ValueMask); + pulses[9] = (uint16_t) ((mData[12]>>3 | mData[13]<<5) & Crsf::ValueMask); + pulses[10] = (uint16_t) ((mData[13]>>6 | mData[14]<<2 | mData[15]<<10) & Crsf::ValueMask); + pulses[11] = (uint16_t) ((mData[15]>>1 | mData[16]<<7) & Crsf::ValueMask); + pulses[12] = (uint16_t) ((mData[16]>>4 | mData[17]<<4) & Crsf::ValueMask); + pulses[13] = (uint16_t) ((mData[17]>>7 | mData[18]<<1 | mData[19]<<9) & Crsf::ValueMask); + pulses[14] = (uint16_t) ((mData[19]>>2 | mData[20]<<6) & Crsf::ValueMask); + pulses[15] = (uint16_t) ((mData[20]>>5 | mData[21]<<3) & Crsf::ValueMask); + + for(size_t i = 0; i < MAX_TRAINER_CHANNELS; ++i) { + pulses[i] = convertCrsfToPuls(pulses[i]); + } + } + private: + static CRC8 csum; + static State mState; + static MesgType mData; + static uint8_t mIndex; + static uint8_t mLength; + static uint16_t mPackages; + static uint8_t mPauseCounter; + }; + + CRC8 Servo::csum; + Servo::State Servo::mState{Servo::State::Undefined}; + Servo::MesgType Servo::mData; + uint8_t Servo::mIndex{0}; + uint8_t Servo::mLength{0}; + uint16_t Servo::mPackages{0}; + uint8_t Servo::mPauseCounter{Servo::mPauseCount}; // 2 ms +} + +void processCrsfInput() { +#if !defined(SIMU) + uint8_t rxchar; + + while (sbusAuxGetByte(&rxchar)) { + CRSF::Servo::process(rxchar, [&](){ + CRSF::Servo::convert(ppmInput); + ppmInputValidityTimer = PPM_IN_VALID_TIMEOUT; + }); + } +#endif +} + +#ifdef __GNUC__ +# pragma GCC diagnostic pop +#endif diff --git a/radio/src/crsf.h b/radio/src/crsf.h new file mode 100644 index 00000000000..042ee61be62 --- /dev/null +++ b/radio/src/crsf.h @@ -0,0 +1,10 @@ +#pragma once + +#include "dataconstants.h" +#include "crc.h" +#include "telemetry/crossfire.h" + +#define CRSF_BAUDRATE 400000 + +void processCrsfInput(); + diff --git a/radio/src/dataconstants.h b/radio/src/dataconstants.h index 0b18f45302a..fdbceeff874 100644 --- a/radio/src/dataconstants.h +++ b/radio/src/dataconstants.h @@ -231,6 +231,8 @@ enum UartModes { UART_MODE_TELEMETRY, UART_MODE_SBUS_TRAINER, UART_MODE_IBUS_TRAINER, + UART_MODE_CRSF_TRAINER, + UART_MODE_SUMD_TRAINER, UART_MODE_LUA, UART_MODE_CLI, UART_MODE_GPS, diff --git a/radio/src/gui/gui_common.cpp b/radio/src/gui/gui_common.cpp index 26ff1210591..9000d809bf1 100644 --- a/radio/src/gui/gui_common.cpp +++ b/radio/src/gui/gui_common.cpp @@ -382,7 +382,7 @@ int hasSerialMode(int mode) bool isTrainerInputMode(const int mode) { - return (mode == UART_MODE_IBUS_TRAINER) || (mode == UART_MODE_SBUS_TRAINER); + return (mode == UART_MODE_IBUS_TRAINER) || (mode == UART_MODE_SBUS_TRAINER) || (mode == UART_MODE_CRSF_TRAINER) || (mode == UART_MODE_SUMD_TRAINER); } int trainerInputActive() @@ -428,7 +428,7 @@ bool isSerialModeAvailable(const uint8_t port_nr, const int mode) #if defined(USB_SERIAL) // Telemetry input & SBUS trainer on VCP is not yet supported if (port_nr == SP_VCP && - (mode == UART_MODE_TELEMETRY || mode == UART_MODE_SBUS_TRAINER || mode == UART_MODE_IBUS_TRAINER)) + (mode == UART_MODE_TELEMETRY || mode == UART_MODE_SBUS_TRAINER || mode == UART_MODE_IBUS_TRAINER || mode == UART_MODE_CRSF_TRAINER || mode == UART_MODE_SUMD_TRAINER)) return false; #endif @@ -857,7 +857,8 @@ bool isTrainerModeAvailable(int mode) if (mode == TRAINER_MODE_MASTER_SERIAL) { #if defined(SBUS_TRAINER) - return (hasSerialMode(UART_MODE_SBUS_TRAINER) >= 0) || (hasSerialMode(UART_MODE_IBUS_TRAINER) >= 0); + return (hasSerialMode(UART_MODE_SBUS_TRAINER) >= 0) || (hasSerialMode(UART_MODE_IBUS_TRAINER) >= 0) + || (hasSerialMode(UART_MODE_CRSF_TRAINER) >= 0) || (hasSerialMode(UART_MODE_SUMD_TRAINER) >= 0); #else return false; #endif diff --git a/radio/src/opentx.h b/radio/src/opentx.h index 741f79e8c2a..11f841a0451 100644 --- a/radio/src/opentx.h +++ b/radio/src/opentx.h @@ -610,6 +610,8 @@ static inline void GET_ADC_IF_MIXER_NOT_RUNNING() #include "sbus.h" #include "ibus.h" +#include "crsf.h" +#include "sumd.h" void resetBacklightTimeout(); void checkBacklight(); diff --git a/radio/src/serial.cpp b/radio/src/serial.cpp index bc80db6cce1..ad0a7568938 100644 --- a/radio/src/serial.cpp +++ b/radio/src/serial.cpp @@ -171,11 +171,12 @@ static void serialSetCallBacks(int mode, void* ctx, const etx_serial_port_t* por #if defined(SBUS_TRAINER) case UART_MODE_SBUS_TRAINER: + case UART_MODE_IBUS_TRAINER: + case UART_MODE_CRSF_TRAINER: + case UART_MODE_SUMD_TRAINER: sbusSetAuxGetByte(ctx, getByte); // TODO: setRxCb (see MODE_LUA) break; - case UART_MODE_IBUS_TRAINER: - sbusSetAuxGetByte(ctx, getByte); break; #endif @@ -251,7 +252,25 @@ static void serialSetupPort(int mode, etx_serial_init& params) params.rx_enable = true; power_required = true; break; + + case UART_MODE_CRSF_TRAINER: + params.baudrate = CRSF_BAUDRATE; + params.word_length = ETX_WordLength_8; + params.parity = ETX_Parity_None; + params.stop_bits = ETX_StopBits_One; + params.rx_enable = true; + power_required = true; + break; + case UART_MODE_SUMD_TRAINER: + params.baudrate = SUMD_BAUDRATE; + params.word_length = ETX_WordLength_8; + params.parity = ETX_Parity_None; + params.stop_bits = ETX_StopBits_One; + params.rx_enable = true; + power_required = true; + break; + #if defined(LUA) case UART_MODE_LUA: params.baudrate = LUA_DEFAULT_BAUDRATE; diff --git a/radio/src/storage/yaml/yaml_datastructs_funcs.cpp b/radio/src/storage/yaml/yaml_datastructs_funcs.cpp index b7086c6cf42..fffaa1fb3ad 100644 --- a/radio/src/storage/yaml/yaml_datastructs_funcs.cpp +++ b/radio/src/storage/yaml/yaml_datastructs_funcs.cpp @@ -1827,6 +1827,8 @@ static const struct YamlIdStr enum_UartModes[] = { { UART_MODE_TELEMETRY, "TELEMETRY_IN" }, { UART_MODE_SBUS_TRAINER, "SBUS_TRAINER" }, { UART_MODE_IBUS_TRAINER, "IBUS_TRAINER" }, + { UART_MODE_CRSF_TRAINER, "CRSF_TRAINER" }, + { UART_MODE_SUMD_TRAINER, "SUMD_TRAINER" }, { UART_MODE_LUA, "LUA" }, { UART_MODE_CLI, "CLI" }, { UART_MODE_GPS, "GPS" }, diff --git a/radio/src/sumd.cpp b/radio/src/sumd.cpp new file mode 100644 index 00000000000..f01beb07ae0 --- /dev/null +++ b/radio/src/sumd.cpp @@ -0,0 +1 @@ +#include "sumd.h" diff --git a/radio/src/sumd.h b/radio/src/sumd.h new file mode 100644 index 00000000000..4a17db6948c --- /dev/null +++ b/radio/src/sumd.h @@ -0,0 +1,6 @@ +#pragma once + +#define SUMD_BAUDRATE 115200 + +void processSumdInput(); + diff --git a/radio/src/tasks.cpp b/radio/src/tasks.cpp index 33cc2fc5412..f35e7ff2927 100644 --- a/radio/src/tasks.cpp +++ b/radio/src/tasks.cpp @@ -96,23 +96,13 @@ void execMixerFrequentActions() else if (hasSerialMode(UART_MODE_IBUS_TRAINER) >= 0) { processIbusInput(); } + else if (hasSerialMode(UART_MODE_CRSF_TRAINER) >= 0) { + processCrsfInput(); + } + else if (hasSerialMode(UART_MODE_SUMD_TRAINER) >= 0) { +// processSumdInput(); + } #endif -//#if defined(SBUS_TRAINER) && (defined(AUX_SERIAL) || defined(AUX2_SERIAL)) -// if ((g_eeGeneral.auxSerialMode == UART_MODE_SBUS_TRAINER) -// #if defined(AUX2_SERIAL) -// || (g_eeGeneral.aux2SerialMode == UART_MODE_SBUS_TRAINER) -// #endif -// ) { -// processSbusInput(); -// } -// else if ((g_eeGeneral.auxSerialMode == UART_MODE_IBUS_TRAINER) -// #if defined(AUX2_SERIAL) -// || (g_eeGeneral.aux2SerialMode == UART_MODE_IBUS_TRAINER) -// #endif -// ) { -// processIbusInput(); -// } -//#endif #if defined(GYRO) gyro.wakeup(); diff --git a/radio/src/telemetry/crossfire.h b/radio/src/telemetry/crossfire.h index 9afd6e26463..5253f3443d7 100644 --- a/radio/src/telemetry/crossfire.h +++ b/radio/src/telemetry/crossfire.h @@ -27,6 +27,7 @@ // Device address #define BROADCAST_ADDRESS 0x00 +#define FC_ADDRESS 0xC8 #define RADIO_ADDRESS 0xEA #define MODULE_ADDRESS 0xEE diff --git a/radio/src/trainer.h b/radio/src/trainer.h index d1b6648b841..845d9f2e2de 100644 --- a/radio/src/trainer.h +++ b/radio/src/trainer.h @@ -56,7 +56,26 @@ namespace Trainer { static constexpr uint16_t MaxValue = 988; static constexpr uint16_t MinValue = 2011; static constexpr uint16_t CenterValue = (MaxValue + MinValue) / 2; - }; + }; + struct Crsf { +// Every frame has the structure: +// + using MesgType = std::array; + + static constexpr uint8_t ValueBits = 11; + static constexpr uint16_t ValueMask = ((1 << ValueBits) - 1); + + static constexpr uint8_t FcAddress = FC_ADDRESS; + static constexpr uint8_t FrametypeChannels = CHANNELS_ID; + static constexpr uint8_t FrametypeLink = LINK_ID; + + static constexpr uint16_t CenterValue = 0x3e0; + + }; + struct SumDV3 { + using MesgType = std::array; + + }; } } diff --git a/radio/src/translations/cn.h.txt b/radio/src/translations/cn.h.txt index 74fd6e9957d..1a26226bc14 100644 --- a/radio/src/translations/cn.h.txt +++ b/radio/src/translations/cn.h.txt @@ -63,7 +63,7 @@ #define TR_TRNCHN "CH1CH2CH3CH4" #define LEN_AUX_SERIAL_MODES "\010" -#define TR_AUX_SERIAL_MODES "调试\0 " "回传镜像" "回传输入" "SBUS教练" "IBUS教练" "LUA脚本\0""CLI\0 ""GPS\0 ""Debug\0 " +#define TR_AUX_SERIAL_MODES "调试\0 " "回传镜像" "回传输入" "SBUS教练" "IBUS教练""CRSF教练""SUMD教练" "LUA脚本\0""CLI\0 ""GPS\0 ""Debug\0 " #define LEN_SWTYPES "\004" #define TR_SWTYPES "无\0 " "回弹" "2段\0""3段\0" diff --git a/radio/src/translations/cz.h.txt b/radio/src/translations/cz.h.txt index 6c4e3b295db..301cbc4cf01 100644 --- a/radio/src/translations/cz.h.txt +++ b/radio/src/translations/cz.h.txt @@ -61,7 +61,7 @@ #define TR_TRNCHN "CH1CH2CH3CH4" #define LEN_AUX_SERIAL_MODES "\015" -#define TR_AUX_SERIAL_MODES "VYP\0 ""Telem Mirror\0""Telemetry In\0""SBUS Trenér\0 ""IBUS Trenér\0 ""LUA\0 ""CLI\0 ""GPS\0 ""Debug\0 " +#define TR_AUX_SERIAL_MODES "VYP\0 ""Telem Mirror\0""Telemetry In\0""SBUS Trenér\0 ""IBUS Trenér\0 ""CRSF Trenér\0 ""SUMD Trenér\0 ""LUA\0 ""CLI\0 ""GPS\0 ""Debug\0 " #define LEN_SWTYPES "\013" #define TR_SWTYPES "Žádný\0 ""Bez aretace""2-polohový\0""3-polohový\0" diff --git a/radio/src/translations/de.h.txt b/radio/src/translations/de.h.txt index 799aac2eeec..d850c37957f 100644 --- a/radio/src/translations/de.h.txt +++ b/radio/src/translations/de.h.txt @@ -62,7 +62,7 @@ #define TR_TRNCHN "CH1CH2CH3CH4" #define LEN_AUX_SERIAL_MODES "\015" -#define TR_AUX_SERIAL_MODES "AUS\0 ""Telem Mirror\0""Telemetry In\0""SBUS Eingang\0""IBUS Eingang\0""LUA\0 ""CLI\0 ""GPS\0 ""Debug\0 " +#define TR_AUX_SERIAL_MODES "AUS\0 ""Telem Mirror\0""Telemetry In\0""SBUS Eingang\0""IBUS Eingang\0""CRSF Eingang\0""SUMD Eingang\0""LUA\0 ""CLI\0 ""GPS\0 ""Debug\0 " #define LEN_SWTYPES "\006" #define TR_SWTYPES "Kein\0 ""Taster""2POS\0 ""3POS\0" diff --git a/radio/src/translations/en.h.txt b/radio/src/translations/en.h.txt index f3f74095bfd..81d9ba181cc 100644 --- a/radio/src/translations/en.h.txt +++ b/radio/src/translations/en.h.txt @@ -62,7 +62,7 @@ #define TR_TRNCHN "CH1CH2CH3CH4" #define LEN_AUX_SERIAL_MODES "\015" -#define TR_AUX_SERIAL_MODES "OFF\0 ""Telem Mirror\0""Telemetry In\0""SBUS Trainer\0""IBUS Trainer\0""LUA\0 ""CLI\0 ""GPS\0 ""Debug\0 " +#define TR_AUX_SERIAL_MODES "OFF\0 ""Telem Mirror\0""Telemetry In\0""SBUS Trainer\0""IBUS Trainer\0""CRSF Trainer\0""SUMD Trainer\0""LUA\0 ""CLI\0 ""GPS\0 ""Debug\0 " #define LEN_SWTYPES "\006" #define TR_SWTYPES "None\0 ""Toggle""2POS\0 ""3POS\0" diff --git a/radio/src/translations/es.h.txt b/radio/src/translations/es.h.txt index a4e94daae63..84677e2ece4 100644 --- a/radio/src/translations/es.h.txt +++ b/radio/src/translations/es.h.txt @@ -62,7 +62,7 @@ #define TR_TRNCHN "CH1CH2CH3CH4" #define LEN_AUX_SERIAL_MODES "\015" -#define TR_AUX_SERIAL_MODES "OFF\0 ""Telem Mirror\0""Telemetría\0 ""Entrenador SBUS""Entrenador IBUS""LUA\0 ""CLI\0 ""GPS\0 ""Debug\0 " +#define TR_AUX_SERIAL_MODES "OFF\0 ""Telem Mirror\0""Telemetría\0 ""Entrenador SBUS""Entrenador IBUS""Entrenador CRSF""Entrenador SUMD""LUA\0 ""CLI\0 ""GPS\0 ""Debug\0 " #define LEN_SWTYPES "\007" #define TR_SWTYPES "Nada\0 ""Palanca""2POS\0 ""3POS\0 " diff --git a/radio/src/translations/fi.h.txt b/radio/src/translations/fi.h.txt index 0885ed24e9f..5ca6e295d1c 100644 --- a/radio/src/translations/fi.h.txt +++ b/radio/src/translations/fi.h.txt @@ -62,7 +62,7 @@ #define TR_TRNCHN "CH1CH2CH3CH4" #define LEN_AUX_SERIAL_MODES "\015" -#define TR_AUX_SERIAL_MODES "POIS\0 ""S-Port Pelik\0""Telemetry In\0""SBUS Trainer\0""IBUS Trainer\0""LUA\0 ""CLI\0 ""GPS\0 ""Debug\0 " +#define TR_AUX_SERIAL_MODES "POIS\0 ""S-Port Pelik\0""Telemetry In\0""SBUS Trainer\0""IBUS Trainer\0""CRSF Trainer\0""SUMD Trainer\0""LUA\0 ""CLI\0 ""GPS\0 ""Debug\0 " #define LEN_SWTYPES "\006" #define TR_SWTYPES "None\0 ""Toggle""2POS\0 ""3POS\0" diff --git a/radio/src/translations/fr.h.txt b/radio/src/translations/fr.h.txt index b77f8e141cc..c88a1366850 100644 --- a/radio/src/translations/fr.h.txt +++ b/radio/src/translations/fr.h.txt @@ -65,7 +65,7 @@ #define TR_TRNCHN "CH1CH2CH3CH4" #define LEN_AUX_SERIAL_MODES "\016" -#define TR_AUX_SERIAL_MODES "OFF\0 ""Recopie Telem\0""Télémétrie In\0""Ecolage SBUS\0 ""Ecolage IBUS\0 ""LUA\0 ""CLI\0 ""GPS\0 ""Debug\0 " +#define TR_AUX_SERIAL_MODES "OFF\0 ""Recopie Telem\0""Télémétrie In\0""Ecolage SBUS\0 ""Ecolage IBUS\0 ""Ecolage CRSF\0 ""Ecolage SUMD\0 ""LUA\0 ""CLI\0 ""GPS\0 ""Debug\0 " #define LEN_SWTYPES "\006" #define TR_SWTYPES "Rien\0 ""Levier""2-POS\0""3-POS\0" diff --git a/radio/src/translations/it.h.txt b/radio/src/translations/it.h.txt index c56a1ffb610..0e9214bbb06 100644 --- a/radio/src/translations/it.h.txt +++ b/radio/src/translations/it.h.txt @@ -63,7 +63,7 @@ #define TR_TRNCHN "ch1ch2ch3ch4" #define LEN_AUX_SERIAL_MODES "\016" -#define TR_AUX_SERIAL_MODES "OFF\0 ""Replica S-Port""Telemetria\0 ""SBUS Trainer\0 ""IBUS Trainer\0 ""LUA\0 ""CLI\0 ""GPS\0 ""Debug\0 " +#define TR_AUX_SERIAL_MODES "OFF\0 ""Replica S-Port""Telemetria\0 ""SBUS Trainer\0 ""IBUS Trainer\0 ""CRSF Trainer\0 ""SUMD Trainer\0 ""LUA\0 ""CLI\0 ""GPS\0 ""Debug\0 " #define LEN_SWTYPES "\006" #define TR_SWTYPES "Dis.\0 ""Toggle""2POS\0 ""3POS\0" diff --git a/radio/src/translations/nl.h.txt b/radio/src/translations/nl.h.txt index 0af125a4b82..ad6973348f2 100644 --- a/radio/src/translations/nl.h.txt +++ b/radio/src/translations/nl.h.txt @@ -62,7 +62,7 @@ #define TR_TRNCHN "CH1CH2CH3CH4" #define LEN_AUX_SERIAL_MODES "\015" -#define TR_AUX_SERIAL_MODES "UIT\0 ""Telem Mirror\0""Telemetry In\0""SBUS Leerling""IBUS Leerling""LUA\0 ""CLI\0 ""GPS\0 ""Debug\0 " +#define TR_AUX_SERIAL_MODES "UIT\0 ""Telem Mirror\0""Telemetry In\0""SBUS Leerling""IBUS Leerling""CRSF Leerling""SUMD Leerling""LUA\0 ""CLI\0 ""GPS\0 ""Debug\0 " #define LEN_SWTYPES "\006" #define TR_SWTYPES "Geen\0 ""Wissel""2POS\0 ""3POS\0" diff --git a/radio/src/translations/pl.h.txt b/radio/src/translations/pl.h.txt index ef1cd10d70e..81eb6b42337 100644 --- a/radio/src/translations/pl.h.txt +++ b/radio/src/translations/pl.h.txt @@ -62,7 +62,7 @@ #define TR_TRNCHN "KN1KN2KN3KN4" #define LEN_AUX_SERIAL_MODES "\015" /*13 decimal*/ -#define TR_AUX_SERIAL_MODES "Wyłącz\0 ""S-Port Kopia ""Telemetria\0 ""Trener SBUS\0 ""Trener IBUS\0 ""LUA\0 ""CLI\0 ""GPS\0 ""Debug\0 " +#define TR_AUX_SERIAL_MODES "Wyłącz\0 ""S-Port Kopia ""Telemetria\0 ""Trener SBUS\0 ""Trener IBUS\0 ""Trener CRSF\0 ""Trener SUMD\0 ""LUA\0 ""CLI\0 ""GPS\0 ""Debug\0 " #define LEN_SWTYPES "\006" #define TR_SWTYPES "Brak\0 ""Chwil.""2POZ\0 ""3POZ\0" diff --git a/radio/src/translations/pt.h.txt b/radio/src/translations/pt.h.txt index 0ecb86e6c5b..0520fda4ef0 100644 --- a/radio/src/translations/pt.h.txt +++ b/radio/src/translations/pt.h.txt @@ -62,7 +62,7 @@ #define TR_TRNCHN "CH1CH2CH3CH4" #define LEN_AUX_SERIAL_MODES "\017" -#define TR_AUX_SERIAL_MODES "OFF\0 ""S-Port Mirror\0 ""Telemetry\0 ""SBUS Trainer\0 ""IBUS Trainer\0 ""LUA\0 ""CLI\0 ""GPS\0 ""Debug\0 " +#define TR_AUX_SERIAL_MODES "OFF\0 ""S-Port Mirror\0 ""Telemetry\0 ""SBUS Trainer\0 ""IBUS Trainer\0 ""CRSF Trainer\0 ""SUMD Trainer\0 ""LUA\0 ""CLI\0 ""GPS\0 ""Debug\0 " #define LEN_SWTYPES "\006" #define TR_SWTYPES "None\0 ""Toggle""2POS\0 ""3POS\0" diff --git a/radio/src/translations/se.h.txt b/radio/src/translations/se.h.txt index 58fb790a56d..be685889a22 100644 --- a/radio/src/translations/se.h.txt +++ b/radio/src/translations/se.h.txt @@ -68,7 +68,7 @@ #define TR_TRNCHN "KN1KN2KN3KN4" #define LEN_AUX_SERIAL_MODES "\022" -#define TR_AUX_SERIAL_MODES "Av\0 ""Spegling av S-Port""Telemetri\0 ""SBUS Trainer\0 ""IBUS Trainer\0 ""LUA\0 ""CLI\0 ""GPS\0 ""Debug\0 " +#define TR_AUX_SERIAL_MODES "Av\0 ""Spegling av S-Port""Telemetri\0 ""SBUS Trainer\0 ""IBUS Trainer\0 ""CRSF Trainer\0 ""SUMD Trainer\0 ""LUA\0 ""CLI\0 ""GPS\0 ""Debug\0 " #define LEN_SWTYPES "\006" #define TR_SWTYPES "Ingen\0""Flipp\0""2Pos\0 ""3Pos\0" From cbe064ae92a9ab1dee43bdfc7ba486aee8661227 Mon Sep 17 00:00:00 2001 From: wimalopaan Date: Fri, 25 Mar 2022 16:10:39 +0100 Subject: [PATCH 15/31] changed baudrate to 420KB for CRSF trainer input --- radio/src/crsf.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/radio/src/crsf.h b/radio/src/crsf.h index 042ee61be62..e0db325e1ac 100644 --- a/radio/src/crsf.h +++ b/radio/src/crsf.h @@ -4,7 +4,7 @@ #include "crc.h" #include "telemetry/crossfire.h" -#define CRSF_BAUDRATE 400000 +#define CRSF_BAUDRATE 420000 void processCrsfInput(); From b53877d2f6fb2e0925d94d01b6fc727122d25e6e Mon Sep 17 00:00:00 2001 From: wimalopaan Date: Sat, 26 Mar 2022 06:57:06 +0100 Subject: [PATCH 16/31] fixed silly mistakes decoding CRSF --- radio/src/crsf.cpp | 28 +++++++++++++------ radio/src/crsf.h | 1 + .../common/arm/stm32/timers_driver.cpp | 2 ++ 3 files changed, 22 insertions(+), 9 deletions(-) diff --git a/radio/src/crsf.cpp b/radio/src/crsf.cpp index 577ca5f046f..512e1f0e106 100644 --- a/radio/src/crsf.cpp +++ b/radio/src/crsf.cpp @@ -63,30 +63,29 @@ namespace CRSF { break; case State::Data: csum += b; - if (++mIndex == mLength) { + if (++mIndex >= mLength) { mState = State::AwaitCRC; } break; case State::Channels: csum += b; + mData[mIndex] = b; + if (++mIndex >= mLength) { + mState = State::AwaitCRCAndDecode; + } break; case State::AwaitCRC: if (csum == b) { - mState = State::Undefined; + // only channel data is decoded, nothing todo here } - else { - mState = State::Undefined; - } + mState = State::Undefined; break; case State::AwaitCRCAndDecode: if (csum == b) { ++mPackages; f(); - mState = State::Undefined; } - else { - mState = State::Undefined; - } + mState = State::Undefined; break; } } @@ -132,6 +131,17 @@ namespace CRSF { uint8_t Servo::mPauseCounter{Servo::mPauseCount}; // 2 ms } +void crsfTrainerPauseCheck() { +#if !defined(SIMU) +# if defined(AUX_SERIAL) || defined(AUX2_SERIAL) || defined(TRAINER_MODULE_SBUS) + if (hasSerialMode(UART_MODE_CRSF_TRAINER) >= 0) { + CRSF::Servo::tick1ms(); + processCrsfInput(); + } +# endif +#endif +} + void processCrsfInput() { #if !defined(SIMU) uint8_t rxchar; diff --git a/radio/src/crsf.h b/radio/src/crsf.h index e0db325e1ac..d535a18a99d 100644 --- a/radio/src/crsf.h +++ b/radio/src/crsf.h @@ -7,4 +7,5 @@ #define CRSF_BAUDRATE 420000 void processCrsfInput(); +void crsfTrainerPauseCheck(); diff --git a/radio/src/targets/common/arm/stm32/timers_driver.cpp b/radio/src/targets/common/arm/stm32/timers_driver.cpp index 02faffd9486..6a992ff0912 100644 --- a/radio/src/targets/common/arm/stm32/timers_driver.cpp +++ b/radio/src/targets/common/arm/stm32/timers_driver.cpp @@ -65,8 +65,10 @@ static void interrupt1ms() flysky_hall_stick_loop(); } #endif + #if defined(SBUS_TRAINER) sbusTrainerPauseCheck(); + crsfTrainerPauseCheck(); #endif // 5ms loop From 1c227616158310e1194b2850367486d1afbcb923 Mon Sep 17 00:00:00 2001 From: wimalopaan Date: Mon, 28 Mar 2022 14:29:55 +0200 Subject: [PATCH 17/31] fixes the freeze problem on startup --- radio/src/hal/serial_driver.h | 2 +- radio/src/lua/api_general.cpp | 6 +++--- radio/src/lua/lua_api.h | 2 +- radio/src/sbus.cpp | 12 ++++++------ radio/src/sbus.h | 6 +++--- radio/src/serial.cpp | 2 +- .../targets/common/arm/stm32/aux_serial_driver.cpp | 4 ++-- radio/src/targets/simu/module_drivers.cpp | 2 +- radio/src/telemetry/telemetry.cpp | 4 ++-- radio/src/telemetry/telemetry.h | 2 +- 10 files changed, 21 insertions(+), 21 deletions(-) diff --git a/radio/src/hal/serial_driver.h b/radio/src/hal/serial_driver.h index 3ebc84b50a5..a3929306569 100644 --- a/radio/src/hal/serial_driver.h +++ b/radio/src/hal/serial_driver.h @@ -73,7 +73,7 @@ typedef struct { void (*waitForTxCompleted)(void* ctx); // Fetch byte from internal buffer - int (*getByte)(void* ctx, uint8_t* data); + bool (*getByte)(void* ctx, uint8_t* data); // Get current baudrate uint32_t (*getBaudrate)(void*); diff --git a/radio/src/lua/api_general.cpp b/radio/src/lua/api_general.cpp index a2a049f7ec7..5b7357b5e30 100644 --- a/radio/src/lua/api_general.cpp +++ b/radio/src/lua/api_general.cpp @@ -101,7 +101,7 @@ static constexpr uint8_t maxSourceNameLength{16}; // static Fifo* luaRxFifo = nullptr; -static int luaRxFifoGetByte(void*, uint8_t* data) +static bool luaRxFifoGetByte(void*, uint8_t* data) { if (!luaRxFifo) return -1; return luaRxFifo->pop(*data); @@ -141,10 +141,10 @@ void luaSetSendCb(void* ctx, void (*cb)(void*, uint8_t)) luaSendDataCb = cb; } -static int (*luaGetSerialByte)(void*, uint8_t*) = nullptr; +static bool (*luaGetSerialByte)(void*, uint8_t*) = nullptr; static void* luaGetSerialByteCtx = nullptr; -void luaSetGetSerialByte(void* ctx, int (*fct)(void*, uint8_t*)) +void luaSetGetSerialByte(void* ctx, bool (*fct)(void*, uint8_t*)) { luaGetSerialByte = nullptr; luaGetSerialByteCtx = ctx; diff --git a/radio/src/lua/lua_api.h b/radio/src/lua/lua_api.h index 38ffe4e745f..23c79c64b68 100644 --- a/radio/src/lua/lua_api.h +++ b/radio/src/lua/lua_api.h @@ -55,7 +55,7 @@ void luaFreeRxFifo(); void luaReceiveData(uint8_t* buf, uint32_t len); void luaSetSendCb(void* ctx, void (*cb)(void*, uint8_t)); -void luaSetGetSerialByte(void* ctx, int (*fct)(void*, uint8_t*)); +void luaSetGetSerialByte(void* ctx, bool (*fct)(void*, uint8_t*)); extern lua_State * lsScripts; diff --git a/radio/src/sbus.cpp b/radio/src/sbus.cpp index cde34d74b30..636c1d243ad 100644 --- a/radio/src/sbus.cpp +++ b/radio/src/sbus.cpp @@ -36,17 +36,17 @@ #define SBUS_CH_CENTER 0x3E0 -static int (*_sbusAuxGetByte)(void*, uint8_t*) = nullptr; +static bool (*_sbusAuxGetByte)(void*, uint8_t*) = nullptr; static void* _sbusAuxGetByteCtx = nullptr; -void sbusSetAuxGetByte(void* ctx, int (*fct)(void*, uint8_t*)) +void sbusSetAuxGetByte(void* ctx, bool (*fct)(void*, uint8_t*)) { _sbusAuxGetByte = nullptr; _sbusAuxGetByteCtx = ctx; _sbusAuxGetByte = fct; } -int sbusAuxGetByte(uint8_t* byte) +bool sbusAuxGetByte(uint8_t* byte) { auto _getByte = _sbusAuxGetByte; auto _ctx = _sbusAuxGetByteCtx; @@ -55,12 +55,12 @@ int sbusAuxGetByte(uint8_t* byte) return _getByte(_ctx, byte); } - return -1; + return false; } -static int (*sbusGetByte)(uint8_t*) = nullptr; +static bool (*sbusGetByte)(uint8_t*) = nullptr; -void sbusSetGetByte(int (*fct)(uint8_t*)) +void sbusSetGetByte(bool (*fct)(uint8_t*)) { sbusGetByte = fct; } diff --git a/radio/src/sbus.h b/radio/src/sbus.h index c26cd02be1b..39ca7ec27fb 100644 --- a/radio/src/sbus.h +++ b/radio/src/sbus.h @@ -26,15 +26,15 @@ #define SBUS_FRAME_SIZE 25 // Setup SBUS AUX serial input -void sbusSetAuxGetByte(void* ctx, int (*fct)(void*, uint8_t*)); +void sbusSetAuxGetByte(void* ctx, bool (*fct)(void*, uint8_t*)); // SBUS AUX serial getter: // if set, it will fetch data from the handler set // with sbusSetAuxGetByte() -int sbusAuxGetByte(uint8_t* byte); +bool sbusAuxGetByte(uint8_t* byte); // Setup general SBUS input source -void sbusSetGetByte(int (*fct)(uint8_t*)); +void sbusSetGetByte(bool (*fct)(uint8_t*)); void processSbusInput(); void sbusTrainerPauseCheck(); diff --git a/radio/src/serial.cpp b/radio/src/serial.cpp index ad0a7568938..55021fffbbc 100644 --- a/radio/src/serial.cpp +++ b/radio/src/serial.cpp @@ -128,7 +128,7 @@ static SerialPortState* getSerialPortState(uint8_t port_nr) static void serialSetCallBacks(int mode, void* ctx, const etx_serial_port_t* port) { void (*sendByte)(void*, uint8_t) = nullptr; - int (*getByte)(void*, uint8_t*) = nullptr; + bool (*getByte)(void*, uint8_t*) = nullptr; void (*setRxCb)(void*, void (*)(uint8_t*, uint32_t)) = nullptr; const etx_serial_driver_t* drv = nullptr; diff --git a/radio/src/targets/common/arm/stm32/aux_serial_driver.cpp b/radio/src/targets/common/arm/stm32/aux_serial_driver.cpp index bc5c78ef5ad..142063a3404 100644 --- a/radio/src/targets/common/arm/stm32/aux_serial_driver.cpp +++ b/radio/src/targets/common/arm/stm32/aux_serial_driver.cpp @@ -85,10 +85,10 @@ static void aux_wait_tx_completed(void* ctx) (void)ctx; } -static int aux_get_byte(void* ctx, uint8_t* data) +static bool aux_get_byte(void* ctx, uint8_t* data) { auto st = (const SerialState*)ctx; - if (!st->rxFifo) return -1; + if (!st->rxFifo) return false; return st->rxFifo->pop(*data); } diff --git a/radio/src/targets/simu/module_drivers.cpp b/radio/src/targets/simu/module_drivers.cpp index f11a2f19ccf..f902808aefc 100644 --- a/radio/src/targets/simu/module_drivers.cpp +++ b/radio/src/targets/simu/module_drivers.cpp @@ -69,7 +69,7 @@ static void deinit(void*) {} static void sendByte(void*, uint8_t) {} static void sendBuffer(void*, const uint8_t*, uint8_t) {} static void waitForTxCompleted(void*) {} -static int getByte(void*,uint8_t*) { return -1; } +static bool getByte(void*,uint8_t*) { return -1; } const etx_serial_driver_t IntmoduleSerialDriver = { .init = init, diff --git a/radio/src/telemetry/telemetry.cpp b/radio/src/telemetry/telemetry.cpp index bd9a8e4cd0c..4356fccf690 100644 --- a/radio/src/telemetry/telemetry.cpp +++ b/radio/src/telemetry/telemetry.cpp @@ -69,10 +69,10 @@ uint8_t &getTelemetryRxBufferCount(uint8_t moduleIdx) return telemetryRxBufferCount; } -static int (*_telemetryGetByte)(void*, uint8_t*) = nullptr; +static bool (*_telemetryGetByte)(void*, uint8_t*) = nullptr; static void* _telemetryGetByteCtx = nullptr; -void telemetrySetGetByte(void* ctx, int (*fct)(void*, uint8_t*)) +void telemetrySetGetByte(void* ctx, bool (*fct)(void*, uint8_t*)) { _telemetryGetByte = nullptr; _telemetryGetByteCtx = ctx; diff --git a/radio/src/telemetry/telemetry.h b/radio/src/telemetry/telemetry.h index cdbc27cd3cf..c6df4ea0f27 100644 --- a/radio/src/telemetry/telemetry.h +++ b/radio/src/telemetry/telemetry.h @@ -79,7 +79,7 @@ uint8_t* getTelemetryRxBuffer(uint8_t moduleIdx); uint8_t& getTelemetryRxBufferCount(uint8_t moduleIdx); // Set alternative telemetry input -void telemetrySetGetByte(void* ctx, int (*fct)(void*, uint8_t*)); +void telemetrySetGetByte(void* ctx, bool (*fct)(void*, uint8_t*)); // Set telemetry mirror callback void telemetrySetMirrorCb(void* ctx, void (*fct)(void*, uint8_t)); From 5cfb85ed958d5e5f8a2a868c6f2ec947c65549cd Mon Sep 17 00:00:00 2001 From: wimalopaan Date: Mon, 28 Mar 2022 15:25:10 +0200 Subject: [PATCH 18/31] follow-up to previous serial rewrite adaption (taranis radios) --- radio/src/targets/simu/simpgmspace.cpp | 2 +- radio/src/targets/taranis/board.h | 2 +- radio/src/targets/taranis/trainer_driver.cpp | 2 +- 3 files changed, 3 insertions(+), 3 deletions(-) diff --git a/radio/src/targets/simu/simpgmspace.cpp b/radio/src/targets/simu/simpgmspace.cpp index c6b2fb3260c..2138709147d 100644 --- a/radio/src/targets/simu/simpgmspace.cpp +++ b/radio/src/targets/simu/simpgmspace.cpp @@ -732,7 +732,7 @@ void disableSpeaker() } #endif -int trainerModuleSbusGetByte(unsigned char*) { return 0; } +bool trainerModuleSbusGetByte(unsigned char*) { return 0; } void rtcInit() { diff --git a/radio/src/targets/taranis/board.h b/radio/src/targets/taranis/board.h index 55938380234..05e7ed3c2c1 100644 --- a/radio/src/targets/taranis/board.h +++ b/radio/src/targets/taranis/board.h @@ -171,7 +171,7 @@ uint32_t isBootloaderStart(const uint8_t * buffer); void init_trainer_module_sbus(); void init_trainer_module_ibus(); void stop_trainer_module_sbus(); - int trainerModuleSbusGetByte(uint8_t* byte); + bool trainerModuleSbusGetByte(uint8_t* byte); #endif void check_telemetry_exti(); diff --git a/radio/src/targets/taranis/trainer_driver.cpp b/radio/src/targets/taranis/trainer_driver.cpp index 34858ae3602..5c6d3fd28eb 100644 --- a/radio/src/targets/taranis/trainer_driver.cpp +++ b/radio/src/targets/taranis/trainer_driver.cpp @@ -378,7 +378,7 @@ void stop_trainer_module_sbus() } } -int trainerModuleSbusGetByte(uint8_t* byte) +bool trainerModuleSbusGetByte(uint8_t* byte) { return trainerSbusFifo.pop(*byte); } From d9fb50c9dcec44668baf4e1de7366d6c9d771961 Mon Sep 17 00:00:00 2001 From: wimalopaan Date: Wed, 30 Mar 2022 11:27:18 +0200 Subject: [PATCH 19/31] WIP for SumDV1/V3 --- radio/src/sumd.cpp | 278 +++++++++++++++++++++++++++++++++++++++++ radio/src/switches.cpp | 4 + radio/src/switches.h | 2 + radio/src/tasks.cpp | 2 +- radio/src/trainer.h | 15 ++- 5 files changed, 299 insertions(+), 2 deletions(-) diff --git a/radio/src/sumd.cpp b/radio/src/sumd.cpp index f01beb07ae0..26c7a71538b 100644 --- a/radio/src/sumd.cpp +++ b/radio/src/sumd.cpp @@ -1 +1,279 @@ #include "sumd.h" +#include "opentx.h" +#include "trainer.h" +#include "switches.h" + +#include +#include + +#ifdef __GNUC__ +# pragma GCC diagnostic push +# pragma GCC diagnostic error "-Wswitch" // unfortunately the project uses -Wnoswitch +#endif + +namespace SumDV3 { + struct Crc16 { + void reset() { + sum = 0; + } + void operator+=(const uint8_t v) { + sum = sum ^ (((uint16_t)v) << 8); + for(uint8_t i = 0; i < 8; ++i) { + if (sum & 0x8000) { + sum = (sum << 1) ^ crc_polynome; + } + else { + sum = (sum << 1); + } + } + } + operator uint16_t() const { + return sum; + } + private: + static constexpr uint16_t crc_polynome = 0x1021; + uint16_t sum{}; + }; + + struct Servo { + using SumDV3 = Trainer::Protocol::SumDV3; + using MesgType = SumDV3::MesgType; + using SwitchesType = SumDV3::SwitchesType; + + enum class State : uint8_t {Undefined, GotStart, StartV1, StartV3, V1ChannelDataH, V1ChannelDataL, CrcH, CrcL, + V3ChannelDataH, V3ChannelDataL, V3FuncCode, V3LastValidPackage, V3ModeCmd, V3SubCmd}; + + enum class Frame : uint8_t {Ch1to12 = 0x00, First = Ch1to12, + Ch1to8and13to16 = 0x01, Ch1to16 = 0x02, Ch1to8and17to24 = 0x03, + Ch1to8and25to32 = 0x04, Ch1to12and64Switches = 0x05, + Last = Ch1to12and64Switches, + Undefined = 0xff}; + + + static inline int16_t convertSumdToPuls(uint16_t const sumdValue) { + return Trainer::clamp(sumdValue - SumDV3::CenterValue); + } + + static inline void process(const uint8_t b, const std::function f) { + switch(mState) { // enum-switch -> no default (intentional) + case State::Undefined: + if (b == SumDV3::start_code) { + csum.reset(); + csum += b; + mState = State::GotStart; + } + break; + case State::GotStart: + csum += b; + if ((b & 0x0f) == SumDV3::version_code1) { + mState = State::StartV1; + } + else if ((b & 0x0f) == SumDV3::version_code3) { + mState = State::StartV3; + } + else { + mState = State::Undefined; + } + break; + case State::StartV1: + if ((b >= 2) && (b <= 32)) { + csum += b; + nChannels = b; + mIndex = 0; + mState = State::V1ChannelDataH; + } + else { + mState = State::Undefined; + } + break; + case State::V1ChannelDataH: + csum += b; + sumdFrame[mIndex].first = b; + mState = State::V1ChannelDataL; + break; + case State::V1ChannelDataL: + csum += b; + sumdFrame[mIndex].second = b; + ++mIndex; + if (mIndex < nChannels) { + mState = State::V1ChannelDataH; + } + else { + mState = State::CrcH; + frame = Frame::Ch1to16; + } + break; + case State::CrcH: + crcH = b; + mState = State::CrcL; + break; + case State::CrcL: + if ((((uint16_t)crcH << 8) | b) == csum) { + ++mPackagesCounter; + f(); + } + mState = State::Undefined; + break; + case State::StartV3: + if ((b >= 2) && (b <= 68)) { + csum += b; + nChannels = b - 2; + mIndex = 0; + mState = State::V3ChannelDataH; + } + else { + mState = State::Undefined; + } + break; + case State::V3ChannelDataH: + csum += b; + sumdFrame[mIndex].first = b; + mState = State::V3ChannelDataL; + break; + case State::V3ChannelDataL: + csum += b; + sumdFrame[mIndex].second = b; + ++mIndex; + if (mIndex < nChannels) { + mState = State::V3ChannelDataH; + } + else { + mState = State::V3FuncCode; + } + break; + case State::V3FuncCode: + csum += b; + if ((b >= uint8_t(Frame::First)) && (b <= uint8_t(Frame::Last))) { + frame = Frame(b); + } + else { + frame = Frame::Undefined; + } + mState = State::V3LastValidPackage; + break; + case State::V3LastValidPackage: + csum += b; + mState = State::V3ModeCmd; + break; + case State::V3ModeCmd: + csum += b; + mState = State::V3SubCmd; + break; + case State::V3SubCmd: + csum += b; + mState = State::CrcH; + break; + } + } + + template struct range_t {}; + + template + using offset_t = std::integral_constant; + + template + static inline void extract(const range_t&, int16_t (&dest)[N], offset_t = offset_t<0>{}) { + static_assert((End - Begin) < (N - Off), "wrong range or target size"); + + } + + static inline void sumSwitches() { + uint64_t sw = sumdFrame[12].first; + sw = (sw << 8) | sumdFrame[12].second; + sw = (sw << 8) | sumdFrame[13].first; + sw = (sw << 8) | sumdFrame[13].second; + sw = (sw << 8) | sumdFrame[14].first; + sw = (sw << 8) | sumdFrame[14].second; + sw = (sw << 8) | sumdFrame[15].first; + sw = (sw << 8) | sumdFrame[15].second; + + for (uint8_t i = 0; i < MAX_LOGICAL_SWITCHES; ++i) { + const uint64_t mask = (1 << i); + if (sw & mask) { + rawSetUnconnectedStickySwitch(i, true); + } + else { + rawSetUnconnectedStickySwitch(i, false); + } + } + } + + template + static inline void convert(int16_t (&pulses)[N]) { + switch(frame) { + case Frame::Ch1to12: + extract(range_t<0, 11>{}, pulses); + break; + case Frame::Ch1to8and13to16: + extract(range_t<0, 7>{}, pulses); + extract(range_t<8, 11>{}, pulses, offset_t<12>{}); + break; + case Frame::Ch1to8and17to24: + extract(range_t<0, 7>{}, pulses); + if constexpr(N > 16) { + extract(range_t<8, 15>{}, pulses, offset_t<16>{}); + } + break; + case Frame::Ch1to8and25to32: + extract(range_t<0, 7>{}, pulses); + if constexpr(N > 16) { + extract(range_t<8, 15>{}, pulses, offset_t<24>{}); + } + break; + case Frame::Ch1to16: + extract(range_t<0, 15>{}, pulses); + break; + case Frame::Ch1to12and64Switches: + extract(range_t<0, 11>{}, pulses); + sumSwitches(); + break; + case Frame::Undefined: + break; + } + } + private: + static uint8_t nChannels; + static Crc16 csum; + static uint8_t crcH; + + static State mState; + static MesgType sumdFrame; + static uint8_t mIndex; + static uint16_t mPackagesCounter; + + static Frame frame; + static uint8_t reserved; + static uint8_t mode_cmd; + static uint8_t sub_cmd; + }; + uint8_t Servo::nChannels{}; + Crc16 Servo::csum{}; + uint8_t Servo::crcH{}; + + Servo::State Servo::mState{Servo::State::Undefined}; + Servo::MesgType Servo::sumdFrame; + uint8_t Servo::mIndex{}; + uint16_t Servo::mPackagesCounter{}; + + Servo::Frame Servo::frame{Servo::Frame::Undefined}; + uint8_t Servo::reserved{}; + uint8_t Servo::mode_cmd{}; + uint8_t Servo::sub_cmd{}; +} + +void processSumdInput() { +#if !defined(SIMU) + uint8_t rxchar; + + while (sbusAuxGetByte(&rxchar)) { + SumDV3::Servo::process(rxchar, [&](){ + SumDV3::Servo::convert(ppmInput); + ppmInputValidityTimer = PPM_IN_VALID_TIMEOUT; + }); + } +#endif +} + +#ifdef __GNUC__ +# pragma GCC diagnostic pop +#endif diff --git a/radio/src/switches.cpp b/radio/src/switches.cpp index 0cb43ae76d1..9726ba16de4 100644 --- a/radio/src/switches.cpp +++ b/radio/src/switches.cpp @@ -1019,3 +1019,7 @@ void logicalSwitchesCopyState(uint8_t src, uint8_t dst) { lswFm[dst] = lswFm[src]; } + +void rawSetUnconnectedStickySwitch(const uint8_t, const bool) { + +} diff --git a/radio/src/switches.h b/radio/src/switches.h index 9bda7bdaaf7..6da0c7566be 100644 --- a/radio/src/switches.h +++ b/radio/src/switches.h @@ -40,3 +40,5 @@ uint8_t lswFamily(uint8_t func); int16_t lswTimerValue(delayval_t val); bool isSwitchWarningRequired(uint16_t &bad_pots); + +void rawSetUnconnectedStickySwitch(uint8_t, bool); diff --git a/radio/src/tasks.cpp b/radio/src/tasks.cpp index f35e7ff2927..b3418a9f647 100644 --- a/radio/src/tasks.cpp +++ b/radio/src/tasks.cpp @@ -100,7 +100,7 @@ void execMixerFrequentActions() processCrsfInput(); } else if (hasSerialMode(UART_MODE_SUMD_TRAINER) >= 0) { -// processSumdInput(); + processSumdInput(); } #endif diff --git a/radio/src/trainer.h b/radio/src/trainer.h index 845d9f2e2de..3bb60f70969 100644 --- a/radio/src/trainer.h +++ b/radio/src/trainer.h @@ -73,8 +73,21 @@ namespace Trainer { }; struct SumDV3 { - using MesgType = std::array; + static constexpr uint8_t start_code = 0xa8; + static constexpr uint8_t version_code1 = 0x01; + static constexpr uint8_t version_code3 = 0x03; + + static constexpr uint16_t ExtendedLow = 0x1c20; // 7200 + static constexpr uint16_t MinValue = 0x2260; // 8800 + static constexpr uint16_t CenterValue = 0x2ee0; // 12000 + static constexpr uint16_t MaxValue = 0x3b60; // 15200 + static constexpr uint16_t ExtendedHigh = 0x41a0; // 16800 + + static constexpr uint8_t MaxChannels = 32; + using MesgType = std::array, MaxChannels>; + using SwitchesType = uint64_t; + }; } } From c8d3d7f08bdb02676c821cc85d05dfce1818912c Mon Sep 17 00:00:00 2001 From: wimalopaan Date: Wed, 6 Apr 2022 06:39:32 +0200 Subject: [PATCH 20/31] refactored protocol adapters (sbus, ibus, crsf, sumd) * moved the interfaces to header files * introduced option EXTENDED_TRAINER (32 trainer channels, switches for sumd) * introduced cli interface to get statistics for trainer input --- radio/src/CMakeLists.txt | 5 + radio/src/cli.cpp | 17 +++ radio/src/crsf.cpp | 153 +++----------------- radio/src/crsf.h | 136 +++++++++++++++++- radio/src/dataconstants.h | 4 + radio/src/ibus.cpp | 139 ++---------------- radio/src/ibus.h | 129 ++++++++++++++++- radio/src/pulses/pulses.h | 2 +- radio/src/sbus.cpp | 158 ++------------------- radio/src/sbus.h | 132 ++++++++++++++++- radio/src/serial.cpp | 8 +- radio/src/sumd.cpp | 288 ++++---------------------------------- radio/src/sumd.h | 269 ++++++++++++++++++++++++++++++++++- radio/src/switches.cpp | 13 +- 14 files changed, 766 insertions(+), 687 deletions(-) diff --git a/radio/src/CMakeLists.txt b/radio/src/CMakeLists.txt index 6708875328b..389d38eceb2 100644 --- a/radio/src/CMakeLists.txt +++ b/radio/src/CMakeLists.txt @@ -58,6 +58,7 @@ option(IMRC_RELEASE "Used to build IMRC released firmware" OFF) option(HARDWARE_TRAINER_MULTI "Allow multi trainer" OFF) option(BOOTLOADER "Include Bootloader" ON) option(YAML_STORAGE "Enable YAML storage" ON) +option(EXTENDED_TRAINER "Enable 32 trainer channels for full SumDV3 support" OFF) # since we reset all default CMAKE compiler flags for firmware builds, provide an alternate way for user to specify additional flags. set(FIRMWARE_C_FLAGS "" CACHE STRING "Additional flags for firmware target c compiler (note: all CMAKE_C_FLAGS[_*] are ignored for firmware/bootloader).") @@ -453,6 +454,10 @@ if(HARDWARE_TRAINER_MULTI) add_definitions(-DHARDWARE_TRAINER_MULTI) endif() +if(EXTENDED_TRAINER) + add_definitions(-DEXTENDED_TRAINER) +endif() + add_definitions(-DPOPUP_LEVEL=${POPUP_LEVEL}) if(INTERNAL_MODULE_MULTI) diff --git a/radio/src/cli.cpp b/radio/src/cli.cpp index 39b3f9eb45a..c8753b067d6 100644 --- a/radio/src/cli.cpp +++ b/radio/src/cli.cpp @@ -31,6 +31,11 @@ #include #include "cli.h" +#include "sbus.h" +#include "ibus.h" +#include "crsf.h" +#include "sumd.h" + #if defined(INTMODULE_USART) #include "intmodule_serial_driver.h" #endif @@ -1559,6 +1564,15 @@ int cliCrypt(const char ** argv) } #endif +#if defined(EXTENDED_TRAINER) +static int trainer_stats(const char** const argv) { + cliSerialPrint("packages: sbus: %d, ibus: %d, crsf: %d, sumd: %d", + SBus::Servo<0>::packages(), IBus::Servo<0>::packages, + CRSF::Servo<0>::packages(), SumDV3::Servo<0>::packages()); + return 0; +} +#endif + const CliCommand cliCommands[] = { { "beep", cliBeep, "[] []" }, { "ls", cliLs, "" }, @@ -1594,6 +1608,9 @@ const CliCommand cliCommands[] = { #if defined(ACCESS_DENIED) && defined(DEBUG_CRYPT) { "crypt", cliCrypt, "" }, #endif +#if defined(EXTENDED_TRAINER) + { "tr_stats", trainer_stats, nullptr}, +#endif { nullptr, nullptr, nullptr } /* sentinel */ }; diff --git a/radio/src/crsf.cpp b/radio/src/crsf.cpp index 512e1f0e106..95e8a586478 100644 --- a/radio/src/crsf.cpp +++ b/radio/src/crsf.cpp @@ -1,141 +1,28 @@ #include "crsf.h" -#include "opentx.h" -#include "trainer.h" - -#include -#include - -#ifdef __GNUC__ -# pragma GCC diagnostic push -# pragma GCC diagnostic error "-Wswitch" // unfortunately the project uses -Wnoswitch -#endif namespace CRSF { - struct Servo { - using Crsf = Trainer::Protocol::Crsf; - using MesgType = Crsf::MesgType; - - static constexpr uint8_t mPauseCount{2}; // 2ms - - enum class State : uint8_t {Undefined, GotFCAddress, GotLength, Channels, Data, AwaitCRC, AwaitCRCAndDecode}; - - static inline int16_t convertCrsfToPuls(uint16_t const value) { - const int16_t centered = value - Crsf::CenterValue; - return Trainer::clamp((centered * 5) / 8); - } - - static inline void tick1ms() { - if (mPauseCounter > 0) { - --mPauseCounter; - } - else { - mState = State::Undefined; - } - } - - static inline void process(const uint8_t b, const std::function f) { - mPauseCounter = mPauseCount; - switch(mState) { // enum-switch -> no default (intentional) - case State::Undefined: - csum.reset(); - if (b == Crsf::FcAddress) { - mState = State::GotFCAddress; - } - break; - case State::GotFCAddress: - if ((b > 2) && (b <= mData.size())) { - mLength = b - 2; // only payload (not including type and crc) - mIndex = 0; - mState = State::GotLength; - } - else { - mState = State::Undefined; - } - break; - case State::GotLength: - csum += b; - if ((b == Crsf::FrametypeChannels) && (mLength == 22)) { - mState = State::Channels; - } - else { - mState = State::Data; - } - break; - case State::Data: - csum += b; - if (++mIndex >= mLength) { - mState = State::AwaitCRC; - } - break; - case State::Channels: - csum += b; - mData[mIndex] = b; - if (++mIndex >= mLength) { - mState = State::AwaitCRCAndDecode; - } - break; - case State::AwaitCRC: - if (csum == b) { - // only channel data is decoded, nothing todo here - } - mState = State::Undefined; - break; - case State::AwaitCRCAndDecode: - if (csum == b) { - ++mPackages; - f(); - } - mState = State::Undefined; - break; - } - } - static inline void convert(int16_t* const pulses) { - static_assert(MAX_TRAINER_CHANNELS == 16); - pulses[0] = (uint16_t) (((mData[0] | mData[1] << 8)) & Crsf::ValueMask); - pulses[1] = (uint16_t) ((mData[1]>>3 | mData[2] <<5) & Crsf::ValueMask); - pulses[2] = (uint16_t) ((mData[2]>>6 | mData[3] <<2 | mData[4]<<10) & Crsf::ValueMask); - pulses[3] = (uint16_t) ((mData[4]>>1 | mData[5] <<7) & Crsf::ValueMask); - pulses[4] = (uint16_t) ((mData[5]>>4 | mData[6] <<4) & Crsf::ValueMask); - pulses[5] = (uint16_t) ((mData[6]>>7 | mData[7] <<1 | mData[8]<<9) & Crsf::ValueMask); - pulses[6] = (uint16_t) ((mData[8]>>2 | mData[9] <<6) & Crsf::ValueMask); - pulses[7] = (uint16_t) ((mData[9]>>5 | mData[10]<<3) & Crsf::ValueMask); - pulses[8] = (uint16_t) ((mData[11] | mData[12]<<8) & Crsf::ValueMask); - pulses[9] = (uint16_t) ((mData[12]>>3 | mData[13]<<5) & Crsf::ValueMask); - pulses[10] = (uint16_t) ((mData[13]>>6 | mData[14]<<2 | mData[15]<<10) & Crsf::ValueMask); - pulses[11] = (uint16_t) ((mData[15]>>1 | mData[16]<<7) & Crsf::ValueMask); - pulses[12] = (uint16_t) ((mData[16]>>4 | mData[17]<<4) & Crsf::ValueMask); - pulses[13] = (uint16_t) ((mData[17]>>7 | mData[18]<<1 | mData[19]<<9) & Crsf::ValueMask); - pulses[14] = (uint16_t) ((mData[19]>>2 | mData[20]<<6) & Crsf::ValueMask); - pulses[15] = (uint16_t) ((mData[20]>>5 | mData[21]<<3) & Crsf::ValueMask); - - for(size_t i = 0; i < MAX_TRAINER_CHANNELS; ++i) { - pulses[i] = convertCrsfToPuls(pulses[i]); - } - } - private: - static CRC8 csum; - static State mState; - static MesgType mData; - static uint8_t mIndex; - static uint8_t mLength; - static uint16_t mPackages; - static uint8_t mPauseCounter; - }; - CRC8 Servo::csum; - Servo::State Servo::mState{Servo::State::Undefined}; - Servo::MesgType Servo::mData; - uint8_t Servo::mIndex{0}; - uint8_t Servo::mLength{0}; - uint16_t Servo::mPackages{0}; - uint8_t Servo::mPauseCounter{Servo::mPauseCount}; // 2 ms + template + CRC8 Servo::csum; + template + typename Servo::State Servo::mState{Servo::State::Undefined}; + template + typename Servo::MesgType Servo::mData; + template + uint8_t Servo::mIndex{0}; + template + uint8_t Servo::mLength{0}; + template + uint16_t Servo::mPackages{0}; + template + uint8_t Servo::mPauseCounter{Servo::mPauseCount}; // 2 ms } void crsfTrainerPauseCheck() { #if !defined(SIMU) -# if defined(AUX_SERIAL) || defined(AUX2_SERIAL) || defined(TRAINER_MODULE_SBUS) +# if defined(AUX_SERIAL) || defined(AUX2_SERIAL) if (hasSerialMode(UART_MODE_CRSF_TRAINER) >= 0) { - CRSF::Servo::tick1ms(); + CRSF::Servo<0>::tick1ms(); processCrsfInput(); } # endif @@ -147,14 +34,10 @@ void processCrsfInput() { uint8_t rxchar; while (sbusAuxGetByte(&rxchar)) { - CRSF::Servo::process(rxchar, [&](){ - CRSF::Servo::convert(ppmInput); + CRSF::Servo<0>::process(rxchar, [&](){ + CRSF::Servo<0>::convert(ppmInput); ppmInputValidityTimer = PPM_IN_VALID_TIMEOUT; }); } #endif } - -#ifdef __GNUC__ -# pragma GCC diagnostic pop -#endif diff --git a/radio/src/crsf.h b/radio/src/crsf.h index d535a18a99d..4f10d496723 100644 --- a/radio/src/crsf.h +++ b/radio/src/crsf.h @@ -1,11 +1,141 @@ #pragma once -#include "dataconstants.h" +#include "opentx.h" +#include "trainer.h" #include "crc.h" -#include "telemetry/crossfire.h" -#define CRSF_BAUDRATE 420000 +#include +#include void processCrsfInput(); void crsfTrainerPauseCheck(); +#ifdef __GNUC__ +# pragma GCC diagnostic push +# pragma GCC diagnostic error "-Wswitch" // unfortunately the project uses -Wnoswitch +#endif + +namespace CRSF { + static constexpr uint32_t baudrate{420000}; + + template + struct Servo { + using Crsf = Trainer::Protocol::Crsf; + using MesgType = Crsf::MesgType; + + static constexpr uint8_t mPauseCount{2}; // 2ms + + static constexpr uint8_t CRSFChannels{16}; + + enum class State : uint8_t {Undefined, GotFCAddress, GotLength, Channels, Data, AwaitCRC, AwaitCRCAndDecode}; + + static inline int16_t convertCrsfToPuls(uint16_t const value) { + const int16_t centered = value - Crsf::CenterValue; + return Trainer::clamp((centered * 5) / 8); + } + + static inline void tick1ms() { + if (mPauseCounter > 0) { + --mPauseCounter; + } + else { + mState = State::Undefined; + } + } + + static inline void process(const uint8_t b, const std::function f) { + mPauseCounter = mPauseCount; + switch(mState) { // enum-switch -> no default (intentional) + case State::Undefined: + csum.reset(); + if (b == Crsf::FcAddress) { + mState = State::GotFCAddress; + } + break; + case State::GotFCAddress: + if ((b > 2) && (b <= mData.size())) { + mLength = b - 2; // only payload (not including type and crc) + mIndex = 0; + mState = State::GotLength; + } + else { + mState = State::Undefined; + } + break; + case State::GotLength: + csum += b; + if ((b == Crsf::FrametypeChannels) && (mLength == 22)) { + mState = State::Channels; + } + else { + mState = State::Data; + } + break; + case State::Data: + csum += b; + if (++mIndex >= mLength) { + mState = State::AwaitCRC; + } + break; + case State::Channels: + csum += b; + mData[mIndex] = b; + if (++mIndex >= mLength) { + mState = State::AwaitCRCAndDecode; + } + break; + case State::AwaitCRC: + if (csum == b) { + // only channel data is decoded, nothing todo here + } + mState = State::Undefined; + break; + case State::AwaitCRCAndDecode: + if (csum == b) { + ++mPackages; + f(); + } + mState = State::Undefined; + break; + } + } + template + static inline void convert(int16_t (&pulses)[N]) { + static_assert(N >= 16, "array too small"); + pulses[0] = (uint16_t) (((mData[0] | mData[1] << 8)) & Crsf::ValueMask); + pulses[1] = (uint16_t) ((mData[1]>>3 | mData[2] <<5) & Crsf::ValueMask); + pulses[2] = (uint16_t) ((mData[2]>>6 | mData[3] <<2 | mData[4]<<10) & Crsf::ValueMask); + pulses[3] = (uint16_t) ((mData[4]>>1 | mData[5] <<7) & Crsf::ValueMask); + pulses[4] = (uint16_t) ((mData[5]>>4 | mData[6] <<4) & Crsf::ValueMask); + pulses[5] = (uint16_t) ((mData[6]>>7 | mData[7] <<1 | mData[8]<<9) & Crsf::ValueMask); + pulses[6] = (uint16_t) ((mData[8]>>2 | mData[9] <<6) & Crsf::ValueMask); + pulses[7] = (uint16_t) ((mData[9]>>5 | mData[10]<<3) & Crsf::ValueMask); + pulses[8] = (uint16_t) ((mData[11] | mData[12]<<8) & Crsf::ValueMask); + pulses[9] = (uint16_t) ((mData[12]>>3 | mData[13]<<5) & Crsf::ValueMask); + pulses[10] = (uint16_t) ((mData[13]>>6 | mData[14]<<2 | mData[15]<<10) & Crsf::ValueMask); + pulses[11] = (uint16_t) ((mData[15]>>1 | mData[16]<<7) & Crsf::ValueMask); + pulses[12] = (uint16_t) ((mData[16]>>4 | mData[17]<<4) & Crsf::ValueMask); + pulses[13] = (uint16_t) ((mData[17]>>7 | mData[18]<<1 | mData[19]<<9) & Crsf::ValueMask); + pulses[14] = (uint16_t) ((mData[19]>>2 | mData[20]<<6) & Crsf::ValueMask); + pulses[15] = (uint16_t) ((mData[20]>>5 | mData[21]<<3) & Crsf::ValueMask); + + for(size_t i = 0; i < N; ++i) { + pulses[i] = convertCrsfToPuls(pulses[i]); + } + } + static inline uint16_t packages() { + return mPackages; + } + private: + static CRC8 csum; + static State mState; + static MesgType mData; + static uint8_t mIndex; + static uint8_t mLength; + static uint16_t mPackages; + static uint8_t mPauseCounter; + }; +} +#ifdef __GNUC__ +# pragma GCC diagnostic pop +#endif diff --git a/radio/src/dataconstants.h b/radio/src/dataconstants.h index fdbceeff874..c6b2760cd55 100644 --- a/radio/src/dataconstants.h +++ b/radio/src/dataconstants.h @@ -49,7 +49,11 @@ #define MAX_INPUTS 32 #define MIN_TRAINER_CHANNELS 4 #define DEF_TRAINER_CHANNELS 8 +#if defined(EXTENDED_TRAINER) #define MAX_TRAINER_CHANNELS 16 +#else + #define MAX_TRAINER_CHANNELS 16 +#endif #define MAX_TELEMETRY_SENSORS 60 #define MAX_CUSTOM_SCREENS 5 #elif defined(PCBX9D) || defined(PCBX9DP) || defined(PCBX9E) diff --git a/radio/src/ibus.cpp b/radio/src/ibus.cpp index 963f205f227..025fade1d13 100644 --- a/radio/src/ibus.cpp +++ b/radio/src/ibus.cpp @@ -1,128 +1,16 @@ #include "ibus.h" -#include "opentx.h" -#include "trainer.h" - -#include -#include - -#ifdef __GNUC__ -# pragma GCC diagnostic push -# pragma GCC diagnostic error "-Wswitch" // unfortunately the project uses -Wnoswitch -#endif namespace IBus { - struct CheckSum final { - inline void reset() { - mSum = std::numeric_limits::max(); - } - inline uint8_t operator+=(const uint8_t b) { - mSum -= b; - return b; - } - inline uint8_t highByte() const { - return uint8_t(mSum >> 8); - } - inline uint8_t lowByte() const { - return uint8_t(mSum); - } - inline void highByte(const uint8_t hb) { - mH = hb; - } - inline void lowByte(const uint8_t lb){ - mL = lb; - } - inline explicit operator bool() const { - return ((mSum & 0xff) == mL) && (((mSum >> 8) & 0xff) == mH); - } - private: - uint8_t mH{}; - uint8_t mL{}; - uint16_t mSum = std::numeric_limits::max(); - }; - - struct Servo { - using IBus = Trainer::Protocol::IBus; - using MesgType = IBus::MesgType; - - enum class State : uint8_t {Undefined, GotStart20, Data, CheckL, CheckH}; - - static inline int16_t convertIbusToPuls(uint16_t const ibusValue) { - return Trainer::clamp(ibusValue - IBus::CenterValue); - } - - static inline void process(const uint8_t b, const std::function f) { - switch(mState) { // enum-switch -> no default (intentional) - case State::Undefined: - csum.reset(); - if (b == IBus::StartByte1) { - csum += b; - mState = State::GotStart20; - } - break; - case State::GotStart20: - if (b == IBus::StartByte2) { - csum += b; - mState = State::Data; - mIndex = 0; - } - else { - mState = State::Undefined; - } - break; - case State::Data: - ibusFrame[mIndex] = b; - csum += b; - if (mIndex >= (ibusFrame.size() - 1)) { - mState = State::CheckL; - } - else { - ++mIndex; - } - break; - case State::CheckL: - csum.lowByte(b); - mState = State::CheckH; - break; - case State::CheckH: - csum.highByte(b); - mState = State::Undefined; - if (csum) { - ++mPackagesCounter; - f(); - } - break; - } - } - static inline void convert(int16_t* const pulses) { - for (size_t chi{0}; chi < MAX_TRAINER_CHANNELS; chi++) { - if (chi < 14) { - const uint8_t h = ibusFrame[2 * chi + 1] & 0x0f; - const uint8_t l = ibusFrame[2 * chi]; - const uint16_t v = (uint16_t(h) << 8) + uint8_t(l); - pulses[chi] = convertIbusToPuls(v); - } - else if (chi < 18) { - const uint8_t h1 = ibusFrame[6 * (chi - 14) + 1] & 0xf0; - const uint8_t h2 = ibusFrame[6 * (chi - 14) + 3] & 0xf0; - const uint8_t h3 = ibusFrame[6 * (chi - 14) + 5] & 0xf0; - const uint16_t v = (uint8_t(h1) >> 4) + uint8_t(h2) + (uint16_t(h3) << 4); - pulses[chi] = convertIbusToPuls(v); - } - } - } - private: - static CheckSum csum; - static State mState; - static MesgType ibusFrame; - static uint8_t mIndex; - static uint16_t mPackagesCounter; - }; - - CheckSum Servo::csum; - Servo::State Servo::mState{Servo::State::Undefined}; - Servo::MesgType Servo::ibusFrame; - uint8_t Servo::mIndex{}; - uint16_t Servo::mPackagesCounter{}; + template + CheckSum Servo::csum; + template + typename Servo::State Servo::mState{Servo::State::Undefined}; + template + typename Servo::MesgType Servo::ibusFrame; + template + uint8_t Servo::mIndex{}; + template + uint16_t Servo::mPackagesCounter{}; } void processIbusInput() { @@ -130,14 +18,11 @@ void processIbusInput() { uint8_t rxchar; while (sbusAuxGetByte(&rxchar)) { - IBus::Servo::process(rxchar, [&](){ - IBus::Servo::convert(ppmInput); + IBus::Servo<0>::process(rxchar, [&](){ + IBus::Servo<0>::convert(ppmInput); ppmInputValidityTimer = PPM_IN_VALID_TIMEOUT; }); } #endif } -#ifdef __GNUC__ -# pragma GCC diagnostic pop -#endif diff --git a/radio/src/ibus.h b/radio/src/ibus.h index cd8493f828a..8e795e9087b 100644 --- a/radio/src/ibus.h +++ b/radio/src/ibus.h @@ -1,6 +1,133 @@ #pragma once -#define IBUS_BAUDRATE 115200 +#include "opentx.h" +#include "trainer.h" + +#include +#include void processIbusInput(); +#ifdef __GNUC__ +# pragma GCC diagnostic push +# pragma GCC diagnostic error "-Wswitch" // unfortunately the project uses -Wnoswitch +#endif + +namespace IBus { + static constexpr uint32_t baudrate{115200}; + + struct CheckSum final { + inline void reset() { + mSum = std::numeric_limits::max(); + } + inline uint8_t operator+=(const uint8_t b) { + mSum -= b; + return b; + } + inline uint8_t highByte() const { + return uint8_t(mSum >> 8); + } + inline uint8_t lowByte() const { + return uint8_t(mSum); + } + inline void highByte(const uint8_t hb) { + mH = hb; + } + inline void lowByte(const uint8_t lb){ + mL = lb; + } + inline explicit operator bool() const { + return ((mSum & 0xff) == mL) && (((mSum >> 8) & 0xff) == mH); + } + private: + uint8_t mH{}; + uint8_t mL{}; + uint16_t mSum = std::numeric_limits::max(); + }; + + template + struct Servo { + using IBus = Trainer::Protocol::IBus; + using MesgType = IBus::MesgType; + + enum class State : uint8_t {Undefined, GotStart20, Data, CheckL, CheckH}; + + static inline int16_t convertIbusToPuls(uint16_t const ibusValue) { + return Trainer::clamp(ibusValue - IBus::CenterValue); + } + + static inline void process(const uint8_t b, const std::function f) { + switch(mState) { // enum-switch -> no default (intentional) + case State::Undefined: + csum.reset(); + if (b == IBus::StartByte1) { + csum += b; + mState = State::GotStart20; + } + break; + case State::GotStart20: + if (b == IBus::StartByte2) { + csum += b; + mState = State::Data; + mIndex = 0; + } + else { + mState = State::Undefined; + } + break; + case State::Data: + ibusFrame[mIndex] = b; + csum += b; + if (mIndex >= (ibusFrame.size() - 1)) { + mState = State::CheckL; + } + else { + ++mIndex; + } + break; + case State::CheckL: + csum.lowByte(b); + mState = State::CheckH; + break; + case State::CheckH: + csum.highByte(b); + mState = State::Undefined; + if (csum) { + ++mPackagesCounter; + f(); + } + break; + } + } + static inline void convert(int16_t* const pulses) { + for (size_t chi{0}; chi < MAX_TRAINER_CHANNELS; chi++) { + if (chi < 14) { + const uint8_t h = ibusFrame[2 * chi + 1] & 0x0f; + const uint8_t l = ibusFrame[2 * chi]; + const uint16_t v = (uint16_t(h) << 8) + uint8_t(l); + pulses[chi] = convertIbusToPuls(v); + } + else if (chi < 18) { + const uint8_t h1 = ibusFrame[6 * (chi - 14) + 1] & 0xf0; + const uint8_t h2 = ibusFrame[6 * (chi - 14) + 3] & 0xf0; + const uint8_t h3 = ibusFrame[6 * (chi - 14) + 5] & 0xf0; + const uint16_t v = (uint8_t(h1) >> 4) + uint8_t(h2) + (uint16_t(h3) << 4); + pulses[chi] = convertIbusToPuls(v); + } + } + } + static inline uint16_t packages() { + return mPackagesCounter; + } + private: + static CheckSum csum; + static State mState; + static MesgType ibusFrame; + static uint8_t mIndex; + static uint16_t mPackagesCounter; + }; +} + +#ifdef __GNUC__ +# pragma GCC diagnostic pop +#endif diff --git a/radio/src/pulses/pulses.h b/radio/src/pulses/pulses.h index 48126248c8e..b1cf5bfc9d6 100644 --- a/radio/src/pulses/pulses.h +++ b/radio/src/pulses/pulses.h @@ -125,7 +125,7 @@ typedef Dsm2TimerPulsesData Dsm2PulsesData; #define PPM_PERIOD(module) (PPM_PERIOD_HALF_US(module) / 2) /*us*/ #define DSM2_BAUDRATE 125000 #define DSM2_PERIOD 22000 /*us*/ -#define SBUS_BAUDRATE 100000 +//#define SBUS_BAUDRATE 100000 #define SBUS_MIN_PERIOD 60 /*6.0ms 1/10ms*/ #define SBUS_MAX_PERIOD 325 /*Overflows uint16_t if set higher*/ #define SBUS_DEF_PERIOD 225 diff --git a/radio/src/sbus.cpp b/radio/src/sbus.cpp index 636c1d243ad..d8b886cb2a3 100644 --- a/radio/src/sbus.cpp +++ b/radio/src/sbus.cpp @@ -19,7 +19,6 @@ * GNU General Public License for more details. */ -#include "opentx.h" #include "sbus.h" #include "timers_driver.h" @@ -66,167 +65,36 @@ void sbusSetGetByte(bool (*fct)(uint8_t*)) } namespace SBus { - struct Servo { - using SBus = Trainer::Protocol::SBus; - using MesgType = SBus::MesgType; - static_assert(std::tuple_size::value == (SBUS_FRAME_SIZE - 2), "consistency check"); - - enum class State : uint8_t {Undefined, Data, GotEnd, WaitEnd}; - - static constexpr uint8_t mPauseCount{2}; // 2ms - - static inline void tick1ms() { - if (mPauseCounter > 0) { - --mPauseCounter; - } - else { - mState = State::Undefined; - } - } - - static inline int16_t convertSbusToPuls(uint16_t const sbusValue) { - const int16_t centered = sbusValue - SBus::CenterValue; - return Trainer::clamp((centered * 5) / 8); - } - - static inline void process(const uint8_t b, const std::function f) { - mPauseCounter = mPauseCount; - switch(mState) { // enum-switch -> no default (intentional) - case State::Undefined: - if (b == SBus::EndByte) { - mState = State::GotEnd; - } - else if (b == SBus::StartByte) { - mState = State::Data; - mIndex = 0; - } - break; - case State::GotEnd: - if (b == SBus::StartByte) { - mState = State::Data; - mIndex = 0; - } - else if (b == SBus::EndByte) { - mState = State::GotEnd; - } - else { - mState = State::Undefined; - } - break; - case State::Data: - mData[mIndex] = b; - if (mIndex >= (mData.size() - 1)) { // got last byte - mState = State::WaitEnd; - } - else { - ++mIndex; - } - break; - case State::WaitEnd: - if (b == SBus::EndByte) { - mState = State::GotEnd; - uint8_t& statusByte = mData[mData.size() - 1]; // last byte - if (!((statusByte & SBus::FrameLostMask) || (statusByte & SBus::FailSafeMask))) { - f(); - ++mPackages; - } - } - else { - mState = State::Undefined; - } - break; - } - } - static inline void convert(int16_t* const pulses) { - static_assert(MAX_TRAINER_CHANNELS == 16); - pulses[0] = (uint16_t) (((mData[0] | mData[1] << 8)) & SBus::ValueMask); - pulses[1] = (uint16_t) ((mData[1]>>3 | mData[2] <<5) & SBus::ValueMask); - pulses[2] = (uint16_t) ((mData[2]>>6 | mData[3] <<2 | mData[4]<<10) & SBus::ValueMask); - pulses[3] = (uint16_t) ((mData[4]>>1 | mData[5] <<7) & SBus::ValueMask); - pulses[4] = (uint16_t) ((mData[5]>>4 | mData[6] <<4) & SBus::ValueMask); - pulses[5] = (uint16_t) ((mData[6]>>7 | mData[7] <<1 | mData[8]<<9) & SBus::ValueMask); - pulses[6] = (uint16_t) ((mData[8]>>2 | mData[9] <<6) & SBus::ValueMask); - pulses[7] = (uint16_t) ((mData[9]>>5 | mData[10]<<3) & SBus::ValueMask); - pulses[8] = (uint16_t) ((mData[11] | mData[12]<<8) & SBus::ValueMask); - pulses[9] = (uint16_t) ((mData[12]>>3 | mData[13]<<5) & SBus::ValueMask); - pulses[10] = (uint16_t) ((mData[13]>>6 | mData[14]<<2 | mData[15]<<10) & SBus::ValueMask); - pulses[11] = (uint16_t) ((mData[15]>>1 | mData[16]<<7) & SBus::ValueMask); - pulses[12] = (uint16_t) ((mData[16]>>4 | mData[17]<<4) & SBus::ValueMask); - pulses[13] = (uint16_t) ((mData[17]>>7 | mData[18]<<1 | mData[19]<<9) & SBus::ValueMask); - pulses[14] = (uint16_t) ((mData[19]>>2 | mData[20]<<6) & SBus::ValueMask); - pulses[15] = (uint16_t) ((mData[20]>>5 | mData[21]<<3) & SBus::ValueMask); - - for(size_t i = 0; i < MAX_TRAINER_CHANNELS; ++i) { - pulses[i] = convertSbusToPuls(pulses[i]); - } - } - private: - static State mState; - static MesgType mData; - static uint8_t mIndex; - static uint16_t mPackages; - static uint8_t mPauseCounter; - }; - Servo::State Servo::mState{Servo::State::Undefined}; - Servo::MesgType Servo::mData; - uint8_t Servo::mIndex{0}; - uint16_t Servo::mPackages{0}; - uint8_t Servo::mPauseCounter{Servo::mPauseCount}; // 2 ms + template + typename Servo::State Servo::mState{Servo::State::Undefined}; + template + typename Servo::MesgType Servo::mData; + template + uint8_t Servo::mIndex{0}; + template + uint16_t Servo::mPackages{0}; + template + uint8_t Servo::mPauseCounter{Servo::mPauseCount}; // 2 ms } void sbusTrainerPauseCheck() { #if !defined(SIMU) # if defined(AUX_SERIAL) || defined(AUX2_SERIAL) || defined(TRAINER_MODULE_SBUS) if (hasSerialMode(UART_MODE_SBUS_TRAINER) >= 0) { - SBus::Servo::tick1ms(); + SBus::Servo<0>::tick1ms(); processSbusInput(); } # endif #endif } -//void processSbusInput() -//{ - -// // TODO: place this outside of the function -// static uint8_t SbusIndex = 0; -// static uint16_t SbusTimer; -// static uint8_t SbusFrame[SBUS_FRAME_SIZE]; - -// uint32_t active = 0; - -// // Drain input first (if existing) -// uint8_t rxchar; -// auto _getByte = sbusGetByte; -// while (_getByte && (_getByte(&rxchar) > 0)) { -// active = 1; -// if (SbusIndex > SBUS_FRAME_SIZE - 1) { -// SbusIndex = SBUS_FRAME_SIZE - 1; -// } -// SbusFrame[SbusIndex++] = rxchar; -// } - -// // Data has been received -// if (active) { -// SbusTimer = getTmr2MHz(); -// return; -// } - -// // Check if end-of-frame is detected -// if (SbusIndex) { -// if ((uint16_t)(getTmr2MHz() - SbusTimer) > SBUS_FRAME_GAP_DELAY) { -// processSbusFrame(SbusFrame, ppmInput, SbusIndex); -// SbusIndex = 0; -// } -// } - void processSbusInput() { #if !defined(SIMU) uint8_t rxchar; while (sbusAuxGetByte(&rxchar)) { - SBus::Servo::process(rxchar, [&](){ - SBus::Servo::convert(ppmInput); + SBus::Servo<0>::process(rxchar, [&](){ + SBus::Servo<0>::convert(ppmInput); ppmInputValidityTimer = PPM_IN_VALID_TIMEOUT; }); } diff --git a/radio/src/sbus.h b/radio/src/sbus.h index 39ca7ec27fb..1031102641e 100644 --- a/radio/src/sbus.h +++ b/radio/src/sbus.h @@ -19,11 +19,11 @@ * GNU General Public License for more details. */ -#ifndef _SBUS_H_ -#define _SBUS_H_ +#pragma once + +#include "opentx.h" +#include "trainer.h" -#define SBUS_BAUDRATE 100000 -#define SBUS_FRAME_SIZE 25 // Setup SBUS AUX serial input void sbusSetAuxGetByte(void* ctx, bool (*fct)(void*, uint8_t*)); @@ -39,4 +39,126 @@ void sbusSetGetByte(bool (*fct)(uint8_t*)); void processSbusInput(); void sbusTrainerPauseCheck(); -#endif // _SBUS_H_ +#ifdef __GNUC__ +# pragma GCC diagnostic push +# pragma GCC diagnostic error "-Wswitch" // unfortunately the project uses -Wnoswitch +#endif + +namespace SBus { + static constexpr uint32_t baudrate{100000}; + + template + struct Servo { + using SBus = Trainer::Protocol::SBus; + using MesgType = SBus::MesgType; + +// static constexpr uint8_t frameSize{25}; +// static_assert(std::tuple_size::value == (SBUS_FRAME_SIZE - 2), "consistency check"); + + enum class State : uint8_t {Undefined, Data, GotEnd, WaitEnd}; + + static constexpr uint8_t mPauseCount{2}; // 2ms + + static inline void tick1ms() { + if (mPauseCounter > 0) { + --mPauseCounter; + } + else { + mState = State::Undefined; + } + } + + static inline int16_t convertSbusToPuls(uint16_t const sbusValue) { + const int16_t centered = sbusValue - SBus::CenterValue; + return Trainer::clamp((centered * 5) / 8); + } + + static inline void process(const uint8_t b, const std::function f) { + mPauseCounter = mPauseCount; + switch(mState) { // enum-switch -> no default (intentional) + case State::Undefined: + if (b == SBus::EndByte) { + mState = State::GotEnd; + } + else if (b == SBus::StartByte) { + mState = State::Data; + mIndex = 0; + } + break; + case State::GotEnd: + if (b == SBus::StartByte) { + mState = State::Data; + mIndex = 0; + } + else if (b == SBus::EndByte) { + mState = State::GotEnd; + } + else { + mState = State::Undefined; + } + break; + case State::Data: + mData[mIndex] = b; + if (mIndex >= (mData.size() - 1)) { // got last byte + mState = State::WaitEnd; + } + else { + ++mIndex; + } + break; + case State::WaitEnd: + if (b == SBus::EndByte) { + mState = State::GotEnd; + uint8_t& statusByte = mData[mData.size() - 1]; // last byte + if (!((statusByte & SBus::FrameLostMask) || (statusByte & SBus::FailSafeMask))) { + f(); + ++mPackages; + } + } + else { + mState = State::Undefined; + } + break; + } + } + + template + static inline void convert(int16_t (&pulses)[N]) { + static_assert(N >= 16, "array too small"); + pulses[0] = (uint16_t) (((mData[0] | mData[1] << 8)) & SBus::ValueMask); + pulses[1] = (uint16_t) ((mData[1]>>3 | mData[2] <<5) & SBus::ValueMask); + pulses[2] = (uint16_t) ((mData[2]>>6 | mData[3] <<2 | mData[4]<<10) & SBus::ValueMask); + pulses[3] = (uint16_t) ((mData[4]>>1 | mData[5] <<7) & SBus::ValueMask); + pulses[4] = (uint16_t) ((mData[5]>>4 | mData[6] <<4) & SBus::ValueMask); + pulses[5] = (uint16_t) ((mData[6]>>7 | mData[7] <<1 | mData[8]<<9) & SBus::ValueMask); + pulses[6] = (uint16_t) ((mData[8]>>2 | mData[9] <<6) & SBus::ValueMask); + pulses[7] = (uint16_t) ((mData[9]>>5 | mData[10]<<3) & SBus::ValueMask); + pulses[8] = (uint16_t) ((mData[11] | mData[12]<<8) & SBus::ValueMask); + pulses[9] = (uint16_t) ((mData[12]>>3 | mData[13]<<5) & SBus::ValueMask); + pulses[10] = (uint16_t) ((mData[13]>>6 | mData[14]<<2 | mData[15]<<10) & SBus::ValueMask); + pulses[11] = (uint16_t) ((mData[15]>>1 | mData[16]<<7) & SBus::ValueMask); + pulses[12] = (uint16_t) ((mData[16]>>4 | mData[17]<<4) & SBus::ValueMask); + pulses[13] = (uint16_t) ((mData[17]>>7 | mData[18]<<1 | mData[19]<<9) & SBus::ValueMask); + pulses[14] = (uint16_t) ((mData[19]>>2 | mData[20]<<6) & SBus::ValueMask); + pulses[15] = (uint16_t) ((mData[20]>>5 | mData[21]<<3) & SBus::ValueMask); + + for(size_t i = 0; i < N; ++i) { + pulses[i] = convertSbusToPuls(pulses[i]); + } + } + + static inline uint16_t packages() { + return mPackages; + } + private: + static State mState; + static MesgType mData; + static uint8_t mIndex; + static uint16_t mPackages; + static uint8_t mPauseCounter; + }; +} + +#ifdef __GNUC__ +# pragma GCC diagnostic pop +#endif diff --git a/radio/src/serial.cpp b/radio/src/serial.cpp index 55021fffbbc..6c6f1d3c5de 100644 --- a/radio/src/serial.cpp +++ b/radio/src/serial.cpp @@ -237,7 +237,7 @@ static void serialSetupPort(int mode, etx_serial_init& params) break; case UART_MODE_SBUS_TRAINER: - params.baudrate = SBUS_BAUDRATE; + params.baudrate = SBus::baudrate; params.word_length = ETX_WordLength_9; params.parity = ETX_Parity_Even; params.stop_bits = ETX_StopBits_Two; @@ -245,7 +245,7 @@ static void serialSetupPort(int mode, etx_serial_init& params) break; case UART_MODE_IBUS_TRAINER: - params.baudrate = IBUS_BAUDRATE; + params.baudrate = IBus::baudrate; params.word_length = ETX_WordLength_8; params.parity = ETX_Parity_None; params.stop_bits = ETX_StopBits_One; @@ -254,7 +254,7 @@ static void serialSetupPort(int mode, etx_serial_init& params) break; case UART_MODE_CRSF_TRAINER: - params.baudrate = CRSF_BAUDRATE; + params.baudrate = CRSF::baudrate; params.word_length = ETX_WordLength_8; params.parity = ETX_Parity_None; params.stop_bits = ETX_StopBits_One; @@ -263,7 +263,7 @@ static void serialSetupPort(int mode, etx_serial_init& params) break; case UART_MODE_SUMD_TRAINER: - params.baudrate = SUMD_BAUDRATE; + params.baudrate = SumDV3::baudrate; params.word_length = ETX_WordLength_8; params.parity = ETX_Parity_None; params.stop_bits = ETX_StopBits_One; diff --git a/radio/src/sumd.cpp b/radio/src/sumd.cpp index 26c7a71538b..7786826bcf0 100644 --- a/radio/src/sumd.cpp +++ b/radio/src/sumd.cpp @@ -1,264 +1,30 @@ #include "sumd.h" -#include "opentx.h" -#include "trainer.h" -#include "switches.h" - -#include -#include - -#ifdef __GNUC__ -# pragma GCC diagnostic push -# pragma GCC diagnostic error "-Wswitch" // unfortunately the project uses -Wnoswitch -#endif namespace SumDV3 { - struct Crc16 { - void reset() { - sum = 0; - } - void operator+=(const uint8_t v) { - sum = sum ^ (((uint16_t)v) << 8); - for(uint8_t i = 0; i < 8; ++i) { - if (sum & 0x8000) { - sum = (sum << 1) ^ crc_polynome; - } - else { - sum = (sum << 1); - } - } - } - operator uint16_t() const { - return sum; - } - private: - static constexpr uint16_t crc_polynome = 0x1021; - uint16_t sum{}; - }; - - struct Servo { - using SumDV3 = Trainer::Protocol::SumDV3; - using MesgType = SumDV3::MesgType; - using SwitchesType = SumDV3::SwitchesType; - - enum class State : uint8_t {Undefined, GotStart, StartV1, StartV3, V1ChannelDataH, V1ChannelDataL, CrcH, CrcL, - V3ChannelDataH, V3ChannelDataL, V3FuncCode, V3LastValidPackage, V3ModeCmd, V3SubCmd}; - - enum class Frame : uint8_t {Ch1to12 = 0x00, First = Ch1to12, - Ch1to8and13to16 = 0x01, Ch1to16 = 0x02, Ch1to8and17to24 = 0x03, - Ch1to8and25to32 = 0x04, Ch1to12and64Switches = 0x05, - Last = Ch1to12and64Switches, - Undefined = 0xff}; - - - static inline int16_t convertSumdToPuls(uint16_t const sumdValue) { - return Trainer::clamp(sumdValue - SumDV3::CenterValue); - } - - static inline void process(const uint8_t b, const std::function f) { - switch(mState) { // enum-switch -> no default (intentional) - case State::Undefined: - if (b == SumDV3::start_code) { - csum.reset(); - csum += b; - mState = State::GotStart; - } - break; - case State::GotStart: - csum += b; - if ((b & 0x0f) == SumDV3::version_code1) { - mState = State::StartV1; - } - else if ((b & 0x0f) == SumDV3::version_code3) { - mState = State::StartV3; - } - else { - mState = State::Undefined; - } - break; - case State::StartV1: - if ((b >= 2) && (b <= 32)) { - csum += b; - nChannels = b; - mIndex = 0; - mState = State::V1ChannelDataH; - } - else { - mState = State::Undefined; - } - break; - case State::V1ChannelDataH: - csum += b; - sumdFrame[mIndex].first = b; - mState = State::V1ChannelDataL; - break; - case State::V1ChannelDataL: - csum += b; - sumdFrame[mIndex].second = b; - ++mIndex; - if (mIndex < nChannels) { - mState = State::V1ChannelDataH; - } - else { - mState = State::CrcH; - frame = Frame::Ch1to16; - } - break; - case State::CrcH: - crcH = b; - mState = State::CrcL; - break; - case State::CrcL: - if ((((uint16_t)crcH << 8) | b) == csum) { - ++mPackagesCounter; - f(); - } - mState = State::Undefined; - break; - case State::StartV3: - if ((b >= 2) && (b <= 68)) { - csum += b; - nChannels = b - 2; - mIndex = 0; - mState = State::V3ChannelDataH; - } - else { - mState = State::Undefined; - } - break; - case State::V3ChannelDataH: - csum += b; - sumdFrame[mIndex].first = b; - mState = State::V3ChannelDataL; - break; - case State::V3ChannelDataL: - csum += b; - sumdFrame[mIndex].second = b; - ++mIndex; - if (mIndex < nChannels) { - mState = State::V3ChannelDataH; - } - else { - mState = State::V3FuncCode; - } - break; - case State::V3FuncCode: - csum += b; - if ((b >= uint8_t(Frame::First)) && (b <= uint8_t(Frame::Last))) { - frame = Frame(b); - } - else { - frame = Frame::Undefined; - } - mState = State::V3LastValidPackage; - break; - case State::V3LastValidPackage: - csum += b; - mState = State::V3ModeCmd; - break; - case State::V3ModeCmd: - csum += b; - mState = State::V3SubCmd; - break; - case State::V3SubCmd: - csum += b; - mState = State::CrcH; - break; - } - } - - template struct range_t {}; - - template - using offset_t = std::integral_constant; - - template - static inline void extract(const range_t&, int16_t (&dest)[N], offset_t = offset_t<0>{}) { - static_assert((End - Begin) < (N - Off), "wrong range or target size"); - - } - - static inline void sumSwitches() { - uint64_t sw = sumdFrame[12].first; - sw = (sw << 8) | sumdFrame[12].second; - sw = (sw << 8) | sumdFrame[13].first; - sw = (sw << 8) | sumdFrame[13].second; - sw = (sw << 8) | sumdFrame[14].first; - sw = (sw << 8) | sumdFrame[14].second; - sw = (sw << 8) | sumdFrame[15].first; - sw = (sw << 8) | sumdFrame[15].second; - - for (uint8_t i = 0; i < MAX_LOGICAL_SWITCHES; ++i) { - const uint64_t mask = (1 << i); - if (sw & mask) { - rawSetUnconnectedStickySwitch(i, true); - } - else { - rawSetUnconnectedStickySwitch(i, false); - } - } - } - - template - static inline void convert(int16_t (&pulses)[N]) { - switch(frame) { - case Frame::Ch1to12: - extract(range_t<0, 11>{}, pulses); - break; - case Frame::Ch1to8and13to16: - extract(range_t<0, 7>{}, pulses); - extract(range_t<8, 11>{}, pulses, offset_t<12>{}); - break; - case Frame::Ch1to8and17to24: - extract(range_t<0, 7>{}, pulses); - if constexpr(N > 16) { - extract(range_t<8, 15>{}, pulses, offset_t<16>{}); - } - break; - case Frame::Ch1to8and25to32: - extract(range_t<0, 7>{}, pulses); - if constexpr(N > 16) { - extract(range_t<8, 15>{}, pulses, offset_t<24>{}); - } - break; - case Frame::Ch1to16: - extract(range_t<0, 15>{}, pulses); - break; - case Frame::Ch1to12and64Switches: - extract(range_t<0, 11>{}, pulses); - sumSwitches(); - break; - case Frame::Undefined: - break; - } - } - private: - static uint8_t nChannels; - static Crc16 csum; - static uint8_t crcH; - - static State mState; - static MesgType sumdFrame; - static uint8_t mIndex; - static uint16_t mPackagesCounter; - - static Frame frame; - static uint8_t reserved; - static uint8_t mode_cmd; - static uint8_t sub_cmd; - }; - uint8_t Servo::nChannels{}; - Crc16 Servo::csum{}; - uint8_t Servo::crcH{}; + template + uint8_t Servo::nChannels{}; + template + Crc16 Servo::csum{}; + template + uint8_t Servo::crcH{}; - Servo::State Servo::mState{Servo::State::Undefined}; - Servo::MesgType Servo::sumdFrame; - uint8_t Servo::mIndex{}; - uint16_t Servo::mPackagesCounter{}; - - Servo::Frame Servo::frame{Servo::Frame::Undefined}; - uint8_t Servo::reserved{}; - uint8_t Servo::mode_cmd{}; - uint8_t Servo::sub_cmd{}; + template + typename Servo::State Servo::mState{Servo::State::Undefined}; + template + typename Servo::MesgType Servo::sumdFrame; + template + uint8_t Servo::mIndex{}; + template + uint16_t Servo::mPackagesCounter{}; + + template + typename Servo::Frame Servo::frame{Servo::Frame::Undefined}; + template + uint8_t Servo::reserved{}; + template + uint8_t Servo::mode_cmd{}; + template + uint8_t Servo::sub_cmd{}; } void processSumdInput() { @@ -266,14 +32,10 @@ void processSumdInput() { uint8_t rxchar; while (sbusAuxGetByte(&rxchar)) { - SumDV3::Servo::process(rxchar, [&](){ - SumDV3::Servo::convert(ppmInput); + SumDV3::Servo<0>::process(rxchar, [&](){ + SumDV3::Servo<0>::convert(ppmInput); ppmInputValidityTimer = PPM_IN_VALID_TIMEOUT; }); } #endif } - -#ifdef __GNUC__ -# pragma GCC diagnostic pop -#endif diff --git a/radio/src/sumd.h b/radio/src/sumd.h index 4a17db6948c..5f1a8f7785e 100644 --- a/radio/src/sumd.h +++ b/radio/src/sumd.h @@ -1,6 +1,273 @@ #pragma once +#include "opentx.h" +#include "trainer.h" +#include "switches.h" -#define SUMD_BAUDRATE 115200 +#include +#include void processSumdInput(); +#ifdef __GNUC__ +# pragma GCC diagnostic push +# pragma GCC diagnostic error "-Wswitch" // unfortunately the project uses -Wnoswitch +#endif + +namespace SumDV3 { + static constexpr uint32_t baudrate{115200}; + + struct Crc16 { + inline void reset() { + sum = 0; + } + inline void operator+=(const uint8_t v) { + sum = sum ^ (((uint16_t)v) << 8); + for(uint8_t i = 0; i < 8; ++i) { + if (sum & 0x8000) { + sum = (sum << 1) ^ crc_polynome; + } + else { + sum = (sum << 1); + } + } + } + inline operator uint16_t() const { + return sum; + } + private: + static constexpr uint16_t crc_polynome = 0x1021; + uint16_t sum{}; + }; + + template + struct Servo { + using SumDV3 = Trainer::Protocol::SumDV3; + using MesgType = SumDV3::MesgType; + using SwitchesType = SumDV3::SwitchesType; + + enum class State : uint8_t {Undefined, GotStart, StartV1, StartV3, V1ChannelDataH, V1ChannelDataL, CrcH, CrcL, + V3ChannelDataH, V3ChannelDataL, V3FuncCode, V3LastValidPackage, V3ModeCmd, V3SubCmd}; + + enum class Frame : uint8_t {Ch1to12 = 0x00, First = Ch1to12, + Ch1to8and13to16 = 0x01, Ch1to16 = 0x02, Ch1to8and17to24 = 0x03, + Ch1to8and25to32 = 0x04, Ch1to12and64Switches = 0x05, + Last = Ch1to12and64Switches, + Undefined = 0xff}; + + + static inline int16_t convertSumdToPuls(uint16_t const value) { + const int32_t centered = value - SumDV3::CenterValue; + return Trainer::clamp(((Trainer::MaxValue - Trainer::MinValue) * centered) / (SumDV3::MaxValue -SumDV3::MinValue)); + } + + static inline void process(const uint8_t b, const std::function f) { + switch(mState) { // enum-switch -> no default (intentional) + case State::Undefined: + if (b == SumDV3::start_code) { + csum.reset(); + csum += b; + mState = State::GotStart; + } + break; + case State::GotStart: + csum += b; + if ((b & 0x0f) == SumDV3::version_code1) { + mState = State::StartV1; + } + else if ((b & 0x0f) == SumDV3::version_code3) { + mState = State::StartV3; + } + else { + mState = State::Undefined; + } + break; + case State::StartV1: + if ((b >= 2) && (b <= 32)) { + csum += b; + nChannels = b; + mIndex = 0; + mState = State::V1ChannelDataH; + } + else { + mState = State::Undefined; + } + break; + case State::V1ChannelDataH: + csum += b; + sumdFrame[mIndex].first = b; + mState = State::V1ChannelDataL; + break; + case State::V1ChannelDataL: + csum += b; + sumdFrame[mIndex].second = b; + ++mIndex; + if (mIndex < nChannels) { + mState = State::V1ChannelDataH; + } + else { + mState = State::CrcH; + frame = Frame::Ch1to16; + } + break; + case State::CrcH: + crcH = b; + mState = State::CrcL; + break; + case State::CrcL: + if ((((uint16_t)crcH << 8) | b) == csum) { + ++mPackagesCounter; + f(); + } + mState = State::Undefined; + break; + case State::StartV3: + if ((b >= 2) && (b <= 68)) { + csum += b; + nChannels = b - 2; + mIndex = 0; + mState = State::V3ChannelDataH; + } + else { + mState = State::Undefined; + } + break; + case State::V3ChannelDataH: + csum += b; + sumdFrame[mIndex].first = b; + mState = State::V3ChannelDataL; + break; + case State::V3ChannelDataL: + csum += b; + sumdFrame[mIndex].second = b; + ++mIndex; + if (mIndex < nChannels) { + mState = State::V3ChannelDataH; + } + else { + mState = State::V3FuncCode; + } + break; + case State::V3FuncCode: + csum += b; + if ((b >= uint8_t(Frame::First)) && (b <= uint8_t(Frame::Last))) { + frame = Frame(b); + } + else { + frame = Frame::Undefined; + } + mState = State::V3LastValidPackage; + break; + case State::V3LastValidPackage: + csum += b; + mState = State::V3ModeCmd; + break; + case State::V3ModeCmd: + csum += b; + mState = State::V3SubCmd; + break; + case State::V3SubCmd: + csum += b; + mState = State::CrcH; + break; + } + } + + template struct range_t {}; + + template + using offset_t = std::integral_constant; + + // uses tag-dispatch because no constexpr-if in c++11 + template + static inline void extract(const range_t&, int16_t (&dest)[N], offset_t = offset_t<0>{}, std::true_type = std::true_type{}) { + static_assert((End - Begin) < (N - Off), "wrong range or target size"); + uint8_t out{Off}; + for(uint8_t i = Begin; i <= End; ++i) { + uint16_t raw = (sumdFrame[i].first << 8) | sumdFrame[i].second; + dest[out++] = convertSumdToPuls(raw); + } + } + template + static inline void extract(const range_t&, int16_t (&dest)[N], offset_t, std::false_type) { + } + + static inline void sumSwitches() { + uint64_t sw = sumdFrame[12].first; + sw = (sw << 8) | sumdFrame[12].second; + sw = (sw << 8) | sumdFrame[13].first; + sw = (sw << 8) | sumdFrame[13].second; + sw = (sw << 8) | sumdFrame[14].first; + sw = (sw << 8) | sumdFrame[14].second; + sw = (sw << 8) | sumdFrame[15].first; + sw = (sw << 8) | sumdFrame[15].second; + + for (uint8_t i = 0; i < MAX_LOGICAL_SWITCHES; ++i) { + const uint64_t mask = (1 << i); + if (sw & mask) { + rawSetUnconnectedStickySwitch(i, true); + } + else { + rawSetUnconnectedStickySwitch(i, false); + } + } + } + + template + static inline void convert(int16_t (&pulses)[N]) { + static_assert(N >= 16, "array too small"); + switch(frame) { + case Frame::Ch1to12: + extract(range_t<0, 11>{}, pulses); + break; + case Frame::Ch1to8and13to16: + extract(range_t<0, 7>{}, pulses); + extract(range_t<8, 11>{}, pulses, offset_t<12>{}); + break; + case Frame::Ch1to8and17to24: + extract(range_t<0, 7>{}, pulses); + extract(range_t<8, 15>{}, pulses, offset_t<16>{}, std::integral_constant 16) >{}); // no constepr-if in c++11 +// if constexpr(N > 16) { +// extract(range_t<8, 15>{}, pulses, offset_t<16>{}); +// } + break; + case Frame::Ch1to8and25to32: + extract(range_t<0, 7>{}, pulses); + extract(range_t<8, 15>{}, pulses, offset_t<24>{}, std::integral_constant 16) >{}); // no constepr-if in c++11 +// if constexpr(N > 16) { +// extract(range_t<8, 15>{}, pulses, offset_t<24>{}); +// } + break; + case Frame::Ch1to16: + extract(range_t<0, 15>{}, pulses); + break; + case Frame::Ch1to12and64Switches: + extract(range_t<0, 11>{}, pulses); + sumSwitches(); + break; + case Frame::Undefined: + break; + } + } + static inline uint16_t packages() { + return mPackagesCounter; + } + private: + static uint8_t nChannels; + static Crc16 csum; + static uint8_t crcH; + + static State mState; + static MesgType sumdFrame; + static uint8_t mIndex; + static uint16_t mPackagesCounter; + + static Frame frame; + static uint8_t reserved; + static uint8_t mode_cmd; + static uint8_t sub_cmd; + }; +} + +#ifdef __GNUC__ +# pragma GCC diagnostic pop +#endif diff --git a/radio/src/switches.cpp b/radio/src/switches.cpp index 9726ba16de4..adb26de8280 100644 --- a/radio/src/switches.cpp +++ b/radio/src/switches.cpp @@ -1020,6 +1020,15 @@ void logicalSwitchesCopyState(uint8_t src, uint8_t dst) lswFm[dst] = lswFm[src]; } -void rawSetUnconnectedStickySwitch(const uint8_t, const bool) { - +void rawSetUnconnectedStickySwitch(const uint8_t i, const bool state) { + if (i >= MAX_LOGICAL_SWITCHES) return; + LogicalSwitchData* const ls = lswAddress(i); + if (ls->func == LS_FUNC_STICKY) { + if ((ls->v1 != SWSRC_NONE) && (ls->v2 != SWSRC_NONE)) { + for (uint8_t fm = 0; fm < MAX_FLIGHT_MODES; fm++) { + ls_sticky_struct& lastValue = (ls_sticky_struct &)LS_LAST_VALUE(fm, i); + lastValue.state = state; + } + } + } } From 67ab6778a46f99ded8ba3c5a7c81deae8a2f1486 Mon Sep 17 00:00:00 2001 From: wimalopaan Date: Wed, 6 Apr 2022 06:53:10 +0200 Subject: [PATCH 21/31] fixed taranis problems --- radio/src/pulses/pulses.h | 2 +- radio/src/targets/taranis/trainer_driver.cpp | 96 ++++++++++---------- 2 files changed, 49 insertions(+), 49 deletions(-) diff --git a/radio/src/pulses/pulses.h b/radio/src/pulses/pulses.h index b1cf5bfc9d6..48126248c8e 100644 --- a/radio/src/pulses/pulses.h +++ b/radio/src/pulses/pulses.h @@ -125,7 +125,7 @@ typedef Dsm2TimerPulsesData Dsm2PulsesData; #define PPM_PERIOD(module) (PPM_PERIOD_HALF_US(module) / 2) /*us*/ #define DSM2_BAUDRATE 125000 #define DSM2_PERIOD 22000 /*us*/ -//#define SBUS_BAUDRATE 100000 +#define SBUS_BAUDRATE 100000 #define SBUS_MIN_PERIOD 60 /*6.0ms 1/10ms*/ #define SBUS_MAX_PERIOD 325 /*Overflows uint16_t if set higher*/ #define SBUS_DEF_PERIOD 225 diff --git a/radio/src/targets/taranis/trainer_driver.cpp b/radio/src/targets/taranis/trainer_driver.cpp index 5c6d3fd28eb..7471ba43661 100644 --- a/radio/src/targets/taranis/trainer_driver.cpp +++ b/radio/src/targets/taranis/trainer_driver.cpp @@ -317,54 +317,54 @@ void init_trainer_module_sbus() DMA_Cmd(TRAINER_MODULE_SBUS_DMA_STREAM, ENABLE); } -void init_trainer_module_ibus() -{ - EXTERNAL_MODULE_ON(); - - USART_InitTypeDef USART_InitStructure; - GPIO_InitTypeDef GPIO_InitStructure; - - GPIO_PinAFConfig(TRAINER_MODULE_SBUS_GPIO, TRAINER_MODULE_SBUS_GPIO_PinSource, TRAINER_MODULE_SBUS_GPIO_AF); - - GPIO_InitStructure.GPIO_Pin = TRAINER_MODULE_SBUS_GPIO_PIN; - GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF; - GPIO_InitStructure.GPIO_OType = GPIO_OType_PP; - GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_UP; - GPIO_InitStructure.GPIO_Speed = GPIO_Speed_2MHz; - GPIO_Init(TRAINER_MODULE_SBUS_GPIO, &GPIO_InitStructure); - - USART_InitStructure.USART_BaudRate = IBUS_BAUDRATE; - USART_InitStructure.USART_WordLength = USART_WordLength_8b; - USART_InitStructure.USART_StopBits = USART_StopBits_1; - USART_InitStructure.USART_Parity = USART_Parity_No; - USART_InitStructure.USART_HardwareFlowControl = USART_HardwareFlowControl_None; - USART_InitStructure.USART_Mode = USART_Mode_Rx; - USART_Init(TRAINER_MODULE_SBUS_USART, &USART_InitStructure); - - DMA_InitTypeDef DMA_InitStructure; - trainerSbusFifo.clear(); - USART_ITConfig(TRAINER_MODULE_SBUS_USART, USART_IT_RXNE, DISABLE); - USART_ITConfig(TRAINER_MODULE_SBUS_USART, USART_IT_TXE, DISABLE); - DMA_InitStructure.DMA_Channel = TRAINER_MODULE_SBUS_DMA_CHANNEL; - DMA_InitStructure.DMA_PeripheralBaseAddr = CONVERT_PTR_UINT(&TRAINER_MODULE_SBUS_USART->DR); - DMA_InitStructure.DMA_Memory0BaseAddr = CONVERT_PTR_UINT(trainerSbusFifo.buffer()); - DMA_InitStructure.DMA_DIR = DMA_DIR_PeripheralToMemory; - DMA_InitStructure.DMA_BufferSize = trainerSbusFifo.size(); - DMA_InitStructure.DMA_PeripheralInc = DMA_PeripheralInc_Disable; - DMA_InitStructure.DMA_MemoryInc = DMA_MemoryInc_Enable; - DMA_InitStructure.DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte; - DMA_InitStructure.DMA_MemoryDataSize = DMA_MemoryDataSize_Byte; - DMA_InitStructure.DMA_Mode = DMA_Mode_Circular; - DMA_InitStructure.DMA_Priority = DMA_Priority_Low; - DMA_InitStructure.DMA_FIFOMode = DMA_FIFOMode_Disable; - DMA_InitStructure.DMA_FIFOThreshold = DMA_FIFOThreshold_Full; - DMA_InitStructure.DMA_MemoryBurst = DMA_MemoryBurst_Single; - DMA_InitStructure.DMA_PeripheralBurst = DMA_PeripheralBurst_Single; - DMA_Init(TRAINER_MODULE_SBUS_DMA_STREAM, &DMA_InitStructure); - USART_DMACmd(TRAINER_MODULE_SBUS_USART, USART_DMAReq_Rx, ENABLE); - USART_Cmd(TRAINER_MODULE_SBUS_USART, ENABLE); - DMA_Cmd(TRAINER_MODULE_SBUS_DMA_STREAM, ENABLE); -} +//void init_trainer_module_ibus() +//{ +// EXTERNAL_MODULE_ON(); + +// USART_InitTypeDef USART_InitStructure; +// GPIO_InitTypeDef GPIO_InitStructure; + +// GPIO_PinAFConfig(TRAINER_MODULE_SBUS_GPIO, TRAINER_MODULE_SBUS_GPIO_PinSource, TRAINER_MODULE_SBUS_GPIO_AF); + +// GPIO_InitStructure.GPIO_Pin = TRAINER_MODULE_SBUS_GPIO_PIN; +// GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF; +// GPIO_InitStructure.GPIO_OType = GPIO_OType_PP; +// GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_UP; +// GPIO_InitStructure.GPIO_Speed = GPIO_Speed_2MHz; +// GPIO_Init(TRAINER_MODULE_SBUS_GPIO, &GPIO_InitStructure); + +// USART_InitStructure.USART_BaudRate = IBUS_BAUDRATE; +// USART_InitStructure.USART_WordLength = USART_WordLength_8b; +// USART_InitStructure.USART_StopBits = USART_StopBits_1; +// USART_InitStructure.USART_Parity = USART_Parity_No; +// USART_InitStructure.USART_HardwareFlowControl = USART_HardwareFlowControl_None; +// USART_InitStructure.USART_Mode = USART_Mode_Rx; +// USART_Init(TRAINER_MODULE_SBUS_USART, &USART_InitStructure); + +// DMA_InitTypeDef DMA_InitStructure; +// trainerSbusFifo.clear(); +// USART_ITConfig(TRAINER_MODULE_SBUS_USART, USART_IT_RXNE, DISABLE); +// USART_ITConfig(TRAINER_MODULE_SBUS_USART, USART_IT_TXE, DISABLE); +// DMA_InitStructure.DMA_Channel = TRAINER_MODULE_SBUS_DMA_CHANNEL; +// DMA_InitStructure.DMA_PeripheralBaseAddr = CONVERT_PTR_UINT(&TRAINER_MODULE_SBUS_USART->DR); +// DMA_InitStructure.DMA_Memory0BaseAddr = CONVERT_PTR_UINT(trainerSbusFifo.buffer()); +// DMA_InitStructure.DMA_DIR = DMA_DIR_PeripheralToMemory; +// DMA_InitStructure.DMA_BufferSize = trainerSbusFifo.size(); +// DMA_InitStructure.DMA_PeripheralInc = DMA_PeripheralInc_Disable; +// DMA_InitStructure.DMA_MemoryInc = DMA_MemoryInc_Enable; +// DMA_InitStructure.DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte; +// DMA_InitStructure.DMA_MemoryDataSize = DMA_MemoryDataSize_Byte; +// DMA_InitStructure.DMA_Mode = DMA_Mode_Circular; +// DMA_InitStructure.DMA_Priority = DMA_Priority_Low; +// DMA_InitStructure.DMA_FIFOMode = DMA_FIFOMode_Disable; +// DMA_InitStructure.DMA_FIFOThreshold = DMA_FIFOThreshold_Full; +// DMA_InitStructure.DMA_MemoryBurst = DMA_MemoryBurst_Single; +// DMA_InitStructure.DMA_PeripheralBurst = DMA_PeripheralBurst_Single; +// DMA_Init(TRAINER_MODULE_SBUS_DMA_STREAM, &DMA_InitStructure); +// USART_DMACmd(TRAINER_MODULE_SBUS_USART, USART_DMAReq_Rx, ENABLE); +// USART_Cmd(TRAINER_MODULE_SBUS_USART, ENABLE); +// DMA_Cmd(TRAINER_MODULE_SBUS_DMA_STREAM, ENABLE); +//} void stop_trainer_module_sbus() { From 34cd38a5522c67a1aa6d3691893c788a2f967aeb Mon Sep 17 00:00:00 2001 From: wimalopaan Date: Wed, 6 Apr 2022 19:47:44 +0200 Subject: [PATCH 22/31] Now also SumDV3 fully functional and working --- radio/src/dataconstants.h | 2 +- radio/src/sumd.cpp | 2 + radio/src/sumd.h | 95 +++++++++++++++++++-------------------- radio/src/switches.cpp | 10 ++--- radio/src/trainer.h | 1 - 5 files changed, 52 insertions(+), 58 deletions(-) diff --git a/radio/src/dataconstants.h b/radio/src/dataconstants.h index c6b2760cd55..d55f876512c 100644 --- a/radio/src/dataconstants.h +++ b/radio/src/dataconstants.h @@ -50,7 +50,7 @@ #define MIN_TRAINER_CHANNELS 4 #define DEF_TRAINER_CHANNELS 8 #if defined(EXTENDED_TRAINER) - #define MAX_TRAINER_CHANNELS 16 + #define MAX_TRAINER_CHANNELS 32 #else #define MAX_TRAINER_CHANNELS 16 #endif diff --git a/radio/src/sumd.cpp b/radio/src/sumd.cpp index 7786826bcf0..b6abdc9c78e 100644 --- a/radio/src/sumd.cpp +++ b/radio/src/sumd.cpp @@ -16,6 +16,8 @@ namespace SumDV3 { uint8_t Servo::mIndex{}; template uint16_t Servo::mPackagesCounter{}; + template + uint16_t Servo::mBytesCounter{}; template typename Servo::Frame Servo::frame{Servo::Frame::Undefined}; diff --git a/radio/src/sumd.h b/radio/src/sumd.h index 5f1a8f7785e..2e0b55a9bfe 100644 --- a/radio/src/sumd.h +++ b/radio/src/sumd.h @@ -44,6 +44,9 @@ namespace SumDV3 { using SumDV3 = Trainer::Protocol::SumDV3; using MesgType = SumDV3::MesgType; using SwitchesType = SumDV3::SwitchesType; + + template struct range_t {}; + template using offset_t = std::integral_constant; enum class State : uint8_t {Undefined, GotStart, StartV1, StartV3, V1ChannelDataH, V1ChannelDataL, CrcH, CrcL, V3ChannelDataH, V3ChannelDataL, V3FuncCode, V3LastValidPackage, V3ModeCmd, V3SubCmd}; @@ -54,13 +57,8 @@ namespace SumDV3 { Last = Ch1to12and64Switches, Undefined = 0xff}; - - static inline int16_t convertSumdToPuls(uint16_t const value) { - const int32_t centered = value - SumDV3::CenterValue; - return Trainer::clamp(((Trainer::MaxValue - Trainer::MinValue) * centered) / (SumDV3::MaxValue -SumDV3::MinValue)); - } - static inline void process(const uint8_t b, const std::function f) { + ++mBytesCounter; switch(mState) { // enum-switch -> no default (intentional) case State::Undefined: if (b == SumDV3::start_code) { @@ -114,7 +112,7 @@ namespace SumDV3 { mState = State::CrcL; break; case State::CrcL: - if ((((uint16_t)crcH << 8) | b) == csum) { + if (((((uint16_t)crcH) << 8) | b) == csum) { ++mPackagesCounter; f(); } @@ -172,46 +170,6 @@ namespace SumDV3 { } } - template struct range_t {}; - - template - using offset_t = std::integral_constant; - - // uses tag-dispatch because no constexpr-if in c++11 - template - static inline void extract(const range_t&, int16_t (&dest)[N], offset_t = offset_t<0>{}, std::true_type = std::true_type{}) { - static_assert((End - Begin) < (N - Off), "wrong range or target size"); - uint8_t out{Off}; - for(uint8_t i = Begin; i <= End; ++i) { - uint16_t raw = (sumdFrame[i].first << 8) | sumdFrame[i].second; - dest[out++] = convertSumdToPuls(raw); - } - } - template - static inline void extract(const range_t&, int16_t (&dest)[N], offset_t, std::false_type) { - } - - static inline void sumSwitches() { - uint64_t sw = sumdFrame[12].first; - sw = (sw << 8) | sumdFrame[12].second; - sw = (sw << 8) | sumdFrame[13].first; - sw = (sw << 8) | sumdFrame[13].second; - sw = (sw << 8) | sumdFrame[14].first; - sw = (sw << 8) | sumdFrame[14].second; - sw = (sw << 8) | sumdFrame[15].first; - sw = (sw << 8) | sumdFrame[15].second; - - for (uint8_t i = 0; i < MAX_LOGICAL_SWITCHES; ++i) { - const uint64_t mask = (1 << i); - if (sw & mask) { - rawSetUnconnectedStickySwitch(i, true); - } - else { - rawSetUnconnectedStickySwitch(i, false); - } - } - } - template static inline void convert(int16_t (&pulses)[N]) { static_assert(N >= 16, "array too small"); @@ -251,16 +209,55 @@ namespace SumDV3 { static inline uint16_t packages() { return mPackagesCounter; } + static inline uint16_t getbytes() { + return mBytesCounter; + } private: + // uses tag-dispatch because no constexpr-if in c++11 + template + static inline void extract(const range_t&, int16_t (&dest)[N], offset_t = offset_t<0>{}, std::true_type = std::true_type{}) { + static_assert((End - Begin) < (N - Off), "wrong range or target size"); + uint8_t out{Off}; + for(uint8_t i = Begin; i <= End; ++i) { + uint16_t raw = (sumdFrame[i].first << 8) | sumdFrame[i].second; + dest[out++] = convertSumdToPuls(raw); + } + } + template + static inline void extract(const range_t&, int16_t (&dest)[N], offset_t, std::false_type) { + } + static inline void sumSwitches() { + uint64_t sw = sumdFrame[12].first; + sw = (sw << 8) | sumdFrame[12].second; + sw = (sw << 8) | sumdFrame[13].first; + sw = (sw << 8) | sumdFrame[13].second; + sw = (sw << 8) | sumdFrame[14].first; + sw = (sw << 8) | sumdFrame[14].second; + sw = (sw << 8) | sumdFrame[15].first; + sw = (sw << 8) | sumdFrame[15].second; + + for (uint8_t i = 0; i < MAX_LOGICAL_SWITCHES; ++i) { + const uint64_t mask = (((uint64_t)0x01) << i); + if (sw & mask) { + rawSetUnconnectedStickySwitch(i, true); + } + else { + rawSetUnconnectedStickySwitch(i, false); + } + } + } + static inline int16_t convertSumdToPuls(uint16_t const value) { + const int32_t centered = value - SumDV3::CenterValue; + return Trainer::clamp(((Trainer::MaxValue - Trainer::MinValue) * centered) / (SumDV3::MaxValue -SumDV3::MinValue)); + } static uint8_t nChannels; static Crc16 csum; static uint8_t crcH; - static State mState; static MesgType sumdFrame; static uint8_t mIndex; static uint16_t mPackagesCounter; - + static uint16_t mBytesCounter; static Frame frame; static uint8_t reserved; static uint8_t mode_cmd; diff --git a/radio/src/switches.cpp b/radio/src/switches.cpp index adb26de8280..7599b88d3d5 100644 --- a/radio/src/switches.cpp +++ b/radio/src/switches.cpp @@ -1023,12 +1023,8 @@ void logicalSwitchesCopyState(uint8_t src, uint8_t dst) void rawSetUnconnectedStickySwitch(const uint8_t i, const bool state) { if (i >= MAX_LOGICAL_SWITCHES) return; LogicalSwitchData* const ls = lswAddress(i); - if (ls->func == LS_FUNC_STICKY) { - if ((ls->v1 != SWSRC_NONE) && (ls->v2 != SWSRC_NONE)) { - for (uint8_t fm = 0; fm < MAX_FLIGHT_MODES; fm++) { - ls_sticky_struct& lastValue = (ls_sticky_struct &)LS_LAST_VALUE(fm, i); - lastValue.state = state; - } - } + if ((ls) && (ls->func == LS_FUNC_STICKY) && (ls->v1 == SWSRC_NONE) && (ls->v2 == SWSRC_NONE)) { + ls_sticky_struct& lastValue = (ls_sticky_struct &)LS_LAST_VALUE(mixerCurrentFlightMode, i); + lastValue.state = state; } } diff --git a/radio/src/trainer.h b/radio/src/trainer.h index 3bb60f70969..a9c1f6a4b55 100644 --- a/radio/src/trainer.h +++ b/radio/src/trainer.h @@ -87,7 +87,6 @@ namespace Trainer { using MesgType = std::array, MaxChannels>; using SwitchesType = uint64_t; - }; } } From aa1a4d92c9658dbce4fee77e079dfdf88593511d Mon Sep 17 00:00:00 2001 From: wimalopaan Date: Fri, 8 Apr 2022 13:17:32 +0200 Subject: [PATCH 23/31] * refactored sbus/crsf/ibus/sumd v1/v3 trainer input * added code to disable BT-Module (put !EN-pin to high) when AUX2 is powered on * added some debug output (statistics of incoming packets) * added sumd V3 decoding of mode-command/sub-command * added lua functions to query the sumd V3 commands --- radio/src/crsf.cpp | 4 +++- radio/src/crsf.h | 11 ++++++++--- radio/src/ibus.cpp | 2 ++ radio/src/ibus.h | 5 +++++ radio/src/lua/api_general.cpp | 18 ++++++++++++++++++ radio/src/pulses/pulses.h | 1 + radio/src/sbus.cpp | 5 +++-- radio/src/sbus.h | 18 +++++++++--------- radio/src/serial.cpp | 21 +++++++++++++++++++-- radio/src/sumd.cpp | 3 +++ radio/src/sumd.h | 21 +++++++++++++++++++-- radio/src/tasks.cpp | 14 ++++++++++++++ radio/src/trainer.h | 1 + 13 files changed, 105 insertions(+), 19 deletions(-) diff --git a/radio/src/crsf.cpp b/radio/src/crsf.cpp index 95e8a586478..68ff4f45c11 100644 --- a/radio/src/crsf.cpp +++ b/radio/src/crsf.cpp @@ -13,7 +13,9 @@ namespace CRSF { template uint8_t Servo::mLength{0}; template - uint16_t Servo::mPackages{0}; + uint16_t Servo::mPackagesCounter{0}; + template + uint16_t Servo::mBytesCounter{0}; template uint8_t Servo::mPauseCounter{Servo::mPauseCount}; // 2 ms } diff --git a/radio/src/crsf.h b/radio/src/crsf.h index 4f10d496723..12bd3752905 100644 --- a/radio/src/crsf.h +++ b/radio/src/crsf.h @@ -45,6 +45,7 @@ namespace CRSF { static inline void process(const uint8_t b, const std::function f) { mPauseCounter = mPauseCount; + ++mBytesCounter; switch(mState) { // enum-switch -> no default (intentional) case State::Undefined: csum.reset(); @@ -92,7 +93,7 @@ namespace CRSF { break; case State::AwaitCRCAndDecode: if (csum == b) { - ++mPackages; + ++mPackagesCounter; f(); } mState = State::Undefined; @@ -124,7 +125,10 @@ namespace CRSF { } } static inline uint16_t packages() { - return mPackages; + return mPackagesCounter; + } + static inline uint16_t getBytes() { + return mBytesCounter; } private: static CRC8 csum; @@ -132,7 +136,8 @@ namespace CRSF { static MesgType mData; static uint8_t mIndex; static uint8_t mLength; - static uint16_t mPackages; + static uint16_t mPackagesCounter; + static uint16_t mBytesCounter; static uint8_t mPauseCounter; }; } diff --git a/radio/src/ibus.cpp b/radio/src/ibus.cpp index 025fade1d13..484e841335d 100644 --- a/radio/src/ibus.cpp +++ b/radio/src/ibus.cpp @@ -11,6 +11,8 @@ namespace IBus { uint8_t Servo::mIndex{}; template uint16_t Servo::mPackagesCounter{}; + template + uint16_t Servo::mBytesCounter{}; } void processIbusInput() { diff --git a/radio/src/ibus.h b/radio/src/ibus.h index 8e795e9087b..cf45c7d3e9b 100644 --- a/radio/src/ibus.h +++ b/radio/src/ibus.h @@ -57,6 +57,7 @@ namespace IBus { } static inline void process(const uint8_t b, const std::function f) { + ++mBytesCounter; switch(mState) { // enum-switch -> no default (intentional) case State::Undefined: csum.reset(); @@ -119,12 +120,16 @@ namespace IBus { static inline uint16_t packages() { return mPackagesCounter; } + static inline uint16_t getBytes() { + return mBytesCounter; + } private: static CheckSum csum; static State mState; static MesgType ibusFrame; static uint8_t mIndex; static uint16_t mPackagesCounter; + static uint16_t mBytesCounter; }; } diff --git a/radio/src/lua/api_general.cpp b/radio/src/lua/api_general.cpp index 5b7357b5e30..6eea40e3050 100644 --- a/radio/src/lua/api_general.cpp +++ b/radio/src/lua/api_general.cpp @@ -2092,6 +2092,21 @@ static int luaGetShmVar(lua_State * L) } #endif +#if defined(EXTENDED_TRAINER) +static int luaGetSumDV3Command(lua_State * L) +{ + if (SumDV3::Servo<0>::hasCommand()) { + const Trainer::Protocol::SumDV3::Command_t command = SumDV3::Servo<0>::command(); + lua_pushunsigned(L, command.first); + lua_pushunsigned(L, command.second); + } + else { + lua_pushnil(L); + } + return 1; +} +#endif + /*luadoc @function setStickySwitch(id, value) @@ -2482,6 +2497,9 @@ const luaL_Reg opentxLib[] = { { "getSourceIndex", luaGetSourceIndex }, { "getSourceName", luaGetSourceName }, { "sources", luaSources }, +#if defined(EXTENDED_TRAINER) + { "getSumDV3Command", luaGetSumDV3Command }, +#endif { nullptr, nullptr } /* sentinel */ }; diff --git a/radio/src/pulses/pulses.h b/radio/src/pulses/pulses.h index 48126248c8e..956512b0d71 100644 --- a/radio/src/pulses/pulses.h +++ b/radio/src/pulses/pulses.h @@ -30,6 +30,7 @@ #include "multi.h" #include "afhds3.h" #include "afhds2.h" +#include "crossfire.h" #include "modules_helpers.h" #include "ff.h" #include "hal/module_driver.h" diff --git a/radio/src/sbus.cpp b/radio/src/sbus.cpp index d8b886cb2a3..8957ecced84 100644 --- a/radio/src/sbus.cpp +++ b/radio/src/sbus.cpp @@ -40,7 +40,6 @@ static void* _sbusAuxGetByteCtx = nullptr; void sbusSetAuxGetByte(void* ctx, bool (*fct)(void*, uint8_t*)) { - _sbusAuxGetByte = nullptr; _sbusAuxGetByteCtx = ctx; _sbusAuxGetByte = fct; } @@ -72,7 +71,9 @@ namespace SBus { template uint8_t Servo::mIndex{0}; template - uint16_t Servo::mPackages{0}; + uint16_t Servo::mPackagesCounter{0}; + template + uint16_t Servo::mBytesCounter{0}; template uint8_t Servo::mPauseCounter{Servo::mPauseCount}; // 2 ms } diff --git a/radio/src/sbus.h b/radio/src/sbus.h index 1031102641e..7138b0be3a1 100644 --- a/radio/src/sbus.h +++ b/radio/src/sbus.h @@ -24,7 +24,6 @@ #include "opentx.h" #include "trainer.h" - // Setup SBUS AUX serial input void sbusSetAuxGetByte(void* ctx, bool (*fct)(void*, uint8_t*)); @@ -33,7 +32,7 @@ void sbusSetAuxGetByte(void* ctx, bool (*fct)(void*, uint8_t*)); // with sbusSetAuxGetByte() bool sbusAuxGetByte(uint8_t* byte); -// Setup general SBUS input source +//// Setup general SBUS input source void sbusSetGetByte(bool (*fct)(uint8_t*)); void processSbusInput(); @@ -52,9 +51,6 @@ namespace SBus { using SBus = Trainer::Protocol::SBus; using MesgType = SBus::MesgType; -// static constexpr uint8_t frameSize{25}; -// static_assert(std::tuple_size::value == (SBUS_FRAME_SIZE - 2), "consistency check"); - enum class State : uint8_t {Undefined, Data, GotEnd, WaitEnd}; static constexpr uint8_t mPauseCount{2}; // 2ms @@ -74,6 +70,7 @@ namespace SBus { } static inline void process(const uint8_t b, const std::function f) { + ++mBytesCounter; mPauseCounter = mPauseCount; switch(mState) { // enum-switch -> no default (intentional) case State::Undefined: @@ -112,7 +109,7 @@ namespace SBus { uint8_t& statusByte = mData[mData.size() - 1]; // last byte if (!((statusByte & SBus::FrameLostMask) || (statusByte & SBus::FailSafeMask))) { f(); - ++mPackages; + ++mPackagesCounter; } } else { @@ -146,15 +143,18 @@ namespace SBus { pulses[i] = convertSbusToPuls(pulses[i]); } } - static inline uint16_t packages() { - return mPackages; + return mPackagesCounter; + } + static inline uint16_t getBytes() { + return mBytesCounter; } private: static State mState; static MesgType mData; static uint8_t mIndex; - static uint16_t mPackages; + static uint16_t mPackagesCounter; + static uint16_t mBytesCounter; static uint8_t mPauseCounter; }; } diff --git a/radio/src/serial.cpp b/radio/src/serial.cpp index 6c6f1d3c5de..9afef295a6d 100644 --- a/radio/src/serial.cpp +++ b/radio/src/serial.cpp @@ -175,8 +175,6 @@ static void serialSetCallBacks(int mode, void* ctx, const etx_serial_port_t* por case UART_MODE_CRSF_TRAINER: case UART_MODE_SUMD_TRAINER: sbusSetAuxGetByte(ctx, getByte); - // TODO: setRxCb (see MODE_LUA) - break; break; #endif @@ -366,6 +364,25 @@ void serialInit(uint8_t port_nr, int mode) if (port) { if (port->uart && port->uart->init) state->usart_ctx = port->uart->init(¶ms); + + // Set power on/off + if (port->set_pwr) { + port->set_pwr(power_required); + } + +#if !defined(BLUETOOTH) + if (port_nr == SP_AUX2) { + TRACE_DEBUG("disbale bt EN (set EN pin high)\n\r"); + GPIO_InitTypeDef GPIO_InitStructure; + GPIO_InitStructure.GPIO_Pin = BT_EN_GPIO_PIN; + GPIO_InitStructure.GPIO_Mode = GPIO_Mode_OUT; + GPIO_InitStructure.GPIO_OType = GPIO_OType_PP; + GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_NOPULL; + GPIO_InitStructure.GPIO_Speed = GPIO_Speed_2MHz; + GPIO_Init(BT_EN_GPIO, &GPIO_InitStructure); + GPIO_SetBits(BT_EN_GPIO, BT_EN_GPIO_PIN); + } +#endif } // Update callbacks once the port is setup diff --git a/radio/src/sumd.cpp b/radio/src/sumd.cpp index b6abdc9c78e..5bdd64e8e7a 100644 --- a/radio/src/sumd.cpp +++ b/radio/src/sumd.cpp @@ -14,6 +14,7 @@ namespace SumDV3 { typename Servo::MesgType Servo::sumdFrame; template uint8_t Servo::mIndex{}; + template uint16_t Servo::mPackagesCounter{}; template @@ -27,6 +28,8 @@ namespace SumDV3 { uint8_t Servo::mode_cmd{}; template uint8_t Servo::sub_cmd{}; + template + typename Servo::Command_t Servo::mCommand{}; } void processSumdInput() { diff --git a/radio/src/sumd.h b/radio/src/sumd.h index 2e0b55a9bfe..fa305d74fc4 100644 --- a/radio/src/sumd.h +++ b/radio/src/sumd.h @@ -38,13 +38,14 @@ namespace SumDV3 { static constexpr uint16_t crc_polynome = 0x1021; uint16_t sum{}; }; - + template struct Servo { using SumDV3 = Trainer::Protocol::SumDV3; using MesgType = SumDV3::MesgType; using SwitchesType = SumDV3::SwitchesType; - + using Command_t = SumDV3::Command_t; + template struct range_t {}; template using offset_t = std::integral_constant; @@ -157,14 +158,20 @@ namespace SumDV3 { break; case State::V3LastValidPackage: csum += b; + reserved = b; mState = State::V3ModeCmd; break; case State::V3ModeCmd: csum += b; + mode_cmd = b; mState = State::V3SubCmd; break; case State::V3SubCmd: csum += b; + sub_cmd = b; + if (!hasCommand()) { + mCommand = Command_t{mode_cmd, sub_cmd}; + } mState = State::CrcH; break; } @@ -212,6 +219,15 @@ namespace SumDV3 { static inline uint16_t getbytes() { return mBytesCounter; } + static inline bool hasCommand() { + return mCommand != Command_t{}; + } + static inline Command_t command() { + Command_t c{}; + using std::swap; + swap(c, mCommand); + return c; + } private: // uses tag-dispatch because no constexpr-if in c++11 template @@ -262,6 +278,7 @@ namespace SumDV3 { static uint8_t reserved; static uint8_t mode_cmd; static uint8_t sub_cmd; + static Command_t mCommand; }; } diff --git a/radio/src/tasks.cpp b/radio/src/tasks.cpp index b3418a9f647..863cbd4fc52 100644 --- a/radio/src/tasks.cpp +++ b/radio/src/tasks.cpp @@ -86,6 +86,10 @@ void sendSynchronousPulses(uint8_t runMask) constexpr uint8_t MIXER_FREQUENT_ACTIONS_PERIOD = 5 /*ms*/; constexpr uint8_t MIXER_MAX_PERIOD = MAX_REFRESH_RATE / 1000 /*ms*/; +#if defined(DEBUG) +static uint16_t dbg_counter{0}; +#endif + void execMixerFrequentActions() { #if defined(SBUS_TRAINER) @@ -102,6 +106,16 @@ void execMixerFrequentActions() else if (hasSerialMode(UART_MODE_SUMD_TRAINER) >= 0) { processSumdInput(); } +#if defined(DEBUG) + if (++dbg_counter > 200) { + dbg_counter = 0; + TRACE_DEBUG("sumd bytes: %d, packages: %d \n\r", SumDV3::Servo<0>::getbytes(), SumDV3::Servo<0>::packages()); + TRACE_DEBUG("ibus bytes: %d, packages: %d \n\r", IBus::Servo<0>::getBytes(), IBus::Servo<0>::packages()); + TRACE_DEBUG("crsf bytes: %d, packages: %d \n\r", CRSF::Servo<0>::getBytes(), CRSF::Servo<0>::packages()); + TRACE_DEBUG("sbus bytes: %d, packages: %d \n\r", SBus::Servo<0>::getBytes(), SBus::Servo<0>::packages()); + } +#endif + #endif #if defined(GYRO) diff --git a/radio/src/trainer.h b/radio/src/trainer.h index a9c1f6a4b55..d687f552068 100644 --- a/radio/src/trainer.h +++ b/radio/src/trainer.h @@ -87,6 +87,7 @@ namespace Trainer { using MesgType = std::array, MaxChannels>; using SwitchesType = uint64_t; + using Command_t = std::pair; }; } } From f2ddc717bb375669d11712228287e2accb064116 Mon Sep 17 00:00:00 2001 From: wimalopaan Date: Fri, 8 Apr 2022 13:34:59 +0200 Subject: [PATCH 24/31] fixed smaller radios w/o BT --- radio/src/serial.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/radio/src/serial.cpp b/radio/src/serial.cpp index 9afef295a6d..60396e4323b 100644 --- a/radio/src/serial.cpp +++ b/radio/src/serial.cpp @@ -370,7 +370,7 @@ void serialInit(uint8_t port_nr, int mode) port->set_pwr(power_required); } -#if !defined(BLUETOOTH) +#if !defined(BLUETOOTH) && defined(BT_EN_GPIO_PIN) if (port_nr == SP_AUX2) { TRACE_DEBUG("disbale bt EN (set EN pin high)\n\r"); GPIO_InitTypeDef GPIO_InitStructure; From 4ae789a0ba7c50143e4249c0c533231bb12cf1d5 Mon Sep 17 00:00:00 2001 From: wimalopaan Date: Fri, 8 Apr 2022 13:51:20 +0200 Subject: [PATCH 25/31] NV14 fix --- radio/src/serial.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/radio/src/serial.cpp b/radio/src/serial.cpp index 60396e4323b..872b4e2e2af 100644 --- a/radio/src/serial.cpp +++ b/radio/src/serial.cpp @@ -370,7 +370,7 @@ void serialInit(uint8_t port_nr, int mode) port->set_pwr(power_required); } -#if !defined(BLUETOOTH) && defined(BT_EN_GPIO_PIN) +#if !defined(BLUETOOTH) && defined(PCBHORUS) && defined(BT_EN_GPIO_PIN) if (port_nr == SP_AUX2) { TRACE_DEBUG("disbale bt EN (set EN pin high)\n\r"); GPIO_InitTypeDef GPIO_InitStructure; From 396de77f5776450b52793ab77a6c890e2dcfc422 Mon Sep 17 00:00:00 2001 From: wimalopaan Date: Fri, 8 Apr 2022 14:08:28 +0200 Subject: [PATCH 26/31] fixed simu and gtests --- radio/src/serial.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/radio/src/serial.cpp b/radio/src/serial.cpp index 872b4e2e2af..6003c952b26 100644 --- a/radio/src/serial.cpp +++ b/radio/src/serial.cpp @@ -370,7 +370,7 @@ void serialInit(uint8_t port_nr, int mode) port->set_pwr(power_required); } -#if !defined(BLUETOOTH) && defined(PCBHORUS) && defined(BT_EN_GPIO_PIN) +#if !defined(BLUETOOTH) && defined(PCBHORUS) && defined(BT_EN_GPIO_PIN) && !defined(SIMU) && !defined(GTESTS) if (port_nr == SP_AUX2) { TRACE_DEBUG("disbale bt EN (set EN pin high)\n\r"); GPIO_InitTypeDef GPIO_InitStructure; From 6198be32aa40b24b455b1f49b1355d00a1d7e673 Mon Sep 17 00:00:00 2001 From: wimalopaan Date: Sat, 9 Apr 2022 07:11:18 +0200 Subject: [PATCH 27/31] refactor class templates --- radio/src/crsf.cpp | 20 -------------------- radio/src/crsf.h | 17 +++++++++++++++++ radio/src/ibus.cpp | 15 --------------- radio/src/ibus.h | 13 +++++++++++++ radio/src/sbus.cpp | 15 --------------- radio/src/sbus.h | 15 ++++++++++++++- radio/src/sumd.cpp | 32 -------------------------------- radio/src/sumd.h | 30 ++++++++++++++++++++++++++++++ 8 files changed, 74 insertions(+), 83 deletions(-) diff --git a/radio/src/crsf.cpp b/radio/src/crsf.cpp index 68ff4f45c11..b3f1857e8d7 100644 --- a/radio/src/crsf.cpp +++ b/radio/src/crsf.cpp @@ -1,25 +1,5 @@ #include "crsf.h" -namespace CRSF { - - template - CRC8 Servo::csum; - template - typename Servo::State Servo::mState{Servo::State::Undefined}; - template - typename Servo::MesgType Servo::mData; - template - uint8_t Servo::mIndex{0}; - template - uint8_t Servo::mLength{0}; - template - uint16_t Servo::mPackagesCounter{0}; - template - uint16_t Servo::mBytesCounter{0}; - template - uint8_t Servo::mPauseCounter{Servo::mPauseCount}; // 2 ms -} - void crsfTrainerPauseCheck() { #if !defined(SIMU) # if defined(AUX_SERIAL) || defined(AUX2_SERIAL) diff --git a/radio/src/crsf.h b/radio/src/crsf.h index 12bd3752905..b36dd05ae3b 100644 --- a/radio/src/crsf.h +++ b/radio/src/crsf.h @@ -140,6 +140,23 @@ namespace CRSF { static uint16_t mBytesCounter; static uint8_t mPauseCounter; }; + // inline static member definitions not until c++17 + template + CRC8 Servo::csum; + template + typename Servo::State Servo::mState{Servo::State::Undefined}; + template + typename Servo::MesgType Servo::mData; + template + uint8_t Servo::mIndex{0}; + template + uint8_t Servo::mLength{0}; + template + uint16_t Servo::mPackagesCounter{0}; + template + uint16_t Servo::mBytesCounter{0}; + template + uint8_t Servo::mPauseCounter{Servo::mPauseCount}; // 2 ms } #ifdef __GNUC__ # pragma GCC diagnostic pop diff --git a/radio/src/ibus.cpp b/radio/src/ibus.cpp index 484e841335d..856b2e317c0 100644 --- a/radio/src/ibus.cpp +++ b/radio/src/ibus.cpp @@ -1,20 +1,5 @@ #include "ibus.h" -namespace IBus { - template - CheckSum Servo::csum; - template - typename Servo::State Servo::mState{Servo::State::Undefined}; - template - typename Servo::MesgType Servo::ibusFrame; - template - uint8_t Servo::mIndex{}; - template - uint16_t Servo::mPackagesCounter{}; - template - uint16_t Servo::mBytesCounter{}; -} - void processIbusInput() { #if !defined(SIMU) uint8_t rxchar; diff --git a/radio/src/ibus.h b/radio/src/ibus.h index cf45c7d3e9b..21451bc9459 100644 --- a/radio/src/ibus.h +++ b/radio/src/ibus.h @@ -131,6 +131,19 @@ namespace IBus { static uint16_t mPackagesCounter; static uint16_t mBytesCounter; }; + // inline static member definitions not until c++17 + template + CheckSum Servo::csum; + template + typename Servo::State Servo::mState{Servo::State::Undefined}; + template + typename Servo::MesgType Servo::ibusFrame; + template + uint8_t Servo::mIndex{}; + template + uint16_t Servo::mPackagesCounter{}; + template + uint16_t Servo::mBytesCounter{}; } #ifdef __GNUC__ diff --git a/radio/src/sbus.cpp b/radio/src/sbus.cpp index 8957ecced84..e8758a197a1 100644 --- a/radio/src/sbus.cpp +++ b/radio/src/sbus.cpp @@ -63,21 +63,6 @@ void sbusSetGetByte(bool (*fct)(uint8_t*)) sbusGetByte = fct; } -namespace SBus { - template - typename Servo::State Servo::mState{Servo::State::Undefined}; - template - typename Servo::MesgType Servo::mData; - template - uint8_t Servo::mIndex{0}; - template - uint16_t Servo::mPackagesCounter{0}; - template - uint16_t Servo::mBytesCounter{0}; - template - uint8_t Servo::mPauseCounter{Servo::mPauseCount}; // 2 ms -} - void sbusTrainerPauseCheck() { #if !defined(SIMU) # if defined(AUX_SERIAL) || defined(AUX2_SERIAL) || defined(TRAINER_MODULE_SBUS) diff --git a/radio/src/sbus.h b/radio/src/sbus.h index 7138b0be3a1..b64a7eac5da 100644 --- a/radio/src/sbus.h +++ b/radio/src/sbus.h @@ -32,7 +32,7 @@ void sbusSetAuxGetByte(void* ctx, bool (*fct)(void*, uint8_t*)); // with sbusSetAuxGetByte() bool sbusAuxGetByte(uint8_t* byte); -//// Setup general SBUS input source +// Setup general SBUS input source void sbusSetGetByte(bool (*fct)(uint8_t*)); void processSbusInput(); @@ -157,6 +157,19 @@ namespace SBus { static uint16_t mBytesCounter; static uint8_t mPauseCounter; }; + // inline static member definitions not until c++17 + template + typename Servo::State Servo::mState{Servo::State::Undefined}; + template + typename Servo::MesgType Servo::mData; + template + uint8_t Servo::mIndex{0}; + template + uint16_t Servo::mPackagesCounter{0}; + template + uint16_t Servo::mBytesCounter{0}; + template + uint8_t Servo::mPauseCounter{Servo::mPauseCount}; // 2 ms } #ifdef __GNUC__ diff --git a/radio/src/sumd.cpp b/radio/src/sumd.cpp index 5bdd64e8e7a..f85766c01f7 100644 --- a/radio/src/sumd.cpp +++ b/radio/src/sumd.cpp @@ -1,37 +1,5 @@ #include "sumd.h" -namespace SumDV3 { - template - uint8_t Servo::nChannels{}; - template - Crc16 Servo::csum{}; - template - uint8_t Servo::crcH{}; - - template - typename Servo::State Servo::mState{Servo::State::Undefined}; - template - typename Servo::MesgType Servo::sumdFrame; - template - uint8_t Servo::mIndex{}; - - template - uint16_t Servo::mPackagesCounter{}; - template - uint16_t Servo::mBytesCounter{}; - - template - typename Servo::Frame Servo::frame{Servo::Frame::Undefined}; - template - uint8_t Servo::reserved{}; - template - uint8_t Servo::mode_cmd{}; - template - uint8_t Servo::sub_cmd{}; - template - typename Servo::Command_t Servo::mCommand{}; -} - void processSumdInput() { #if !defined(SIMU) uint8_t rxchar; diff --git a/radio/src/sumd.h b/radio/src/sumd.h index fa305d74fc4..d98323aec07 100644 --- a/radio/src/sumd.h +++ b/radio/src/sumd.h @@ -280,6 +280,36 @@ namespace SumDV3 { static uint8_t sub_cmd; static Command_t mCommand; }; + // inline static member definitions not until c++17 + template + uint8_t Servo::nChannels{}; + template + Crc16 Servo::csum{}; + template + uint8_t Servo::crcH{}; + + template + typename Servo::State Servo::mState{Servo::State::Undefined}; + template + typename Servo::MesgType Servo::sumdFrame; + template + uint8_t Servo::mIndex{}; + + template + uint16_t Servo::mPackagesCounter{}; + template + uint16_t Servo::mBytesCounter{}; + + template + typename Servo::Frame Servo::frame{Servo::Frame::Undefined}; + template + uint8_t Servo::reserved{}; + template + uint8_t Servo::mode_cmd{}; + template + uint8_t Servo::sub_cmd{}; + template + typename Servo::Command_t Servo::mCommand{}; } #ifdef __GNUC__ From cd7b87cbc5cc6beb7f4fe91d5acdf08c7910e5a6 Mon Sep 17 00:00:00 2001 From: wimalopaan Date: Sat, 9 Apr 2022 15:32:14 +0200 Subject: [PATCH 28/31] fixed lua api --- radio/src/lua/api_general.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/radio/src/lua/api_general.cpp b/radio/src/lua/api_general.cpp index 6eea40e3050..45dc4dabdcf 100644 --- a/radio/src/lua/api_general.cpp +++ b/radio/src/lua/api_general.cpp @@ -2099,6 +2099,7 @@ static int luaGetSumDV3Command(lua_State * L) const Trainer::Protocol::SumDV3::Command_t command = SumDV3::Servo<0>::command(); lua_pushunsigned(L, command.first); lua_pushunsigned(L, command.second); + return 2; } else { lua_pushnil(L); From c3339c81bce9da40b7b69dafb8649a719e75700d Mon Sep 17 00:00:00 2001 From: wimalopaan Date: Sun, 10 Apr 2022 18:48:04 +0200 Subject: [PATCH 29/31] improved LS switch handling --- radio/src/sumd.h | 18 +++++++++++++----- 1 file changed, 13 insertions(+), 5 deletions(-) diff --git a/radio/src/sumd.h b/radio/src/sumd.h index d98323aec07..c25c07f9d25 100644 --- a/radio/src/sumd.h +++ b/radio/src/sumd.h @@ -252,15 +252,20 @@ namespace SumDV3 { sw = (sw << 8) | sumdFrame[15].first; sw = (sw << 8) | sumdFrame[15].second; + uint64_t diff = lastSwitches ^ sw; + for (uint8_t i = 0; i < MAX_LOGICAL_SWITCHES; ++i) { const uint64_t mask = (((uint64_t)0x01) << i); - if (sw & mask) { - rawSetUnconnectedStickySwitch(i, true); - } - else { - rawSetUnconnectedStickySwitch(i, false); + if (diff & mask) { + if (sw & mask) { + rawSetUnconnectedStickySwitch(i, true); + } + else { + rawSetUnconnectedStickySwitch(i, false); + } } } + lastSwitches = sw; } static inline int16_t convertSumdToPuls(uint16_t const value) { const int32_t centered = value - SumDV3::CenterValue; @@ -278,6 +283,7 @@ namespace SumDV3 { static uint8_t reserved; static uint8_t mode_cmd; static uint8_t sub_cmd; + static uint64_t lastSwitches; static Command_t mCommand; }; // inline static member definitions not until c++17 @@ -309,6 +315,8 @@ namespace SumDV3 { template uint8_t Servo::sub_cmd{}; template + uint64_t Servo::lastSwitches{}; + template typename Servo::Command_t Servo::mCommand{}; } From ba6fc3d4cfea7d5e2890a17b8b967338d4846566 Mon Sep 17 00:00:00 2001 From: wimalopaan Date: Thu, 14 Apr 2022 17:42:40 +0200 Subject: [PATCH 30/31] added EXTENDED_TARINER for X9e --- radio/src/dataconstants.h | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/radio/src/dataconstants.h b/radio/src/dataconstants.h index d55f876512c..1fb3b7ec65d 100644 --- a/radio/src/dataconstants.h +++ b/radio/src/dataconstants.h @@ -68,7 +68,11 @@ #define MAX_INPUTS 32 #define MIN_TRAINER_CHANNELS 4 #define DEF_TRAINER_CHANNELS 8 +#if defined(EXTENDED_TRAINER) + #define MAX_TRAINER_CHANNELS 32 +#else #define MAX_TRAINER_CHANNELS 16 +#endif #define MAX_TELEMETRY_SENSORS 60 #elif defined(PCBTARANIS) #define MAX_MODELS 60 From 9eeb1eebc49188a96df457c45f3f598afd2174e3 Mon Sep 17 00:00:00 2001 From: wimalopaan Date: Fri, 15 Apr 2022 11:28:29 +0200 Subject: [PATCH 31/31] * little cleanup * fixed wrong return value conversion --- radio/src/lua/api_general.cpp | 2 +- radio/src/sbus.cpp | 4 ++-- radio/src/serial.cpp | 8 -------- 3 files changed, 3 insertions(+), 11 deletions(-) diff --git a/radio/src/lua/api_general.cpp b/radio/src/lua/api_general.cpp index 45dc4dabdcf..f998ae32522 100644 --- a/radio/src/lua/api_general.cpp +++ b/radio/src/lua/api_general.cpp @@ -103,7 +103,7 @@ static Fifo* luaRxFifo = nullptr; static bool luaRxFifoGetByte(void*, uint8_t* data) { - if (!luaRxFifo) return -1; + if (!luaRxFifo) return false; return luaRxFifo->pop(*data); } diff --git a/radio/src/sbus.cpp b/radio/src/sbus.cpp index e8758a197a1..1a964438f93 100644 --- a/radio/src/sbus.cpp +++ b/radio/src/sbus.cpp @@ -38,13 +38,13 @@ static bool (*_sbusAuxGetByte)(void*, uint8_t*) = nullptr; static void* _sbusAuxGetByteCtx = nullptr; -void sbusSetAuxGetByte(void* ctx, bool (*fct)(void*, uint8_t*)) +void sbusSetAuxGetByte(void* const ctx, bool (* const fct)(void*, uint8_t*)) { _sbusAuxGetByteCtx = ctx; _sbusAuxGetByte = fct; } -bool sbusAuxGetByte(uint8_t* byte) +bool sbusAuxGetByte(uint8_t* const byte) { auto _getByte = _sbusAuxGetByte; auto _ctx = _sbusAuxGetByteCtx; diff --git a/radio/src/serial.cpp b/radio/src/serial.cpp index 6003c952b26..cc4e8e68809 100644 --- a/radio/src/serial.cpp +++ b/radio/src/serial.cpp @@ -248,7 +248,6 @@ static void serialSetupPort(int mode, etx_serial_init& params) params.parity = ETX_Parity_None; params.stop_bits = ETX_StopBits_One; params.rx_enable = true; - power_required = true; break; case UART_MODE_CRSF_TRAINER: @@ -257,7 +256,6 @@ static void serialSetupPort(int mode, etx_serial_init& params) params.parity = ETX_Parity_None; params.stop_bits = ETX_StopBits_One; params.rx_enable = true; - power_required = true; break; case UART_MODE_SUMD_TRAINER: @@ -266,7 +264,6 @@ static void serialSetupPort(int mode, etx_serial_init& params) params.parity = ETX_Parity_None; params.stop_bits = ETX_StopBits_One; params.rx_enable = true; - power_required = true; break; #if defined(LUA) @@ -365,11 +362,6 @@ void serialInit(uint8_t port_nr, int mode) if (port->uart && port->uart->init) state->usart_ctx = port->uart->init(¶ms); - // Set power on/off - if (port->set_pwr) { - port->set_pwr(power_required); - } - #if !defined(BLUETOOTH) && defined(PCBHORUS) && defined(BT_EN_GPIO_PIN) && !defined(SIMU) && !defined(GTESTS) if (port_nr == SP_AUX2) { TRACE_DEBUG("disbale bt EN (set EN pin high)\n\r");