diff --git a/src/debug.cpp b/src/debug.cpp index 197605113..1087ad562 100644 --- a/src/debug.cpp +++ b/src/debug.cpp @@ -36,7 +36,6 @@ #include "../opentx.h" #include -#include "fifo.h" #if !defined(SIMU) diff --git a/src/gtests.cpp b/src/gtests.cpp index aa4c52dc4..54f0bf38a 100644 --- a/src/gtests.cpp +++ b/src/gtests.cpp @@ -466,6 +466,34 @@ TEST(Mixer, RecursiveAddChannel) EXPECT_EQ(chans[1], 0); } +TEST(Mixer, RecursiveAddChannelAfterInactivePhase) +{ + MODEL_RESET(); + MIXER_RESET(); + g_model.phaseData[1].swtch = SWSRC_ID1; + g_model.mixData[0].destCh = 0; + g_model.mixData[0].mltpx = MLTPX_ADD; + g_model.mixData[0].srcRaw = MIXSRC_CH2; + g_model.mixData[0].phases = 0b11110; + g_model.mixData[0].weight = 50; + g_model.mixData[1].destCh = 0; + g_model.mixData[1].mltpx = MLTPX_ADD; + g_model.mixData[1].srcRaw = MIXSRC_MAX; + g_model.mixData[1].phases = 0b11101; + g_model.mixData[1].weight = 50; + g_model.mixData[2].destCh = 1; + g_model.mixData[2].srcRaw = MIXSRC_MAX; + g_model.mixData[2].weight = 100; + simuSetSwitch(3, -1); + perMain(); + EXPECT_EQ(chans[0], CHANNEL_MAX/2); + EXPECT_EQ(chans[1], CHANNEL_MAX); + simuSetSwitch(3, 0); + perMain(); + EXPECT_EQ(chans[0], CHANNEL_MAX/2); + EXPECT_EQ(chans[1], CHANNEL_MAX); +} + #define CHECK_SLOW_MOVEMENT(channel, sign, duration) \ do { \ for (int i=1; i<=(duration); i++) { \ diff --git a/src/opentx.cpp b/src/opentx.cpp index f7f305e13..03b62784d 100644 --- a/src/opentx.cpp +++ b/src/opentx.cpp @@ -2834,11 +2834,11 @@ void perOut(uint8_t mode, uint8_t tick10ms) do { - // printf("[pass %d]\n", pass); fflush(stdout); + // TRACE("[pass %d]", pass); bitfield_channels_t passDirtyChannels = 0; - for (uint8_t i = 0; i < MAX_MIXERS; i++) { + for (uint8_t i=0; idestCh))) continue; + // if this is the first calculation for the destination channel, initialize it with 0 (otherwise would be random) + if (i == 0 || md->destCh != (md-1)->destCh) { + chans[md->destCh] = 0; + } + //========== PHASE && SWITCH ===== bool mixCondition = (md->phases != 0 || md->swtch); delayval_t mixEnabled = !(md->phases & (1 << s_perout_flight_phase)) && getSwitch(md->swtch); @@ -2873,7 +2878,7 @@ void perOut(uint8_t mode, uint8_t tick10ms) else { v = getValue(k); if (k>=MIXSRC_CH1-1 && k<=MIXSRC_LAST_CH-1 && md->destCh != k-MIXSRC_CH1+1) { - if (dirtyChannels & ((bitfield_channels_t)1 << (k-MIXSRC_CH1+1)) & (passDirtyChannels|~(((bitfield_channels_t) 1 << md->destCh)-1))) + if (dirtyChannels & ((bitfield_channels_t)1 << (k-MIXSRC_CH1+1))) passDirtyChannels |= (bitfield_channels_t) 1 << md->destCh; if (k-MIXSRC_CH1+1 < md->destCh || pass > 0) v = chans[k-MIXSRC_CH1+1] >> 8; @@ -2886,7 +2891,7 @@ void perOut(uint8_t mode, uint8_t tick10ms) bool apply_offset_and_curve = true; - // ========== DELAYS =============== + //========== DELAYS =============== delayval_t _swOn = swOn[i].now; delayval_t _swPrev = swOn[i].prev; bool swTog = (mixEnabled != _swOn); @@ -2927,7 +2932,7 @@ void perOut(uint8_t mode, uint8_t tick10ms) int16_t offset = GET_GVAR(MD_OFFSET(md), GV_RANGELARGE_NEG, GV_RANGELARGE, s_perout_flight_phase); if (offset) v += calc100toRESX_16Bits(offset); - //========== TRIMS =============== + //========== TRIMS ================ if (!(mode & e_perout_mode_notrims)) { int8_t mix_trim = md->carryTrim; if (mix_trim < TRIM_ON) @@ -2976,13 +2981,13 @@ void perOut(uint8_t mode, uint8_t tick10ms) int32_t newValue = tact-rate/((int16_t)(100/SLOW_STEP)*md->speedDown); if (newValue>currentValue) currentValue = newValue; // Endposition; prevent toggling around the destination } - } //endif diff>0 + } act[i] = tact = currentValue; // open.20.fsguruh: this implementation would save about 50 bytes code } // endif tick10ms ; in case no time passed assign the old value, not the current value from source v = (tact >> DEL_MULT_SHIFT); - } //endif diff - } //endif speed + } + } //========== CURVES =============== if (apply_offset_and_curve && md->curveParam && md->curveMode == MODE_CURVE) { @@ -3004,10 +3009,6 @@ void perOut(uint8_t mode, uint8_t tick10ms) int32_t *ptr = &chans[md->destCh]; // Save calculating address several times - if (i == 0 || md->destCh != (md - 1)->destCh) - *ptr = 0; - // if this is the first calculation for the destination channel, initialize it with 0 (otherwise would be random) - switch (md->mltpx) { case MLTPX_REP: *ptr = dv; diff --git a/src/opentx.h b/src/opentx.h index 006772eed..7a4e88bd9 100644 --- a/src/opentx.h +++ b/src/opentx.h @@ -407,6 +407,7 @@ enum EnumKeys { #endif #if defined(CPUARM) + #include "fifo.h" // This doesn't need protection on this processor #define tmr10ms_t uint32_t extern volatile tmr10ms_t g_tmr10ms; diff --git a/src/targets/sky9x/bluetooth.cpp b/src/targets/sky9x/bluetooth.cpp index 7671f6c28..e7e03fb27 100644 --- a/src/targets/sky9x/bluetooth.cpp +++ b/src/targets/sky9x/bluetooth.cpp @@ -35,7 +35,6 @@ */ #include "../opentx.h" -#include "../fifo.h" void btSetBaudrate(uint32_t index) { diff --git a/src/targets/taranis/pulses_driver.cpp b/src/targets/taranis/pulses_driver.cpp index abe46df78..c079a494a 100644 --- a/src/targets/taranis/pulses_driver.cpp +++ b/src/targets/taranis/pulses_driver.cpp @@ -466,7 +466,7 @@ static void init_pa7_dsm2() TIM8->CR1 &= ~TIM_CR1_CEN ; TIM8->ARR = 44000 ; // 22mS - TIM8->CCR2 = 30000 ; // Update time + TIM8->CCR2 = 40000 ; // Update time TIM8->PSC = (PERI2_FREQUENCY * TIMER_MULT_APB2) / 2000000 - 1 ; // 0.5uS from 30MHz #if defined(REV3) TIM8->CCER = TIM_CCER_CC1E | TIM_CCER_CC1P ; @@ -608,8 +608,7 @@ extern "C" void TIM8_UP_TIM13_IRQHandler() TIM8->SR &= ~TIM_SR_UIF ; // Clear flag TIM8->ARR = *ppmStreamPtr[EXTERNAL_MODULE]++ ; - if (*ppmStreamPtr[EXTERNAL_MODULE] == 0) - { + if (*ppmStreamPtr[EXTERNAL_MODULE] == 0) { TIM8->SR &= ~TIM_SR_CC2IF ; // Clear this flag TIM8->DIER |= TIM_DIER_CC2IE ; // Enable this interrupt } diff --git a/src/targets/taranis/sport_driver.cpp b/src/targets/taranis/sport_driver.cpp index 2f63a0b1a..1fa9b60e8 100644 --- a/src/targets/taranis/sport_driver.cpp +++ b/src/targets/taranis/sport_driver.cpp @@ -36,6 +36,8 @@ #include "../opentx.h" +extern Fifo<512> telemetryFifo; + void sportInit(void) { USART_InitTypeDef USART_InitStructure; @@ -85,16 +87,25 @@ void sportPutc(const char c) GPIO_ResetBits(GPIO_PIN_SPORT_ON, PIN_SPORT_ON); } -void processSerialData(uint8_t data); - #if !defined(SIMU) + +#define USART_FLAG_ERRORS (USART_FLAG_ORE | USART_FLAG_NE | USART_FLAG_FE | USART_FLAG_PE) + extern "C" void USART2_IRQHandler() { - uint16_t data; + uint32_t status; + uint8_t data; - if (USART_GetFlagStatus(SPORT, USART_FLAG_RXNE) || USART_GetFlagStatus(SPORT, USART_FLAG_ORE)) { - data = USART_ReceiveData(SPORT); - processSerialData((uint8_t) data); + status = SPORT->SR; + + while (status & (USART_FLAG_RXNE | USART_FLAG_ERRORS)) { + data = SPORT->DR; + + if (!(status & USART_FLAG_ERRORS)) + telemetryFifo.push(data); + + status = SPORT->SR; } -} +} + #endif diff --git a/src/targets/taranis/uart_driver.cpp b/src/targets/taranis/uart_driver.cpp index 825140099..8c1ec1de6 100644 --- a/src/targets/taranis/uart_driver.cpp +++ b/src/targets/taranis/uart_driver.cpp @@ -72,7 +72,6 @@ void uartInit(uint32_t baudrate) } #if defined(DEBUG) -#include "../../fifo.h" Fifo<512> debugTxFifo; void debugPutc(const char c) { diff --git a/src/telemetry/frsky_sport.cpp b/src/telemetry/frsky_sport.cpp index 27ae03535..ac50ad5d9 100644 --- a/src/telemetry/frsky_sport.cpp +++ b/src/telemetry/frsky_sport.cpp @@ -125,11 +125,11 @@ uint8_t link_counter = 0; FrskyData frskyData; #if defined(DEBUG) && !defined(SIMU) -#include "../fifo.h" extern FIL g_telemetryFile; -Fifo<512> debugTelemetryFifo; #endif +Fifo<512> telemetryFifo; + void setBaroAltitude(int32_t baroAltitude) { // First received barometer altitude => Altitude offset @@ -266,6 +266,19 @@ void processHubPacket(uint8_t id, uint16_t value) } } +bool checkSportPacket(uint8_t *packet) +{ + short t_short =0; + for (int i=0; i> 8; //0-100 + t_short &= 0x00ff; + t_short += t_short >> 8; //0-0FF + t_short &= 0x00ff; + } + return packet[FRSKY_RX_PACKET_SIZE-1] == (uint8_t)t_short; +} + #define SPORT_DATA_U8(packet) (packet[4]) #define SPORT_DATA_S32(packet) (*((int32_t *)(packet+4))) #define SPORT_DATA_U32(packet) (*((uint32_t *)(packet+4))) @@ -277,6 +290,9 @@ void processSportPacket(uint8_t *packet) uint8_t prim = packet[1]; uint16_t appId = *((uint16_t *)(packet+2)); + if (!checkSportPacket(packet)) + return; + switch (prim) { case DATA_FRAME: @@ -404,10 +420,6 @@ void processSerialData(uint8_t data) btPushByte(data); #endif -#if defined(DEBUG) && !defined(SIMU) - debugTelemetryFifo.push(data); -#endif - if (data == START_STOP) { dataState = STATE_DATA_IN_FRAME; numPktBytes = 0; @@ -489,7 +501,27 @@ inline bool alarmRaised(uint8_t channel, uint8_t idx) void telemetryWakeup() { -#if defined(PCBSKY9X) +#if defined(PCBTARANIS) + uint8_t data; +#if defined(DEBUG) && !defined(SIMU) + static tmr10ms_t lastTime = 0; + tmr10ms_t newTime = get_tmr10ms(); + struct gtm utm; + gettime(&utm); +#endif + while (telemetryFifo.pop(data)) { + processSerialData(data); +#if defined(DEBUG) && !defined(SIMU) + if (lastTime != newTime) { + f_printf(&g_telemetryFile, "\r\n%4d-%02d-%02d,%02d:%02d:%02d.%02d0: %02X", utm.tm_year+1900, utm.tm_mon+1, utm.tm_mday, utm.tm_hour, utm.tm_min, utm.tm_sec, g_ms100, data); + lastTime = newTime; + } + else { + f_printf(&g_telemetryFile, " %02X", data); + } +#endif + } +#elif defined(PCBSKY9X) rxPdcUsart(processSerialData); // Receive serial data here #endif @@ -507,24 +539,6 @@ void telemetryWakeup() varioWakeup(); #endif -#if defined(DEBUG) && !defined(SIMU) - static tmr10ms_t lastTime = 0; - tmr10ms_t newTime = get_tmr10ms(); - uint8_t data; - struct gtm utm; - gettime(&utm); - - while (debugTelemetryFifo.pop(data)) { - if (lastTime != newTime) { - f_printf(&g_telemetryFile, "\r\n%4d-%02d-%02d,%02d:%02d:%02d.%02d0: %02X", utm.tm_year+1900, utm.tm_mon+1, utm.tm_mday, utm.tm_hour, utm.tm_min, utm.tm_sec, g_ms100, data); - lastTime = newTime; - } - else { - f_printf(&g_telemetryFile, " %02X", data); - } - } -#endif - static tmr10ms_t alarmsCheckTime = 0; static uint8_t alarmsCheckStep = 0;