260 lines
7.5 KiB
C++
260 lines
7.5 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 "edgetx.h"
|
|
#include "ghost.h"
|
|
#include "telemetry/ghost.h"
|
|
#include "telemetry/ghost_menu.h"
|
|
#include "hal/module_port.h"
|
|
#include "mixer_scheduler.h"
|
|
|
|
static uint8_t getGhostModuleAddr()
|
|
{
|
|
// TODO: check if asymmetric baudrate is really needed
|
|
// as it introduces a lot of clutter
|
|
//
|
|
// #if SPORT_MAX_BAUDRATE < 400000
|
|
// return g_model.moduleData[EXTERNAL_MODULE].ghost.telemetryBaudrate ==
|
|
// GHST_TELEMETRY_RATE_400K
|
|
// ? GHST_ADDR_MODULE_SYM
|
|
// : GHST_ADDR_MODULE_ASYM;
|
|
// #else
|
|
return GHST_ADDR_MODULE_SYM;
|
|
// #endif
|
|
}
|
|
|
|
static uint8_t createGhostMenuControlFrame(uint8_t * frame, int16_t * pulses)
|
|
{
|
|
uint8_t * buf = frame;
|
|
memset(buf, 0, GHST_UL_RC_CHANS_SIZE + 2);
|
|
|
|
*buf++ = getGhostModuleAddr(); // addr
|
|
*buf++ = GHST_UL_RC_CHANS_SIZE; // length
|
|
const uint8_t * crc_start = buf;
|
|
*buf++ = GHST_UL_MENU_CTRL; // type
|
|
|
|
// payload
|
|
*buf++ = reusableBuffer.ghostMenu.buttonAction; // Joystick states, Up, Down, Left, Right, Press
|
|
*buf++ = reusableBuffer.ghostMenu.menuAction; // menu control, open, close, etc.
|
|
|
|
// skip padding
|
|
buf += 8;
|
|
|
|
// crc
|
|
*buf++ = crc8(crc_start, GHST_UL_RC_CHANS_SIZE - 1);
|
|
|
|
return buf - frame;
|
|
}
|
|
|
|
// Range for pulses (channels output) is [-1024:+1024]
|
|
static uint8_t createGhostChannelsFrame(uint8_t * frame, int16_t * pulses, bool raw12bits)
|
|
{
|
|
static uint8_t lastGhostFrameId = 0;
|
|
uint8_t ghostUpper4Offset = 0;
|
|
|
|
switch (lastGhostFrameId) {
|
|
case GHST_UL_RC_CHANS_HS4_5TO8:
|
|
case GHST_UL_RC_CHANS_HS4_12_5TO8:
|
|
lastGhostFrameId = raw12bits ? GHST_UL_RC_CHANS_HS4_12_9TO12 : GHST_UL_RC_CHANS_HS4_9TO12;
|
|
ghostUpper4Offset = 4;
|
|
break;
|
|
|
|
case GHST_UL_RC_CHANS_HS4_9TO12:
|
|
case GHST_UL_RC_CHANS_HS4_12_9TO12:
|
|
lastGhostFrameId = raw12bits ? GHST_UL_RC_CHANS_HS4_12_13TO16 : GHST_UL_RC_CHANS_HS4_13TO16;
|
|
ghostUpper4Offset = 8;
|
|
break;
|
|
|
|
case GHST_UL_RC_CHANS_HS4_13TO16:
|
|
case GHST_UL_RC_CHANS_HS4_12_13TO16:
|
|
lastGhostFrameId = raw12bits ? GHST_UL_RC_CHANS_HS4_12_5TO8 : GHST_UL_RC_CHANS_HS4_5TO8;
|
|
ghostUpper4Offset = 0;
|
|
break;
|
|
|
|
default: // We don't have known previous state so init
|
|
lastGhostFrameId = raw12bits ? GHST_UL_RC_CHANS_HS4_12_5TO8 : GHST_UL_RC_CHANS_HS4_5TO8;
|
|
ghostUpper4Offset = 0;
|
|
break;
|
|
}
|
|
|
|
uint8_t * buf = frame;
|
|
*buf++ = getGhostModuleAddr(); // addr
|
|
*buf++ = GHST_UL_RC_CHANS_SIZE; // len
|
|
uint8_t * crc_start = buf;
|
|
*buf++ = lastGhostFrameId; // type
|
|
|
|
// payload
|
|
// first 4 high speed, 12 bit channels (11 relevant bits with openTx)
|
|
uint32_t bits = 0;
|
|
uint8_t bitsavailable = 0;
|
|
for (int i = 0; i < 4; i++) {
|
|
uint32_t value;
|
|
if (raw12bits) {
|
|
value = limit(
|
|
0, (1024 + (pulses[i] + 2 * PPM_CH_CENTER(i) - 2 * PPM_CENTER)) << 1,
|
|
0xFFF);
|
|
} else {
|
|
value = limit(
|
|
0,
|
|
GHST_RC_CTR_VAL_12BIT +
|
|
(((pulses[i] + 2 * PPM_CH_CENTER(i) - 2 * PPM_CENTER) << 3) / 5),
|
|
2 * GHST_RC_CTR_VAL_12BIT);
|
|
}
|
|
bits |= value << bitsavailable;
|
|
bitsavailable += GHST_CH_BITS_12;
|
|
while (bitsavailable >= 8) {
|
|
*buf++ = bits;
|
|
bits >>= 8;
|
|
bitsavailable -= 8;
|
|
}
|
|
}
|
|
|
|
// second 4 lower speed, 8 bit channels
|
|
for (int i = 4; i < 8; ++i) {
|
|
uint8_t channelIndex = i + ghostUpper4Offset;
|
|
uint8_t value;
|
|
if (raw12bits) {
|
|
value = limit(0,
|
|
128 + ((pulses[channelIndex] +
|
|
2 * PPM_CH_CENTER(channelIndex) - 2 * PPM_CENTER) >>
|
|
3),
|
|
0xFF);
|
|
} else {
|
|
value = limit(0,
|
|
GHST_RC_CTR_VAL_8BIT +
|
|
(((pulses[channelIndex] +
|
|
2 * PPM_CH_CENTER(channelIndex) - 2 * PPM_CENTER) >>
|
|
1) /
|
|
5),
|
|
2 * GHST_RC_CTR_VAL_8BIT);
|
|
}
|
|
*buf++ = value;
|
|
}
|
|
|
|
// crc
|
|
*buf++ = crc8(crc_start, GHST_UL_RC_CHANS_SIZE - 1);
|
|
|
|
return buf - frame;
|
|
}
|
|
|
|
static const etx_serial_init ghostSerialParams = {
|
|
.baudrate = GHOST_BAUDRATE,
|
|
.encoding = ETX_Encoding_8N1,
|
|
.direction = ETX_Dir_TX_RX,
|
|
.polarity = ETX_Pol_Normal,
|
|
};
|
|
|
|
static void* ghostInit(uint8_t module)
|
|
{
|
|
auto mod_st = modulePortInitSerial(module, ETX_MOD_PORT_SPORT, &ghostSerialParams, false);
|
|
if (!mod_st) return nullptr;
|
|
|
|
mixerSchedulerSetPeriod(module, GHOST_PERIOD);
|
|
return mod_st;
|
|
}
|
|
|
|
static void ghostDeInit(void* ctx)
|
|
{
|
|
auto mod_st = (etx_module_state_t*)ctx;
|
|
modulePortDeInit(mod_st);
|
|
}
|
|
|
|
static void ghostSendPulses(void* ctx, uint8_t* buffer, int16_t* channels, uint8_t nChannels)
|
|
{
|
|
auto mod_st = (etx_module_state_t*)ctx;
|
|
auto module = modulePortGetModule(mod_st);
|
|
|
|
ModuleSyncStatus& status = getModuleSyncStatus(module);
|
|
if (status.isValid()) {
|
|
mixerSchedulerSetPeriod(module, status.getAdjustedRefreshRate());
|
|
} else {
|
|
mixerSchedulerSetPeriod(module, GHOST_PERIOD);
|
|
}
|
|
|
|
const auto& mod_cfg = g_model.moduleData[module].ghost;
|
|
auto p_data = buffer;
|
|
|
|
#if defined(LUA)
|
|
if (outputTelemetryBuffer.destination == TELEMETRY_ENDPOINT_SPORT) {
|
|
auto len = outputTelemetryBuffer.size;
|
|
auto f_data = outputTelemetryBuffer.data;
|
|
while (len >= 12) { // type(1B) + payload(10B) + crc(1B)
|
|
// insert address byte
|
|
*p_data++ = getGhostModuleAddr();
|
|
// and length
|
|
*p_data++ = 12;
|
|
memcpy(p_data, f_data, 12);
|
|
p_data += 12; f_data += 12;
|
|
len -= 12;
|
|
}
|
|
outputTelemetryBuffer.reset();
|
|
} else
|
|
#endif
|
|
if (moduleState[module].counter == GHST_MENU_CONTROL) {
|
|
p_data += createGhostMenuControlFrame(p_data, channels);
|
|
} else {
|
|
p_data += createGhostChannelsFrame(p_data, channels, mod_cfg.raw12bits);
|
|
}
|
|
|
|
moduleState[module].counter = GHST_FRAME_CHANNEL;
|
|
|
|
auto drv = modulePortGetSerialDrv(mod_st->tx);
|
|
auto drv_ctx = modulePortGetCtx(mod_st->tx);
|
|
drv->sendBuffer(drv_ctx, buffer, p_data - buffer);
|
|
}
|
|
|
|
static void ghostProcessData(void* ctx, uint8_t data, uint8_t* buffer, uint8_t* len)
|
|
{
|
|
if (*len == 0 && data != GHST_ADDR_RADIO) {
|
|
TRACE("[GH] address 0x%02X error", data);
|
|
return;
|
|
}
|
|
|
|
if (*len < TELEMETRY_RX_PACKET_SIZE) {
|
|
buffer[(*len)++] = data;
|
|
}
|
|
else {
|
|
TRACE("[GH] array size %d error", *len);
|
|
*len = 0;
|
|
}
|
|
|
|
if (*len > 4) {
|
|
uint8_t length = buffer[1];
|
|
if (length + 2 == *len) {
|
|
auto mod_st = (etx_module_state_t*)ctx;
|
|
auto module = modulePortGetModule(mod_st);
|
|
processGhostTelemetryFrame(module, buffer, *len);
|
|
*len = 0;
|
|
}
|
|
}
|
|
}
|
|
|
|
const etx_proto_driver_t GhostDriver = {
|
|
.protocol = PROTOCOL_CHANNELS_GHOST,
|
|
.init = ghostInit,
|
|
.deinit = ghostDeInit,
|
|
.sendPulses = ghostSendPulses,
|
|
.processData = ghostProcessData,
|
|
.processFrame = nullptr,
|
|
.onConfigChange = nullptr,
|
|
.txCompleted = modulePortSerialTxCompleted,
|
|
};
|