Files
edgetx-gx12/radio/src/pulses/crossfire.cpp
T

106 lines
3.6 KiB
C++

/*
* Copyright (C) EdgeTX
*
* Based on code named
* opentx - https://github.com/opentx/opentx
* 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"
#define CROSSFIRE_CH_BITS 11
#define CROSSFIRE_CENTER 0x3E0
#if defined(PPM_CENTER_ADJUSTABLE)
#define CROSSFIRE_CENTER_CH_OFFSET(ch) ((2 * limitAddress(ch)->ppmCenter) + 1) // + 1 is for rouding
#else
#define CROSSFIRE_CENTER_CH_OFFSET(ch) (0)
#endif
uint8_t createCrossfireModelIDFrame(uint8_t * frame)
{
uint8_t * buf = frame;
*buf++ = UART_SYNC; /* device address */
*buf++ = 8; /* frame length */
*buf++ = COMMAND_ID; /* cmd type */
*buf++ = MODULE_ADDRESS; /* Destination Address */
*buf++ = RADIO_ADDRESS; /* Origin Address */
*buf++ = SUBCOMMAND_CRSF; /* sub command */
*buf++ = COMMAND_MODEL_SELECT_ID; /* command of set model/receiver id */
*buf++ = g_model.header.modelId[EXTERNAL_MODULE]; /* model ID */
*buf++ = crc8_BA(frame + 2, 6);
*buf++ = crc8(frame + 2, 7);
return buf - frame;
}
// Range for pulses (channels output) is [-1024:+1024]
uint8_t createCrossfireChannelsFrame(uint8_t * frame, int16_t * pulses)
{
uint8_t * buf = frame;
*buf++ = MODULE_ADDRESS;
*buf++ = 24; // 1(ID) + 22 + 1(CRC)
uint8_t * crc_start = buf;
*buf++ = CHANNELS_ID;
uint32_t bits = 0;
uint8_t bitsavailable = 0;
for (int i=0; i<CROSSFIRE_CHANNELS_COUNT; i++) {
uint32_t val = limit(0, CROSSFIRE_CENTER + (CROSSFIRE_CENTER_CH_OFFSET(i) * 4) / 5 + (pulses[i] * 4) / 5, 2 * CROSSFIRE_CENTER);
bits |= val << bitsavailable;
bitsavailable += CROSSFIRE_CH_BITS;
while (bitsavailable >= 8) {
*buf++ = bits;
bits >>= 8;
bitsavailable -= 8;
}
}
*buf++ = crc8(crc_start, 23);
return buf - frame;
}
static void setupPulsesCrossfire(uint8_t idx, CrossfirePulsesData* p_data,
uint8_t endpoint)
{
#if defined(LUA)
if (outputTelemetryBuffer.destination == endpoint) {
memcpy(p_data->pulses, outputTelemetryBuffer.data,
outputTelemetryBuffer.size);
p_data->length = outputTelemetryBuffer.size;
outputTelemetryBuffer.reset();
} else
#endif
{
if (moduleState[idx].counter == CRSF_FRAME_MODELID) {
p_data->length = createCrossfireModelIDFrame(p_data->pulses);
moduleState[idx].counter = CRSF_FRAME_MODELID_SENT;
} else {
p_data->length = createCrossfireChannelsFrame(
p_data->pulses,
&channelOutputs[g_model.moduleData[idx].channelsStart]);
}
}
}
void setupPulsesCrossfire(uint8_t idx)
{
if (idx == INTERNAL_MODULE) {
auto* p_data = &intmodulePulsesData.crossfire;
setupPulsesCrossfire(idx, p_data, 0);
} else if (telemetryProtocol == PROTOCOL_TELEMETRY_CROSSFIRE) {
auto* p_data = &extmodulePulsesData.crossfire;
setupPulsesCrossfire(idx, p_data, TELEMETRY_ENDPOINT_SPORT);
}
}