piotr/edgetx-gx12
1
0
Fork 0
Code Issues 2 Pull Requests Actions Packages Projects Releases Wiki Activity

Support new Multi 1.3 features (#7077)

Browse Source
This commit is contained in:
3djc
2019-11-15 16:20:24 +01:00
committed by GitHub
parent 86b186fafd
commit 3a7e006635
57 changed files with 1542 additions and 355 deletions
Download Patch File Download Diff File Expand all files Collapse all files
radio/src/pulses/multi.cpp
+153 -45
View File
@@ -19,6 +19,7 @@
*/
#include "opentx.h"
#include "multi.h"
// for the MULTI protocol definition
// see https://github.com/pascallanger/DIY-Multiprotocol-TX-Module
@@ -31,9 +32,59 @@
#define MULTI_CHANS 16
#define MULTI_CHAN_BITS 11
static void sendFrameProtocolHeader(uint8_t moduleIdx, bool failsafe);
#define MULTI_NORMAL 0x00
#define MULTI_FAILSAFE 0x01
#define MULTI_DATA 0x02
static void sendFrameProtocolHeader(uint8_t moduleIdx, bool failsafe);
void sendChannels(uint8_t moduleIdx);
#if defined(LUA)
static void sendSport(uint8_t moduleIdx);
static void sendHott(uint8_t moduleIdx);
#endif
void multiPatchCustom(uint8_t moduleIdx)
{
if (g_model.moduleData[moduleIdx].multi.customProto) {
uint8_t type = g_model.moduleData[moduleIdx].getMultiProtocol() - 1; // custom where starting at 1, otx list at 0
int subtype = g_model.moduleData[moduleIdx].subType;
g_model.moduleData[moduleIdx].multi.customProto = 0;
if (type == 2) { // multi PROTO_FRSKYD
g_model.moduleData[moduleIdx].subType = 1; // D8
return;
}
else if (type == 14) { // multi PROTO_FRSKYX
g_model.moduleData[moduleIdx].setMultiProtocol(2);
switch (subtype) {
case 0: //D16-16
g_model.moduleData[moduleIdx].subType = 0;
break;
case 1: //D16-8
g_model.moduleData[moduleIdx].subType = 2;
break;
case 2: //EU-16
g_model.moduleData[moduleIdx].subType = 4;
break;
case 3: //EU-8
g_model.moduleData[moduleIdx].subType = 5;
break;
}
return;
}
else if (type == 24) { // multi PROTO_FRSKYV
g_model.moduleData[moduleIdx].setMultiProtocol(2);
g_model.moduleData[moduleIdx].subType = 3;
return;
}
if (type > 14)
type -= 1;
if (type > 24)
type -= 1;
g_model.moduleData[moduleIdx].setMultiProtocol(type);
}
}
static void sendMulti(uint8_t moduleIdx, uint8_t b)
{
@@ -46,22 +97,6 @@ static void sendMulti(uint8_t moduleIdx, uint8_t b)
sendByteSbus(b);
}
static void sendSetupFrame(uint8_t moduleIdx)
{
// Old multi firmware will mark config messsages as invalid frame and throw them away
sendMulti(moduleIdx, 'M');
sendMulti(moduleIdx, 'P');
sendMulti(moduleIdx, 0x80); // Module Configuration
sendMulti(moduleIdx, 1); // 1 byte data
uint8_t config = 0x01 | 0x02; // inversion + multi_telemetry
#if !defined(PPM_PIN_SERIAL)
// TODO why PPM_PIN_SERIAL would change MULTI protocol?
config |= 0x04; // input synchronsisation
#endif
sendMulti(moduleIdx, config);
}
static void sendFailsafeChannels(uint8_t moduleIdx)
{
uint32_t bits = 0;
@@ -95,21 +130,64 @@ static void sendFailsafeChannels(uint8_t moduleIdx)
void setupPulsesMulti(uint8_t moduleIdx)
{
static int counter[2] = {0,0}; //TODO
static uint8_t invert[2] = {0x00, //internal
#if defined(PCBTARANIS) || defined(PCBHORUS)
0x08 //external
#else
0x00 //external
#endif
};
uint8_t type=MULTI_NORMAL;
// Failsafe packets
if (counter[moduleIdx] % 1000 == 0 && g_model.moduleData[moduleIdx].failsafeMode != FAILSAFE_NOT_SET && g_model.moduleData[moduleIdx].failsafeMode != FAILSAFE_RECEIVER) {
type|=MULTI_FAILSAFE;
}
// Invert telemetry if needed
if (invert[moduleIdx] & 0x80 && !g_model.moduleData[moduleIdx].multi.disableTelemetry) {
if (getMultiModuleStatus(moduleIdx).isValid()) {
invert[moduleIdx] &= 0x08; // Telemetry received, stop searching
}
else if (counter[moduleIdx] % 100 == 0) {
invert[moduleIdx] ^= 0x08; // Try inverting telemetry
}
}
// Every 1000 cycles (=9s) send a config packet that configures the multimodule (inversion, telemetry type)
counter[moduleIdx]++;
if (counter[moduleIdx] % 1000 == 500) {
sendSetupFrame(moduleIdx);
}
else if (counter[moduleIdx] % 1000 == 0 && g_model.moduleData[moduleIdx].failsafeMode != FAILSAFE_NOT_SET && g_model.moduleData[moduleIdx].failsafeMode != FAILSAFE_RECEIVER) {
sendFrameProtocolHeader(moduleIdx, true);
// Send header
sendFrameProtocolHeader(moduleIdx, type&MULTI_FAILSAFE);
// Send channels
if (type & MULTI_FAILSAFE)
sendFailsafeChannels(moduleIdx);
}
else {
// Normal Frame
sendFrameProtocolHeader(moduleIdx, false);
else
sendChannels(moduleIdx);
// Multi V1.3.X.X -> Send byte 26, Protocol (bits 7 & 6), RX_Num (bits 5 & 4), invert, not used, disable telemetry, disable mapping
sendMulti(moduleIdx, (uint8_t) (((g_model.moduleData[moduleIdx].getMultiProtocol()+3)&0xC0)
| (g_model.header.modelId[moduleIdx] & 0x30)
| (invert[moduleIdx] & 0x08)
//| 0x04 // Future use
| (g_model.moduleData[moduleIdx].multi.disableTelemetry << 1)
| g_model.moduleData[moduleIdx].multi.disableMapping));
// Multi V1.3.X.X -> Send protocol additional data: max 9 bytes
#if defined(LUA)
if (getMultiModuleStatus(moduleIdx).isValid()) {
MultiModuleStatus &status = getMultiModuleStatus(moduleIdx);
if (status.minor >= 3 && !(status.flags & 0x80)) { //Version 1.3.x.x or more and Buffer not full
// SPort send
if (IS_D16_MULTI(moduleIdx) && outputTelemetryBuffer.destination == TELEMETRY_ENDPOINT_SPORT && outputTelemetryBuffer.size) {
sendSport(moduleIdx); //8 bytes of additional data
}
else if (IS_HOTT_MULTI(moduleIdx)) {
sendHott(moduleIdx); //1 byte of additional data
}
}
}
#endif
}
void setupPulsesMultiExternalModule()
@@ -166,7 +244,7 @@ void sendFrameProtocolHeader(uint8_t moduleIdx, bool failsafe)
{// byte 1+2, protocol information
// Our enumeration starts at 0
int type = g_model.moduleData[moduleIdx].getMultiProtocol(false) + 1;
int type = g_model.moduleData[moduleIdx].getMultiProtocol() + 1;
int subtype = g_model.moduleData[moduleIdx].subType;
int8_t optionValue = g_model.moduleData[moduleIdx].multi.optionValue;
@@ -186,16 +264,18 @@ void sendFrameProtocolHeader(uint8_t moduleIdx, bool failsafe)
protoByte |= MULTI_SEND_RANGECHECK;
// rfProtocol
if (g_model.moduleData[moduleIdx].getMultiProtocol(true) == MODULE_SUBTYPE_MULTI_DSM2) {
if (g_model.moduleData[moduleIdx].getMultiProtocol() == MODULE_SUBTYPE_MULTI_DSM2) {
// Autobinding should always be done in DSMX 11ms
if (g_model.moduleData[moduleIdx].multi.autoBindMode && moduleState[moduleIdx].mode == MODULE_MODE_BIND)
subtype = MM_RF_DSM2_SUBTYPE_AUTO;
// Multi module in DSM mode wants the number of channels to be used as option value
optionValue = sentModuleChannels(moduleIdx);
}
if (optionValue)
optionValue = 0x80 | sentModuleChannels(moduleIdx); // Max throw
else
optionValue = sentModuleChannels(moduleIdx);
}
// 15 for Multimodule is FrskyX or D16 which we map as a subprotocol of 3 (FrSky)
// all protos > frskyx are therefore also off by one
@@ -206,7 +286,7 @@ void sendFrameProtocolHeader(uint8_t moduleIdx, bool failsafe)
if (type >= 25)
type = type + 1;
if (g_model.moduleData[moduleIdx].getMultiProtocol(true) == MODULE_SUBTYPE_MULTI_FRSKY) {
if (g_model.moduleData[moduleIdx].getMultiProtocol() == MODULE_SUBTYPE_MULTI_FRSKY) {
if (subtype == MM_RF_FRSKY_SUBTYPE_D8) {
//D8
type = 3;
@@ -230,28 +310,27 @@ void sendFrameProtocolHeader(uint8_t moduleIdx, bool failsafe)
// Set the highest bit of option byte in AFHDS2A protocol to instruct MULTI to passthrough telemetry bytes instead
// of sending Frsky D telemetry
if (g_model.moduleData[moduleIdx].getMultiProtocol(false) == MODULE_SUBTYPE_MULTI_FS_AFHDS2A)
if (g_model.moduleData[moduleIdx].getMultiProtocol() == MODULE_SUBTYPE_MULTI_FS_AFHDS2A)
optionValue = optionValue | 0x80;
// For custom protocol send unmodified type byte
if (g_model.moduleData[moduleIdx].getMultiProtocol(true) == MM_RF_CUSTOM_SELECTED)
type = g_model.moduleData[moduleIdx].getMultiProtocol(false);
if (g_model.moduleData[moduleIdx].getMultiProtocol() == MM_RF_CUSTOM_SELECTED)
type = g_model.moduleData[moduleIdx].getMultiProtocol();
uint8_t headerByte = 0x54;
uint8_t headerByte = 0x55;
// header, byte 0, 0x55 for proto 0-31, 0x54 for proto 32-63
if (type & 0x20)
headerByte &= 0xFE;
if (failsafe)
headerByte = 0x56;
// header, byte 0, 0x55 for proto 0-31 0x54 for 32-63
if (type <= 31)
sendMulti(moduleIdx, headerByte+1);
else
sendMulti(moduleIdx, headerByte);
headerByte |= 0x02;
sendMulti(moduleIdx, headerByte);
// protocol byte
protoByte |= (type & 0x1f);
if (g_model.moduleData[moduleIdx].getMultiProtocol(true) != MODULE_SUBTYPE_MULTI_DSM2)
if (g_model.moduleData[moduleIdx].getMultiProtocol() != MODULE_SUBTYPE_MULTI_DSM2)
protoByte |= (g_model.moduleData[moduleIdx].multi.autoBindMode << 6);
sendMulti(moduleIdx, protoByte);
@@ -265,3 +344,32 @@ void sendFrameProtocolHeader(uint8_t moduleIdx, bool failsafe)
// byte 3
sendMulti(moduleIdx, (uint8_t) optionValue);
}
#if defined(LUA)
void sendSport(uint8_t moduleIdx)
{
// example: B7 30 30 0C 80 00 00 00 13
uint8_t j=0;
// unstuff and remove crc
for (uint8_t i = 0; i < outputTelemetryBuffer.size - 1 && j < 8; i++, j++) {
if (outputTelemetryBuffer.data[i] == BYTESTUFF) {
i++;
sendMulti(moduleIdx, outputTelemetryBuffer.data[i] ^ STUFF_MASK);
}
else {
sendMulti(moduleIdx, outputTelemetryBuffer.data[i]);
}
}
outputTelemetryBuffer.reset(); // empty buffer
}
void sendHott(uint8_t moduleIdx)
{
if (Multi_Buffer && memcmp(Multi_Buffer, "HoTT", 4) == 0 && Multi_Buffer[5] >= 0xD7 && Multi_Buffer[5] <= 0xDF) {
// HoTT Lua script is running
sendMulti(moduleIdx, Multi_Buffer[5]);
}
}
#endif