Pulses refactoring ... WIP

This commit is contained in:
Bertrand Songis
2018-10-09 07:57:58 +02:00
parent f798bbfcc7
commit 946f66b128
12 changed files with 536 additions and 586 deletions
+16 -125
View File
@@ -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
}