diff --git a/radio/src/pulses/pxx.cpp b/radio/src/pulses/pxx.cpp index f63ee0580..af389aecb 100644 --- a/radio/src/pulses/pxx.cpp +++ b/radio/src/pulses/pxx.cpp @@ -127,56 +127,6 @@ void PxxPulses::addChannels(uint8_t port, uint8_t sendFailsafe, ui } } -template -uint8_t PxxPulses::addFlag1(uint8_t port) -{ - uint8_t flag1 = (g_model.moduleData[port].rfProtocol << 6); - if (moduleFlag[port] == MODULE_BIND) { - flag1 |= (g_eeGeneral.countryCode << 1) | PXX_SEND_BIND; - } - else if (moduleFlag[port] == MODULE_RANGECHECK) { - flag1 |= PXX_SEND_RANGECHECK; - } - else if (g_model.moduleData[port].failsafeMode != FAILSAFE_NOT_SET && g_model.moduleData[port].failsafeMode != FAILSAFE_RECEIVER) { - if (failsafeCounter[port]-- == 0) { - failsafeCounter[port] = 1000; - flag1 |= PXX_SEND_FAILSAFE; - } - if (failsafeCounter[port] == 0 && g_model.moduleData[port].channelsCount > 0) { - flag1 |= PXX_SEND_FAILSAFE; - } - } - PxxTransport::addByte(flag1); - return flag1; -} - -template -void PxxPulses::addExtraFlags(uint8_t port) -{ - // Ext. flag (holds antenna selection on Horus internal module, 0x00 otherwise) - uint8_t extra_flags = 0; - -#if defined(PCBHORUS) || defined(PCBXLITE) - if (port == INTERNAL_MODULE) { - extra_flags |= (g_model.moduleData[port].pxx.external_antenna << 0); - } -#endif - - extra_flags |= (g_model.moduleData[port].pxx.receiver_telem_off << 1); - extra_flags |= (g_model.moduleData[port].pxx.receiver_channel_9_16 << 2); - if (isModuleR9M(port)) { - extra_flags |= (min(g_model.moduleData[port].pxx.power, isModuleR9M_FCC_VARIANT(port) ? (uint8_t)R9M_FCC_POWER_MAX : (uint8_t)R9M_LBT_POWER_MAX) << 3); - if (isModuleR9M_EUPLUS(port)) - extra_flags |= (1 << 6); - } - - // Disable S.PORT if internal module is active - if (IS_TELEMETRY_INTERNAL_MODULE()) { - extra_flags |= (1 << 5); - } - PxxTransport::addByte(extra_flags); -} - template class PxxPulses >; template class PxxPulses >; template class PxxPulses; diff --git a/radio/src/pulses/pxx.h b/radio/src/pulses/pxx.h index 0fcf7f89e..3ce8ea2db 100644 --- a/radio/src/pulses/pxx.h +++ b/radio/src/pulses/pxx.h @@ -232,9 +232,7 @@ class UartPxxTransport: public DataBuffer, public PxxCrcMixin { template class PxxPulses: public PxxTransport { protected: - uint8_t addFlag1(uint8_t port); void addChannels(uint8_t port, uint8_t sendFailsafe, uint8_t sendUpperChannels); - void addExtraFlags(uint8_t port); }; #endif diff --git a/radio/src/pulses/pxx1.cpp b/radio/src/pulses/pxx1.cpp index 3ab3963d8..4db5e7e68 100644 --- a/radio/src/pulses/pxx1.cpp +++ b/radio/src/pulses/pxx1.cpp @@ -21,6 +21,56 @@ #include "opentx.h" #include "pulses/pxx1.h" +template +uint8_t Pxx1Pulses::addFlag1(uint8_t port) +{ + uint8_t flag1 = (g_model.moduleData[port].rfProtocol << 6); + if (moduleFlag[port] == MODULE_BIND) { + flag1 |= (g_eeGeneral.countryCode << 1) | PXX_SEND_BIND; + } + else if (moduleFlag[port] == MODULE_RANGECHECK) { + flag1 |= PXX_SEND_RANGECHECK; + } + else if (g_model.moduleData[port].failsafeMode != FAILSAFE_NOT_SET && g_model.moduleData[port].failsafeMode != FAILSAFE_RECEIVER) { + if (failsafeCounter[port]-- == 0) { + failsafeCounter[port] = 1000; + flag1 |= PXX_SEND_FAILSAFE; + } + if (failsafeCounter[port] == 0 && g_model.moduleData[port].channelsCount > 0) { + flag1 |= PXX_SEND_FAILSAFE; + } + } + PxxTransport::addByte(flag1); + return flag1; +} + +template +void Pxx1Pulses::addExtraFlags(uint8_t port) +{ + // Ext. flag (holds antenna selection on Horus internal module, 0x00 otherwise) + uint8_t extra_flags = 0; + +#if defined(PCBHORUS) || defined(PCBXLITE) + if (port == INTERNAL_MODULE) { + extra_flags |= (g_model.moduleData[port].pxx.external_antenna << 0); + } +#endif + + extra_flags |= (g_model.moduleData[port].pxx.receiver_telem_off << 1); + extra_flags |= (g_model.moduleData[port].pxx.receiver_channel_9_16 << 2); + if (isModuleR9M(port)) { + extra_flags |= (min(g_model.moduleData[port].pxx.power, isModuleR9M_FCC_VARIANT(port) ? (uint8_t)R9M_FCC_POWER_MAX : (uint8_t)R9M_LBT_POWER_MAX) << 3); + if (isModuleR9M_EUPLUS(port)) + extra_flags |= (1 << 6); + } + + // Disable S.PORT if internal module is active + if (IS_TELEMETRY_INTERNAL_MODULE()) { + extra_flags |= (1 << 5); + } + PxxTransport::addByte(extra_flags); +} + template void Pxx1Pulses::add8ChannelsFrame(uint8_t port, uint8_t sendUpperChannels) { @@ -33,7 +83,7 @@ void Pxx1Pulses::add8ChannelsFrame(uint8_t port, uint8_t sendUpper PxxTransport::addByte(g_model.header.modelId[port]); // Flag1 - uint8_t flag1 = PxxPulses::addFlag1(port); + uint8_t flag1 = addFlag1(port); // Flag2 PxxTransport::addByte(0); @@ -42,7 +92,7 @@ void Pxx1Pulses::add8ChannelsFrame(uint8_t port, uint8_t sendUpper PxxPulses::addChannels(port, flag1 & PXX_SEND_FAILSAFE, sendUpperChannels); // Extra flags - PxxPulses::addExtraFlags(port); + addExtraFlags(port); // CRC addCrc(); diff --git a/radio/src/pulses/pxx1.h b/radio/src/pulses/pxx1.h index 03624e7ea..b1ad7a623 100644 --- a/radio/src/pulses/pxx1.h +++ b/radio/src/pulses/pxx1.h @@ -42,6 +42,9 @@ class Pxx1Pulses: public PxxPulses PxxTransport::addByteWithoutCrc(PxxCrcMixin::crc); } + uint8_t addFlag1(uint8_t port); + void addExtraFlags(uint8_t port); + void add8ChannelsFrame(uint8_t port, uint8_t sendUpperChannels); }; diff --git a/radio/src/pulses/pxx2.cpp b/radio/src/pulses/pxx2.cpp index 0f613074b..f04166284 100644 --- a/radio/src/pulses/pxx2.cpp +++ b/radio/src/pulses/pxx2.cpp @@ -21,29 +21,39 @@ #include "opentx.h" #include "pulses/pxx2.h" -void Pxx2Pulses::setupFrame(uint8_t port) +uint8_t Pxx2Pulses::addFlag1(uint8_t module) +{ + uint8_t flag1 = g_model.header.modelId[module] & 0x3F; + if (g_model.moduleData[module].failsafeMode != FAILSAFE_NOT_SET && g_model.moduleData[module].failsafeMode != FAILSAFE_RECEIVER) { + if (failsafeCounter[module]-- == 0) { + failsafeCounter[module] = 1000; + flag1 |= PXX2_FLAG1_FAILSAFE; + } + } + Pxx2Transport::addByte(flag1); + return flag1; +} + +void Pxx2Pulses::setupFrame(uint8_t module) { initFrame(); - static uint8_t pass[NUM_MODULES] = { MODULES_INIT(0) }; - uint8_t sendUpperChannels = 0; - if (pass[port]++ & 0x01) { - sendUpperChannels = g_model.moduleData[port].channelsCount; - } + addFrameType(PXX2_TYPE_C_MODULE, PXX2_TYPE_ID_CHANNELS); // Model ID - addByte(g_model.header.modelId[port]); + // TODO addByte(g_model.header.modelId[port]); // Flag1 - uint8_t flag1 = addFlag1(port); + uint8_t flag1 = addFlag1(module); // Flag2 = Extra flags - addExtraFlags(port); + // TODO addExtraFlags(module); // Channels - addChannels(port, flag1 & PXX_SEND_FAILSAFE, sendUpperChannels); + addChannels(module, flag1 & PXX2_FLAG1_FAILSAFE, 0); + addChannels(module, flag1 & PXX2_FLAG1_FAILSAFE, 1); -#if defined(LUA) +#if 0 if (outputTelemetryBufferTrigger != 0x7E && outputTelemetryBufferSize > 0) { // primID (1 byte) + dataID (2 bytes) + value (4 bytes) addByte(outputTelemetryBufferTrigger); diff --git a/radio/src/pulses/pxx2.h b/radio/src/pulses/pxx2.h index f78985947..b2aba4d3a 100644 --- a/radio/src/pulses/pxx2.h +++ b/radio/src/pulses/pxx2.h @@ -23,6 +23,19 @@ #include "./pxx.h" +#define PXX2_TYPE_C_MODULE 0x01 + #define PXX2_TYPE_ID_REGISTER 0x01 + #define PXX2_TYPE_ID_BIND 0x02 + #define PXX2_TYPE_ID_CHANNELS 0x03 + #define PXX2_TYPE_ID_SPORT 0xFE + +#define PXX2_TYPE_C_POWER_METER 0x02 + +#define PXX2_TYPE_C_OTA 0xFE + +#define PXX2_FLAG1_FAILSAFE (1 << 6) + + // should not be used anymore class SportCrcMixin { protected: @@ -67,17 +80,19 @@ class Pxx2Pulses: public PxxPulses { // reserve 1 byte for LEN Pxx2Transport::addByteWithoutCrc(0x00); - - // TYPE_C + TYPE_ID - // TODO optimization ? Pxx2Transport::addByte(0x26); // This one is CRC-ed on purpose } void addFrameType(uint8_t type_c, uint8_t type_id) { + // TYPE_C + TYPE_ID + // TODO optimization ? Pxx2Transport::addByte(0x26); // This one is CRC-ed on purpose + Pxx2Transport::addByte(type_c); Pxx2Transport::addByte(type_id); } + uint8_t addFlag1(uint8_t module); + void addCrc() { Pxx2Transport::addByteWithoutCrc(PxxCrcMixin::crc >> 8); diff --git a/radio/src/targets/taranis/extmodule_driver.cpp b/radio/src/targets/taranis/extmodule_driver.cpp index 109275c6f..5153bb9fb 100644 --- a/radio/src/targets/taranis/extmodule_driver.cpp +++ b/radio/src/targets/taranis/extmodule_driver.cpp @@ -20,8 +20,6 @@ #include "opentx.h" -void extmoduleSendNextFrame(); - void extmoduleStop() { NVIC_DisableIRQ(EXTMODULE_TIMER_DMA_STREAM_IRQn); @@ -98,8 +96,6 @@ void extmodulePpmStart() EXTMODULE_TIMER->CCMR1 = TIM_CCMR1_OC1M_1 | TIM_CCMR1_OC1M_2 | TIM_CCMR1_OC2PE; // PWM mode 1 EXTMODULE_TIMER->CR1 |= TIM_CR1_CEN; - extmoduleSendNextFrame(); - NVIC_EnableIRQ(EXTMODULE_TIMER_DMA_STREAM_IRQn); NVIC_SetPriority(EXTMODULE_TIMER_DMA_STREAM_IRQn, 7); NVIC_EnableIRQ(EXTMODULE_TIMER_CC_IRQn); @@ -132,8 +128,6 @@ void extmoduleSerialStart(uint32_t /*baudrate*/, uint32_t period_half_us) EXTMODULE_TIMER->CCMR1 = TIM_CCMR1_OC1M_1 | TIM_CCMR1_OC1M_0; EXTMODULE_TIMER->CR1 |= TIM_CR1_CEN; - extmoduleSendNextFrame(); - NVIC_EnableIRQ(EXTMODULE_TIMER_DMA_STREAM_IRQn); NVIC_SetPriority(EXTMODULE_TIMER_DMA_STREAM_IRQn, 7); NVIC_EnableIRQ(EXTMODULE_TIMER_CC_IRQn); @@ -232,8 +226,6 @@ void extmodulePxxStart() EXTMODULE_TIMER->CCMR1 = TIM_CCMR1_OC1M_1 | TIM_CCMR1_OC1M_2; EXTMODULE_TIMER->CR1 |= TIM_CR1_CEN; - extmoduleSendNextFrame(); - NVIC_EnableIRQ(EXTMODULE_TIMER_DMA_STREAM_IRQn); NVIC_SetPriority(EXTMODULE_TIMER_DMA_STREAM_IRQn, 7); NVIC_EnableIRQ(EXTMODULE_TIMER_CC_IRQn); diff --git a/radio/src/tasks.cpp b/radio/src/tasks.cpp index 41901446d..b618de3d7 100644 --- a/radio/src/tasks.cpp +++ b/radio/src/tasks.cpp @@ -179,8 +179,19 @@ TASK_FUNCTION(mixerTask) void scheduleNextMixerCalculation(uint8_t module, uint16_t period_ms) { // Schedule next mixer calculation time, - // for now assume mixer calculation takes 2 ms. - nextMixerTime[module] = (uint32_t)RTOS_GET_TIME() + (period_ms / RTOS_MS_PER_TICK) - 1 /* 1 tick in advance*/; + + if (isProtocolSynchronous(s_current_protocol[module])) { + nextMixerTime[module] += period_ms / RTOS_MS_PER_TICK; + if (nextMixerTime[module] < RTOS_GET_TIME()) { + // we are late ... let's add some small delay + nextMixerTime[module] = (uint32_t) RTOS_GET_TIME() + (period_ms / RTOS_MS_PER_TICK); + } + } + else { + // for now assume mixer calculation takes 2 ms. + nextMixerTime[module] = (uint32_t) RTOS_GET_TIME() + (period_ms / RTOS_MS_PER_TICK) - 1 /* 1 tick in advance*/; + } + DEBUG_TIMER_STOP(debugTimerMixerCalcToUsage); }