From 946f66b1286f935f58a71e9576f14ef4bcf31eab Mon Sep 17 00:00:00 2001 From: Bertrand Songis Date: Tue, 9 Oct 2018 07:57:58 +0200 Subject: [PATCH] Pulses refactoring ... WIP --- radio/src/pulses/pulses_arm.cpp | 4 +- radio/src/pulses/pulses_arm.h | 12 +- radio/src/pulses/pxx.cpp | 230 +++++++++ radio/src/pulses/pxx.h | 221 +++++++++ radio/src/pulses/pxx2.cpp | 141 +----- radio/src/pulses/pxx2.h | 54 ++- radio/src/pulses/pxx_arm.cpp | 450 ------------------ radio/src/targets/common/arm/CMakeLists.txt | 2 +- radio/src/targets/horus/board.h | 2 +- radio/src/targets/horus/telemetry_driver.cpp | 2 +- radio/src/targets/taranis/board.h | 2 +- .../src/targets/taranis/telemetry_driver.cpp | 2 +- 12 files changed, 536 insertions(+), 586 deletions(-) create mode 100644 radio/src/pulses/pxx.cpp delete mode 100644 radio/src/pulses/pxx_arm.cpp diff --git a/radio/src/pulses/pulses_arm.cpp b/radio/src/pulses/pulses_arm.cpp index 14543ebd7..6ade90621 100755 --- a/radio/src/pulses/pulses_arm.cpp +++ b/radio/src/pulses/pulses_arm.cpp @@ -221,8 +221,8 @@ void setupPulses(uint8_t port) #if defined(PXX2) case PROTO_PXX2: if (telemetryProtocol == PROTOCOL_FRSKY_SPORT && !init_needed) { - createPXX2ChannelsFrame(port); - sportSendBuffer(modulePulsesData[port].pxx_uart.pulses, modulePulsesData[port].pxx_uart.ptr - modulePulsesData[port].pxx_uart.pulses); + modulePulsesData[port].pxx2.setupFrame(port); + sportSendBuffer(modulePulsesData[port].pxx2.getData(), modulePulsesData[port].pxx2.getSize()); } scheduleNextMixerCalculation(port, PXX2_PERIOD); break; diff --git a/radio/src/pulses/pulses_arm.h b/radio/src/pulses/pulses_arm.h index 92841f311..aab6832bb 100644 --- a/radio/src/pulses/pulses_arm.h +++ b/radio/src/pulses/pulses_arm.h @@ -54,8 +54,6 @@ template struct PpmPulsesData { #define PXX_PERIOD 9/*ms*/ #endif -#define PXX_PERIOD_HALF_US (PXX_PERIOD * 2000) - #if defined(PPM_PIN_SERIAL) PACK(struct PxxSerialPulsesData { uint8_t pulses[64]; @@ -125,13 +123,17 @@ PACK(struct PxxTimerPulsesData { }); #endif +#if !defined(BOOT) +#include "pulses/pxx2.h" +#endif + #define CROSSFIRE_FRAME_MAXLEN 64 PACK(struct CrossfirePulsesData { uint8_t pulses[CROSSFIRE_FRAME_MAXLEN]; }); union ModulePulsesData { -#if defined(INTMODULE_USART) || defined(EXTMODULE_USART) || defined(PXX2) +#if defined(INTMODULE_USART) || defined(EXTMODULE_USART) PxxUartPulsesData pxx_uart; #endif #if defined(PPM_PIN_SERIAL) @@ -140,6 +142,10 @@ union ModulePulsesData { PxxTimerPulsesData pxx; #endif +#if defined(PXX2) && !defined(BOOT) + Pxx2Pulses pxx2; +#endif + #if defined(PPM_PIN_SERIAL) Dsm2SerialPulsesData dsm2; #else diff --git a/radio/src/pulses/pxx.cpp b/radio/src/pulses/pxx.cpp new file mode 100644 index 000000000..5d491c5dd --- /dev/null +++ b/radio/src/pulses/pxx.cpp @@ -0,0 +1,230 @@ +/* + * Copyright (C) OpenTX + * + * Based on code named + * th9x - http://code.google.com/p/th9x + * er9x - http://code.google.com/p/er9x + * gruvin9x - http://code.google.com/p/gruvin9x + * + * License GPLv2: http://www.gnu.org/licenses/gpl-2.0.html + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License version 2 as + * published by the Free Software Foundation. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + */ + +#include "opentx.h" +#include "pulses/pxx.h" + +const uint16_t PxxCrcMixin::CRCTable[] = { + 0x0000,0x1189,0x2312,0x329b,0x4624,0x57ad,0x6536,0x74bf, + 0x8c48,0x9dc1,0xaf5a,0xbed3,0xca6c,0xdbe5,0xe97e,0xf8f7, + 0x1081,0x0108,0x3393,0x221a,0x56a5,0x472c,0x75b7,0x643e, + 0x9cc9,0x8d40,0xbfdb,0xae52,0xdaed,0xcb64,0xf9ff,0xe876, + 0x2102,0x308b,0x0210,0x1399,0x6726,0x76af,0x4434,0x55bd, + 0xad4a,0xbcc3,0x8e58,0x9fd1,0xeb6e,0xfae7,0xc87c,0xd9f5, + 0x3183,0x200a,0x1291,0x0318,0x77a7,0x662e,0x54b5,0x453c, + 0xbdcb,0xac42,0x9ed9,0x8f50,0xfbef,0xea66,0xd8fd,0xc974, + 0x4204,0x538d,0x6116,0x709f,0x0420,0x15a9,0x2732,0x36bb, + 0xce4c,0xdfc5,0xed5e,0xfcd7,0x8868,0x99e1,0xab7a,0xbaf3, + 0x5285,0x430c,0x7197,0x601e,0x14a1,0x0528,0x37b3,0x263a, + 0xdecd,0xcf44,0xfddf,0xec56,0x98e9,0x8960,0xbbfb,0xaa72, + 0x6306,0x728f,0x4014,0x519d,0x2522,0x34ab,0x0630,0x17b9, + 0xef4e,0xfec7,0xcc5c,0xddd5,0xa96a,0xb8e3,0x8a78,0x9bf1, + 0x7387,0x620e,0x5095,0x411c,0x35a3,0x242a,0x16b1,0x0738, + 0xffcf,0xee46,0xdcdd,0xcd54,0xb9eb,0xa862,0x9af9,0x8b70, + 0x8408,0x9581,0xa71a,0xb693,0xc22c,0xd3a5,0xe13e,0xf0b7, + 0x0840,0x19c9,0x2b52,0x3adb,0x4e64,0x5fed,0x6d76,0x7cff, + 0x9489,0x8500,0xb79b,0xa612,0xd2ad,0xc324,0xf1bf,0xe036, + 0x18c1,0x0948,0x3bd3,0x2a5a,0x5ee5,0x4f6c,0x7df7,0x6c7e, + 0xa50a,0xb483,0x8618,0x9791,0xe32e,0xf2a7,0xc03c,0xd1b5, + 0x2942,0x38cb,0x0a50,0x1bd9,0x6f66,0x7eef,0x4c74,0x5dfd, + 0xb58b,0xa402,0x9699,0x8710,0xf3af,0xe226,0xd0bd,0xc134, + 0x39c3,0x284a,0x1ad1,0x0b58,0x7fe7,0x6e6e,0x5cf5,0x4d7c, + 0xc60c,0xd785,0xe51e,0xf497,0x8028,0x91a1,0xa33a,0xb2b3, + 0x4a44,0x5bcd,0x6956,0x78df,0x0c60,0x1de9,0x2f72,0x3efb, + 0xd68d,0xc704,0xf59f,0xe416,0x90a9,0x8120,0xb3bb,0xa232, + 0x5ac5,0x4b4c,0x79d7,0x685e,0x1ce1,0x0d68,0x3ff3,0x2e7a, + 0xe70e,0xf687,0xc41c,0xd595,0xa12a,0xb0a3,0x8238,0x93b1, + 0x6b46,0x7acf,0x4854,0x59dd,0x2d62,0x3ceb,0x0e70,0x1ff9, + 0xf78f,0xe606,0xd49d,0xc514,0xb1ab,0xa022,0x92b9,0x8330, + 0x7bc7,0x6a4e,0x58d5,0x495c,0x3de3,0x2c6a,0x1ef1,0x0f78 +}; + +template +void StandardPxxPulses::setup8ChannelsFrame(uint8_t port, uint8_t sendUpperChannels) +{ + PxxTransport::initCrc(); + + // Sync + addHead(); + + // RX Number + PxxTransport::addByte(g_model.header.modelId[port]); + + // Flag1 + uint8_t flag1 = PxxPulses::addFlag1(port); + + // Flag2 + PxxTransport::addByte(0); + + // Channels + PxxPulses::addChannels(port, flag1 & PXX_SEND_FAILSAFE, sendUpperChannels); + + // Extra flags + PxxPulses::addExtraFlags(); + + // CRC + addCrc(); + + // Sync = HEAD + addHead(); + + // Tail + PxxTransport::addTail(); +} + +template +void PxxPulses::addChannels(uint8_t port, uint8_t sendFailsafe, uint8_t sendUpperChannels) +{ + uint16_t pulseValue = 0; + uint16_t pulseValueLow = 0; + + for (int8_t i=0; i<8; i++) { + if (sendFailsafe) { + if (g_model.moduleData[port].failsafeMode == FAILSAFE_HOLD) { + pulseValue = (i < sendUpperChannels ? 4095 : 2047); + } + else if (g_model.moduleData[port].failsafeMode == FAILSAFE_NOPULSES) { + pulseValue = (i < sendUpperChannels ? 2048 : 0); + } + else { + if (i < sendUpperChannels) { + int16_t failsafeValue = g_model.moduleData[port].failsafeChannels[8+i]; + if (failsafeValue == FAILSAFE_CHANNEL_HOLD) { + pulseValue = 4095; + } + else if (failsafeValue == FAILSAFE_CHANNEL_NOPULSE) { + pulseValue = 2048; + } + else { + failsafeValue += 2*PPM_CH_CENTER(8+g_model.moduleData[port].channelsStart+i) - 2*PPM_CENTER; + pulseValue = limit(2049, (failsafeValue * 512 / 682) + 3072, 4094); + } + } + else { + int16_t failsafeValue = g_model.moduleData[port].failsafeChannels[i]; + if (failsafeValue == FAILSAFE_CHANNEL_HOLD) { + pulseValue = 2047; + } + else if (failsafeValue == FAILSAFE_CHANNEL_NOPULSE) { + pulseValue = 0; + } + else { + failsafeValue += 2*PPM_CH_CENTER(g_model.moduleData[port].channelsStart+i) - 2*PPM_CENTER; + pulseValue = limit(1, (failsafeValue * 512 / 682) + 1024, 2046); + } + } + } + } + else { + if (i < sendUpperChannels) { + int channel = 8 + g_model.moduleData[port].channelsStart + i; + int value = channelOutputs[channel] + 2*PPM_CH_CENTER(channel) - 2*PPM_CENTER; + pulseValue = limit(2049, (value * 512 / 682) + 3072, 4094); + } + else if (i < sentModuleChannels(port)) { + int channel = g_model.moduleData[port].channelsStart + i; + int value = channelOutputs[channel] + 2*PPM_CH_CENTER(channel) - 2*PPM_CENTER; + pulseValue = limit(1, (value * 512 / 682) + 1024, 2046); + } + else { + pulseValue = 1024; + } + } + + if (i & 1) { + PxxTransport::addByte(pulseValueLow); // Low byte of channel + PxxTransport::addByte(((pulseValueLow >> 8) & 0x0F) | (pulseValue << 4)); // 4 bits each from 2 channels + PxxTransport::addByte(pulseValue >> 4); // High byte of channel + } + else { + pulseValueLow = pulseValue; + } + } +} + +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 +void StandardPxxPulses::setupFrame(uint8_t port) +{ + PxxPulses::initFrame(); + +#if defined(PXX_FREQUENCY_HIGH) + setup8ChannelsFrame(port, 0); + if (sentModuleChannels(port) > 8) { + setup8ChannelsFrame(port, 8); + } +#else + static uint8_t pass[NUM_MODULES] = { MODULES_INIT(0) }; + uint8_t sendUpperChannels = 0; + if (pass[port]++ & 0x01) { + sendUpperChannels = g_model.moduleData[port].channelsCount; + } + setup8ChannelsFrame(port, sendUpperChannels); +#endif +} diff --git a/radio/src/pulses/pxx.h b/radio/src/pulses/pxx.h index 15ac96784..5f6362e2d 100644 --- a/radio/src/pulses/pxx.h +++ b/radio/src/pulses/pxx.h @@ -18,6 +18,227 @@ * GNU General Public License for more details. */ +#ifndef _PULSES_PXX_H_ +#define _PULSES_PXX_H_ + +#include + #define PXX_SEND_BIND 0x01 #define PXX_SEND_FAILSAFE (1 << 4) #define PXX_SEND_RANGECHECK (1 << 5) + +class PxxCrcMixin { + protected: + void initCrc() + { + crc = 0; + } + + void addToCrc(uint8_t byte) + { + crc = (crc << 8) ^ (CRCTable[((crc >> 8) ^ byte) & 0xFF]); + } + + uint8_t crc; + static const uint16_t CRCTable[]; +}; + + +class SerialPxxBitTransport { + protected: + uint8_t data[64]; + uint8_t * ptr; + uint8_t byte; + uint8_t bits_count; + uint8_t padding[2]; + + void initFrame() + { + ptr = data; + byte = 0; + bits_count = 0; + } + + void addSerialBit(uint8_t bit) + { + byte >>= 1; + if (bit & 1) { + byte |= 0x80; + } + if (++bits_count >= 8) { + *ptr++ = byte; + bits_count = 0; + } + } + + // 8uS/bit 01 = 0, 001 = 1 + void addPart(uint8_t value) + { + addSerialBit(0); + if (value) { + addSerialBit(0); + } + addSerialBit(1); + } + + void addTail() + { + while (bits_count != 0) { + addSerialBit(1); + } + } +}; + +// TODO elsewhere !!! + +#if defined(PCBX12S) && PCBREV < 13 + #define pulse_duration_t uint32_t + #define trainer_pulse_duration_t uint16_t +#else + #define pulse_duration_t uint16_t + #define trainer_pulse_duration_t uint16_t +#endif + +#define PXX_PERIOD_HALF_US (PXX_PERIOD * 2000) + +class PwmPxxBitTransport { + protected: + pulse_duration_t pulses[200]; + pulse_duration_t * ptr; + uint16_t rest; + uint8_t padding[2]; + + void initFrame() + { + ptr = pulses; + rest = PXX_PERIOD_HALF_US; + } + + void addPart(uint8_t value) + { + pulse_duration_t duration = value ? 47 : 31; + *ptr++ = duration; + rest -= duration + 1; + } + + void addTail(uint8_t port) + { + // rest min value is 18000 - 200 * 48 = 8400 (4.2ms) + *(ptr - 1) += rest; + } +}; + +template +class StandardPxxTransport: public PxxCrcMixin, public BitTransport { + protected: + uint8_t ones_count; + uint8_t padding[3]; + + void addByte(uint8_t byte) + { + PxxCrcMixin::addToCrc(byte); + addByteWithoutCrc(byte); + }; + + void addByteWithoutCrc(uint8_t byte) + { + for (uint8_t i = 0; i < 8; i++) { + addBit(byte & 0x80); + byte <<= 1; + } + } + + void addBit(uint8_t bit) + { + if (bit) { + BitTransport::addPart(1); + if (++ones_count == 5) { + ones_count = 0; + BitTransport::addPart(0); // Stuff a 0 bit in + } + } + else { + BitTransport::addPart(0); + ones_count = 0; + } + } + + void initFrame() + { + ones_count = 0; + } +}; + +class UartPxxTransport: public PxxCrcMixin { + protected: + uint8_t data[64]; + uint8_t * ptr; + + void initFrame() + { + ptr = data; + } + + void addByte(uint8_t byte) + { + PxxCrcMixin::addToCrc(byte); + addWithByteStuffing(byte); + } + + void addWithByteStuffing(uint8_t byte) + { + if (0x7E == byte) { + *ptr++ = 0x7D; + *ptr++ = 0x5E; + } + else if (0x7D == byte) { + *ptr++ = 0x7D; + *ptr++ = 0x5D; + } + else { + *ptr++ = byte; + } + } + + void addTail() + { + // nothing + } +}; + +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); +}; + +typedef PxxPulses UartPxxPulses; + +template +class StandardPxxPulses: public PxxPulses +{ + public: + void setupFrame(uint8_t port); + + protected: + void addHead() + { + // send 7E, do not CRC + PxxTransport::addByteWithoutCrc(0x7E); + } + + void addCrc() + { + addByteWithoutCrc(PxxCrcMixin::crc >> 8); + addByteWithoutCrc(PxxCrcMixin::crc); + } + + void setup8ChannelsFrame(uint8_t port, uint8_t sendUpperChannels); +}; + +typedef StandardPxxPulses> PwmPxxPxxPulses; + + +#endif diff --git a/radio/src/pulses/pxx2.cpp b/radio/src/pulses/pxx2.cpp index 93df83ec1..d7d32b588 100644 --- a/radio/src/pulses/pxx2.cpp +++ b/radio/src/pulses/pxx2.cpp @@ -19,23 +19,12 @@ */ #include "opentx.h" -#include "pulses/pxx.h" +#include "pulses/pxx2.h" -void uartInitPcmArray(uint8_t port) +void Pxx2Pulses::setupFrame(uint8_t port) { - modulePulsesData[port].pxx_uart.pcmCrc = 0; - modulePulsesData[port].pxx_uart.ptr = modulePulsesData[port].pxx_uart.pulses; -} - -void putPXX2Byte(uint8_t port, uint8_t byte) -{ - modulePulsesData[port].pxx_uart.pcmCrc = (modulePulsesData[port].pxx_uart.pcmCrc<<8) ^ (CRCTable[((modulePulsesData[port].pxx_uart.pcmCrc>>8) ^ byte) & 0xFF]); - *modulePulsesData[port].pxx_uart.ptr++ = byte; -} - -void createPXX2ChannelsFrame(uint8_t port) -{ - uint16_t pulseValue=0, pulseValueLow=0; + initFrame(); + initCrc(); static uint8_t pass[NUM_MODULES] = { MODULES_INIT(0) }; uint8_t sendUpperChannels = 0; @@ -43,142 +32,44 @@ void createPXX2ChannelsFrame(uint8_t port) sendUpperChannels = g_model.moduleData[port].channelsCount; } - uartInitPcmArray(port); - // Model ID - putPXX2Byte(port, g_model.header.modelId[port]); + addByte(g_model.header.modelId[port]); // Flag1 - 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; - } - } - putPXX2Byte(port, flag1); + uint8_t flag1 = addFlag1(port); // Flag2 - putPXX2Byte(port, 0); + addByte(0); // Channels - for (int i=0; i<8; i++) { - if (flag1 & PXX_SEND_FAILSAFE) { - if (g_model.moduleData[port].failsafeMode == FAILSAFE_HOLD) { - pulseValue = (i < sendUpperChannels ? 4095 : 2047); - } - else if (g_model.moduleData[port].failsafeMode == FAILSAFE_NOPULSES) { - pulseValue = (i < sendUpperChannels ? 2048 : 0); - } - else { - if (i < sendUpperChannels) { - int16_t failsafeValue = g_model.moduleData[port].failsafeChannels[8+i]; - if (failsafeValue == FAILSAFE_CHANNEL_HOLD) { - pulseValue = 4095; - } - else if (failsafeValue == FAILSAFE_CHANNEL_NOPULSE) { - pulseValue = 2048; - } - else { - failsafeValue += 2*PPM_CH_CENTER(8+g_model.moduleData[port].channelsStart+i) - 2*PPM_CENTER; - pulseValue = limit(2049, (failsafeValue * 512 / 682) + 3072, 4094); - } - } - else { - int16_t failsafeValue = g_model.moduleData[port].failsafeChannels[i]; - if (failsafeValue == FAILSAFE_CHANNEL_HOLD) { - pulseValue = 2047; - } - else if (failsafeValue == FAILSAFE_CHANNEL_NOPULSE) { - pulseValue = 0; - } - else { - failsafeValue += 2*PPM_CH_CENTER(g_model.moduleData[port].channelsStart+i) - 2*PPM_CENTER; - pulseValue = limit(1, (failsafeValue * 512 / 682) + 1024, 2046); - } - } - } - } - else { - if (i < sendUpperChannels) { - int channel = 8 + g_model.moduleData[port].channelsStart + i; - int value = channelOutputs[channel] + 2*PPM_CH_CENTER(channel) - 2*PPM_CENTER; - pulseValue = limit(2049, (value * 512 / 682) + 3072, 4094); - } - else if (i < sentModuleChannels(port)) { - int channel = g_model.moduleData[port].channelsStart + i; - int value = channelOutputs[channel] + 2*PPM_CH_CENTER(channel) - 2*PPM_CENTER; - pulseValue = limit(1, (value * 512 / 682) + 1024, 2046); - } - else { - pulseValue = 1024; - } - } + addChannels(port, flag1 & PXX_SEND_FAILSAFE, sendUpperChannels); - if (i & 1) { - putPXX2Byte(port, pulseValueLow); // Low byte of channel - putPXX2Byte(port, ((pulseValueLow >> 8) & 0x0F) | (pulseValue << 4)); // 4 bits each from 2 channels - putPXX2Byte(port, pulseValue >> 4); // High byte of channel - } - else { - pulseValueLow = pulseValue; - } - } - - // 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); - } - putPXX2Byte(port, extra_flags); + // Extra flags + addExtraFlags(port); // Flag3, reserved - putPXX2Byte(port, 0); + addByte(0); #if defined(LUA) if (outputTelemetryBufferTrigger != 0x00 && outputTelemetryBufferSize > 0) { // CMD/Resp - putPXX2Byte(port, 1); + addByte(1); // primID (1 byte) + dataID (2 bytes) + value (4 bytes) for (uint8_t i=0; i<7; i++) { - putPXX2Byte(port, outputTelemetryBuffer[i]); + addByte(outputTelemetryBuffer[i]); } outputTelemetryBufferTrigger = 0x00; outputTelemetryBufferSize = 0; } else { // CMD/Resp - putPXX2Byte(port, 0); + addByte(0); } #else // CMD/Resp - putPXX2Byte(port, 0); + addByte(0); #endif // CRC - putPXX2Byte(port, modulePulsesData[port].pxx_uart.pcmCrc); // TODO need to check the CRC chosen algorithm + addByte(crc); // TODO need to check the CRC chosen algorithm } - diff --git a/radio/src/pulses/pxx2.h b/radio/src/pulses/pxx2.h index fc5f8116a..ce2a83f2e 100644 --- a/radio/src/pulses/pxx2.h +++ b/radio/src/pulses/pxx2.h @@ -18,4 +18,56 @@ * GNU General Public License for more details. */ -void createPXX2ChannelsFrame(uint8_t port); +#ifndef _PULSES_PXX2_H_ +#define _PULSES_PXX2_H_ + +#include "pulses/pxx.h" + +class Pxx2Transport: public PxxCrcMixin { + protected: + uint8_t data[64]; + uint8_t * ptr; + + void initFrame() + { + ptr = data; + } + + void addByte(uint8_t byte) + { + PxxCrcMixin::addToCrc(byte); + *ptr++ = byte; + } + + void addTail() + { + // nothing + } +}; + + +class Pxx2Pulses: public PxxPulses { + public: + void setupFrame(uint8_t port); + + const uint8_t * getData() + { + return data; + } + + uint8_t getSize() + { + return ptr - data; + } + + protected: + uint8_t data[64]; + uint8_t * ptr; + + void initFrame() + { + ptr = data; + } +}; + +#endif diff --git a/radio/src/pulses/pxx_arm.cpp b/radio/src/pulses/pxx_arm.cpp deleted file mode 100644 index 8d19a1642..000000000 --- a/radio/src/pulses/pxx_arm.cpp +++ /dev/null @@ -1,450 +0,0 @@ -/* - * Copyright (C) OpenTX - * - * Based on code named - * th9x - http://code.google.com/p/th9x - * er9x - http://code.google.com/p/er9x - * gruvin9x - http://code.google.com/p/gruvin9x - * - * License GPLv2: http://www.gnu.org/licenses/gpl-2.0.html - * - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License version 2 as - * published by the Free Software Foundation. - * - * This program is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. - */ - -#include "opentx.h" -#include "pulses/pxx.h" - -const uint16_t CRCTable[]= -{ - 0x0000,0x1189,0x2312,0x329b,0x4624,0x57ad,0x6536,0x74bf, - 0x8c48,0x9dc1,0xaf5a,0xbed3,0xca6c,0xdbe5,0xe97e,0xf8f7, - 0x1081,0x0108,0x3393,0x221a,0x56a5,0x472c,0x75b7,0x643e, - 0x9cc9,0x8d40,0xbfdb,0xae52,0xdaed,0xcb64,0xf9ff,0xe876, - 0x2102,0x308b,0x0210,0x1399,0x6726,0x76af,0x4434,0x55bd, - 0xad4a,0xbcc3,0x8e58,0x9fd1,0xeb6e,0xfae7,0xc87c,0xd9f5, - 0x3183,0x200a,0x1291,0x0318,0x77a7,0x662e,0x54b5,0x453c, - 0xbdcb,0xac42,0x9ed9,0x8f50,0xfbef,0xea66,0xd8fd,0xc974, - 0x4204,0x538d,0x6116,0x709f,0x0420,0x15a9,0x2732,0x36bb, - 0xce4c,0xdfc5,0xed5e,0xfcd7,0x8868,0x99e1,0xab7a,0xbaf3, - 0x5285,0x430c,0x7197,0x601e,0x14a1,0x0528,0x37b3,0x263a, - 0xdecd,0xcf44,0xfddf,0xec56,0x98e9,0x8960,0xbbfb,0xaa72, - 0x6306,0x728f,0x4014,0x519d,0x2522,0x34ab,0x0630,0x17b9, - 0xef4e,0xfec7,0xcc5c,0xddd5,0xa96a,0xb8e3,0x8a78,0x9bf1, - 0x7387,0x620e,0x5095,0x411c,0x35a3,0x242a,0x16b1,0x0738, - 0xffcf,0xee46,0xdcdd,0xcd54,0xb9eb,0xa862,0x9af9,0x8b70, - 0x8408,0x9581,0xa71a,0xb693,0xc22c,0xd3a5,0xe13e,0xf0b7, - 0x0840,0x19c9,0x2b52,0x3adb,0x4e64,0x5fed,0x6d76,0x7cff, - 0x9489,0x8500,0xb79b,0xa612,0xd2ad,0xc324,0xf1bf,0xe036, - 0x18c1,0x0948,0x3bd3,0x2a5a,0x5ee5,0x4f6c,0x7df7,0x6c7e, - 0xa50a,0xb483,0x8618,0x9791,0xe32e,0xf2a7,0xc03c,0xd1b5, - 0x2942,0x38cb,0x0a50,0x1bd9,0x6f66,0x7eef,0x4c74,0x5dfd, - 0xb58b,0xa402,0x9699,0x8710,0xf3af,0xe226,0xd0bd,0xc134, - 0x39c3,0x284a,0x1ad1,0x0b58,0x7fe7,0x6e6e,0x5cf5,0x4d7c, - 0xc60c,0xd785,0xe51e,0xf497,0x8028,0x91a1,0xa33a,0xb2b3, - 0x4a44,0x5bcd,0x6956,0x78df,0x0c60,0x1de9,0x2f72,0x3efb, - 0xd68d,0xc704,0xf59f,0xe416,0x90a9,0x8120,0xb3bb,0xa232, - 0x5ac5,0x4b4c,0x79d7,0x685e,0x1ce1,0x0d68,0x3ff3,0x2e7a, - 0xe70e,0xf687,0xc41c,0xd595,0xa12a,0xb0a3,0x8238,0x93b1, - 0x6b46,0x7acf,0x4854,0x59dd,0x2d62,0x3ceb,0x0e70,0x1ff9, - 0xf78f,0xe606,0xd49d,0xc514,0xb1ab,0xa022,0x92b9,0x8330, - 0x7bc7,0x6a4e,0x58d5,0x495c,0x3de3,0x2c6a,0x1ef1,0x0f78 -}; - -#if defined(INTMODULE_USART) || defined(EXTMODULE_USART) -inline void uartPutPcmPart(uint8_t port, uint8_t byte) -{ - if (0x7E == byte) { - *modulePulsesData[port].pxx_uart.ptr++ = 0x7D; - *modulePulsesData[port].pxx_uart.ptr++ = 0x5E; - } - else if (0x7D == byte) { - *modulePulsesData[port].pxx_uart.ptr++ = 0x7D; - *modulePulsesData[port].pxx_uart.ptr++ = 0x5D; - } - else { - *modulePulsesData[port].pxx_uart.ptr++ = byte; - } -} - -void uartPutPcmByte(uint8_t port, uint8_t byte) -{ - modulePulsesData[port].pxx_uart.pcmCrc = (modulePulsesData[port].pxx_uart.pcmCrc<<8) ^ (CRCTable[((modulePulsesData[port].pxx_uart.pcmCrc>>8)^byte) & 0xFF]); - uartPutPcmPart(port, byte); -} - -void uartInitPcmArray(uint8_t port) -{ - modulePulsesData[port].pxx_uart.ptr = modulePulsesData[port].pxx_uart.pulses; -} - -void uartInitPcmCrc(uint8_t port) -{ - modulePulsesData[port].pxx_uart.pcmCrc = 0; -} - -void uartPutPcmHead(uint8_t port) -{ - // send 7E, do not CRC - *modulePulsesData[port].pxx_uart.ptr++ = 0x7E; -} - -void uartPutPcmCrc(uint8_t port) -{ - uint16_t pulseValue = modulePulsesData[port].pxx_uart.pcmCrc; - uartPutPcmByte(port, pulseValue >> 8); - uartPutPcmByte(port, pulseValue); -} -#endif - -#if !defined(INTMODULE_USART) || !defined(EXTMODULE_USART) - -#if defined(PPM_PIN_SERIAL) -void pxxPutPcmSerialBit(uint8_t port, uint8_t bit) -{ - modulePulsesData[port].pxx.serialByte >>= 1; - if (bit & 1) { - modulePulsesData[port].pxx.serialByte |= 0x80; - } - if (++modulePulsesData[port].pxx.serialBitCount >= 8) { - *modulePulsesData[port].pxx.ptr++ = modulePulsesData[port].pxx.serialByte; - modulePulsesData[port].pxx.serialBitCount = 0; - } -} - -// 8uS/bit 01 = 0, 001 = 1 -void pxxPutPcmPart(uint8_t port, uint8_t value) -{ - pxxPutPcmSerialBit(port, 0); - if (value) { - pxxPutPcmSerialBit(port, 0); - } - pxxPutPcmSerialBit(port, 1); -} - -void pxxPutPcmTail(uint8_t port) -{ - while (modulePulsesData[port].pxx.serialBitCount != 0) { - pxxPutPcmSerialBit(port, 1); - } -} -#else -void pxxPutPcmPart(uint8_t port, uint8_t value) -{ - pulse_duration_t duration = value ? 47 : 31; - *modulePulsesData[port].pxx.ptr++ = duration; - modulePulsesData[port].pxx.rest -= duration + 1; -} - -void pxxPutPcmTail(uint8_t port) -{ - // rest min value is 18000 - 200 * 48 = 8400 (4.2ms) - *(modulePulsesData[port].pxx.ptr-1) += modulePulsesData[port].pxx.rest; -} -#endif - -void pxxPutPcmBit(uint8_t port, uint8_t bit) -{ - if (bit) { - pxxPutPcmPart(port, 1); - if (++modulePulsesData[port].pxx.pcmOnesCount == 5) { - modulePulsesData[port].pxx.pcmOnesCount = 0; - pxxPutPcmPart(port, 0); // Stuff a 0 bit in - } - } - else { - pxxPutPcmPart(port, 0); - modulePulsesData[port].pxx.pcmOnesCount = 0; - } -} - -void pxxPutPcmByte(uint8_t port, uint8_t byte) -{ - modulePulsesData[port].pxx.pcmCrc = (modulePulsesData[port].pxx.pcmCrc<<8) ^ (CRCTable[((modulePulsesData[port].pxx.pcmCrc>>8)^byte) & 0xFF]); - for (uint8_t i=0; i<8; i++) { - pxxPutPcmBit(port, byte & 0x80); - byte <<= 1; - } -} - -void pxxInitPcmArray(uint8_t port) -{ - modulePulsesData[port].pxx.ptr = modulePulsesData[port].pxx.pulses; -#if defined(PPM_PIN_SERIAL) - modulePulsesData[port].pxx.pcmValue = 0; -#else - modulePulsesData[port].pxx.rest = PXX_PERIOD_HALF_US; -#endif - - modulePulsesData[port].pxx.pcmOnesCount = 0; -} - -void pxxInitPcmCrc(uint8_t port) -{ - modulePulsesData[port].pxx.pcmCrc = 0; -} - -void pxxPutPcmHead(uint8_t port) -{ - // send 7E, do not CRC - // 01111110 - pxxPutPcmPart(port, 0); - pxxPutPcmPart(port, 1); - pxxPutPcmPart(port, 1); - pxxPutPcmPart(port, 1); - pxxPutPcmPart(port, 1); - pxxPutPcmPart(port, 1); - pxxPutPcmPart(port, 1); - pxxPutPcmPart(port, 0); -} - -void pxxPutPcmCrc(uint8_t port) -{ - uint16_t pulseValue = modulePulsesData[port].pxx.pcmCrc; - pxxPutPcmByte(port, pulseValue >> 8); - pxxPutPcmByte(port, pulseValue); -} -#else - // those functions should not be used, a link error will occur if wrong - void pxxInitPcmArray(uint8_t port); - void pxxInitPcmCrc(uint8_t port); - void pxxPutPcmByte(uint8_t port, uint8_t byte); - void pxxPutPcmHead(uint8_t port); - void pxxPutPcmTail(uint8_t port); - void pxxPutPcmCrc(uint8_t port); -#endif - -#if defined(INTMODULE_USART) || defined(EXTMODULE_USART) -inline void initPcmArray(uint8_t port) -{ - if (IS_UART_MODULE(port)) - uartInitPcmArray(port); - else - pxxInitPcmArray(port); -} - -inline void initPcmCrc(uint8_t port) -{ - if (IS_UART_MODULE(port)) - uartInitPcmCrc(port); - else - pxxInitPcmCrc(port); -} - -inline void putPcmHead(uint8_t port) -{ - if (IS_UART_MODULE(port)) - uartPutPcmHead(port); - else - pxxPutPcmHead(port); -} - -inline void putPcmByte(uint8_t port, uint8_t byte) -{ - if (IS_UART_MODULE(port)) - uartPutPcmByte(port, byte); - else - pxxPutPcmByte(port, byte); -} - -inline void putPcmCrc(uint8_t port) -{ - if (IS_UART_MODULE(port)) - uartPutPcmCrc(port); - else - pxxPutPcmCrc(port); -} - -inline void putPcmTail(uint8_t port) -{ - if (!IS_UART_MODULE(port)) - pxxPutPcmTail(port); -} -#else -inline void initPcmArray(uint8_t port) -{ - pxxInitPcmArray(port); -} - -inline void initPcmCrc(uint8_t port) -{ - pxxInitPcmCrc(port); -} - -inline void putPcmHead(uint8_t port) -{ - pxxPutPcmHead(port); -} - -inline void putPcmByte(uint8_t port, uint8_t byte) -{ - pxxPutPcmByte(port, byte); -} - -inline void putPcmCrc(uint8_t port) -{ - pxxPutPcmCrc(port); -} - -inline void putPcmTail(uint8_t port) -{ - pxxPutPcmTail(port); -} -#endif - -inline void setupFramePXX(uint8_t port, uint8_t sendUpperChannels) -{ - uint16_t pulseValue=0, pulseValueLow=0; - - initPcmCrc(port); - - /* Sync */ - putPcmHead(port); - - /* Rx Number */ - putPcmByte(port, g_model.header.modelId[port]); - - /* FLAG1 */ - 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; - } - } - - putPcmByte(port, flag1); - - /* FLAG2 */ - putPcmByte(port, 0); - - /* CHANNELS */ - for (int i=0; i<8; i++) { - if (flag1 & PXX_SEND_FAILSAFE) { - if (g_model.moduleData[port].failsafeMode == FAILSAFE_HOLD) { - pulseValue = (i < sendUpperChannels ? 4095 : 2047); - } - else if (g_model.moduleData[port].failsafeMode == FAILSAFE_NOPULSES) { - pulseValue = (i < sendUpperChannels ? 2048 : 0); - } - else { - if (i < sendUpperChannels) { - int16_t failsafeValue = g_model.moduleData[port].failsafeChannels[8+i]; - if (failsafeValue == FAILSAFE_CHANNEL_HOLD) { - pulseValue = 4095; - } - else if (failsafeValue == FAILSAFE_CHANNEL_NOPULSE) { - pulseValue = 2048; - } - else { - failsafeValue += 2*PPM_CH_CENTER(8+g_model.moduleData[port].channelsStart+i) - 2*PPM_CENTER; - pulseValue = limit(2049, (failsafeValue * 512 / 682) + 3072, 4094); - } - } - else { - int16_t failsafeValue = g_model.moduleData[port].failsafeChannels[i]; - if (failsafeValue == FAILSAFE_CHANNEL_HOLD) { - pulseValue = 2047; - } - else if (failsafeValue == FAILSAFE_CHANNEL_NOPULSE) { - pulseValue = 0; - } - else { - failsafeValue += 2*PPM_CH_CENTER(g_model.moduleData[port].channelsStart+i) - 2*PPM_CENTER; - pulseValue = limit(1, (failsafeValue * 512 / 682) + 1024, 2046); - } - } - } - } - else { - if (i < sendUpperChannels) { - int channel = 8 + g_model.moduleData[port].channelsStart + i; - int value = channelOutputs[channel] + 2*PPM_CH_CENTER(channel) - 2*PPM_CENTER; - pulseValue = limit(2049, (value * 512 / 682) + 3072, 4094); - } - else if (i < sentModuleChannels(port)) { - int channel = g_model.moduleData[port].channelsStart + i; - int value = channelOutputs[channel] + 2*PPM_CH_CENTER(channel) - 2*PPM_CENTER; - pulseValue = limit(1, (value * 512 / 682) + 1024, 2046); - } - else { - pulseValue = 1024; - } - } - - if (i & 1) { - putPcmByte(port, pulseValueLow); // Low byte of channel - putPcmByte(port, ((pulseValueLow >> 8) & 0x0F) | (pulseValue << 4)); // 4 bits each from 2 channels - putPcmByte(port, pulseValue >> 4); // High byte of channel - } - else { - pulseValueLow = pulseValue; - } - } - - uint8_t extra_flags = 0; - -/* Ext. flag (holds antenna selection on Horus internal module, 0x00 otherwise) */ -#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); - } - - putPcmByte(port, extra_flags); - - /* CRC */ - putPcmCrc(port); - - /* Sync */ - putPcmHead(port); - - putPcmTail(port); -} - -void setupPulsesPXX(uint8_t port) -{ - initPcmArray(port); - -#if defined(PXX_FREQUENCY_HIGH) - setupFramePXX(port, 0); - if (sentModuleChannels(port) > 8) { - setupFramePXX(port, 8); - } -#else - static uint8_t pass[NUM_MODULES] = { MODULES_INIT(0) }; - uint8_t sendUpperChannels = 0; - if (pass[port]++ & 0x01) { - sendUpperChannels = g_model.moduleData[port].channelsCount; - } - setupFramePXX(port, sendUpperChannels); -#endif -} diff --git a/radio/src/targets/common/arm/CMakeLists.txt b/radio/src/targets/common/arm/CMakeLists.txt index 8099d6d56..0d552c781 100644 --- a/radio/src/targets/common/arm/CMakeLists.txt +++ b/radio/src/targets/common/arm/CMakeLists.txt @@ -129,7 +129,7 @@ set(PULSES_SRC ${PULSES_SRC} pulses_arm.cpp ppm_arm.cpp - pxx_arm.cpp + pxx.cpp dsm2_arm.cpp sbus_arm.cpp ) \ No newline at end of file diff --git a/radio/src/targets/horus/board.h b/radio/src/targets/horus/board.h index 0818657da..cc513c449 100644 --- a/radio/src/targets/horus/board.h +++ b/radio/src/targets/horus/board.h @@ -526,7 +526,7 @@ int32_t getVolume(void); #define TELEMETRY_FIFO_SIZE 512 void telemetryPortInit(uint32_t baudrate, uint8_t mode); void telemetryPortSetDirectionOutput(void); -void sportSendBuffer(uint8_t * buffer, uint32_t count); +void sportSendBuffer(const uint8_t * buffer, uint32_t count); uint8_t telemetryGetByte(uint8_t * byte); extern uint32_t telemetryErrors; diff --git a/radio/src/targets/horus/telemetry_driver.cpp b/radio/src/targets/horus/telemetry_driver.cpp index 75e31cb81..8541c0742 100644 --- a/radio/src/targets/horus/telemetry_driver.cpp +++ b/radio/src/targets/horus/telemetry_driver.cpp @@ -138,7 +138,7 @@ void telemetryPortSetDirectionInput() TELEMETRY_USART->CR1 |= USART_CR1_RE; // turn on receiver } -void sportSendBuffer(uint8_t * buffer, uint32_t count) +void sportSendBuffer(const uint8_t * buffer, uint32_t count) { telemetryPortSetDirectionOutput(); diff --git a/radio/src/targets/taranis/board.h b/radio/src/targets/taranis/board.h index 235afc1d4..004f55648 100644 --- a/radio/src/targets/taranis/board.h +++ b/radio/src/targets/taranis/board.h @@ -567,7 +567,7 @@ void debugPutc(const char c); // Telemetry driver void telemetryPortInit(uint32_t baudrate, uint8_t mode); void telemetryPortSetDirectionOutput(void); -void sportSendBuffer(uint8_t * buffer, uint32_t count); +void sportSendBuffer(const uint8_t * buffer, uint32_t count); uint8_t telemetryGetByte(uint8_t * byte); extern uint32_t telemetryErrors; diff --git a/radio/src/targets/taranis/telemetry_driver.cpp b/radio/src/targets/taranis/telemetry_driver.cpp index fbcfdbd56..f3b58159e 100644 --- a/radio/src/targets/taranis/telemetry_driver.cpp +++ b/radio/src/targets/taranis/telemetry_driver.cpp @@ -89,7 +89,7 @@ void telemetryPortSetDirectionInput() TELEMETRY_USART->CR1 |= USART_CR1_RE; // turn on receiver } -void sportSendBuffer(uint8_t * buffer, uint32_t count) +void sportSendBuffer(const uint8_t * buffer, uint32_t count) { telemetryPortSetDirectionOutput();