mirror of
https://github.com/EdgeTX/edgetx.git
synced 2025-07-24 00:35:14 +03:00
Multi improvements (#5322)
* implementing syncing with multi * Send config command * Implement setting failsafe for multimodule * Fix * Fix last bugs. Multi sync now working as expected. * Add eww multi protocol
This commit is contained in:
parent
8e9d370770
commit
613b2edf49
24 changed files with 409 additions and 130 deletions
|
@ -46,7 +46,7 @@ static const QStringList STR_SUBTYPE_SYMAX {"Standard", "Syma X5C"};
|
|||
static const QStringList STR_SUBTYPE_SLT {"SLT", "Vista"};
|
||||
static const QStringList STR_SUBTYPE_CX10 {"Green", "Blue", "DM007", "-", "JC3015a", "JC3015b", "MK33041", "Q242"};
|
||||
static const QStringList STR_SUBTYPE_CG023 {"CG023", "YD829", "H8 3D"};
|
||||
static const QStringList STR_SUBTYPE_BAYANG {"Bayang", "H8S3D"};
|
||||
static const QStringList STR_SUBTYPE_BAYANG {"Bayang", "H8S3D", "X16 AH"};
|
||||
static const QStringList STR_SUBTYPE_KN {"WLtoys", "FeiLun"};
|
||||
static const QStringList STR_SUBTYPE_MT99 {"MT99", "H7", "YZ", "LS", "FY805"};
|
||||
static const QStringList STR_SUBTYPE_MJXQ {"WLH08", "X600", "X800", "H26D", "E010", "H26WH"};
|
||||
|
@ -62,33 +62,33 @@ static const QStringList NO_SUBTYPE {STR_MULTI_DEFAULT};
|
|||
// Table is designed to be shared with gui_common_arm.cpp
|
||||
|
||||
const Multiprotocols multiProtocols {
|
||||
{ MM_RF_PROTO_FLYSKY, 4, STR_SUBTYPE_FLYSKY, nullptr },
|
||||
{ MM_RF_PROTO_HUBSAN, 0, NO_SUBTYPE, STR_MULTI_VIDFREQ },
|
||||
{ MM_RF_PROTO_FRSKY, 5, STR_SUBTYPE_FRSKY, STR_MULTI_RFTUNE },
|
||||
{ MM_RF_PROTO_HISKY, 1, STR_SUBTYPE_HISKY, nullptr },
|
||||
{ MM_RF_PROTO_V2X2, 1, STR_SUBTYPE_V2X2, nullptr },
|
||||
{ MM_RF_PROTO_DSM2, 3, STR_SUBTYPE_DSM, nullptr },
|
||||
{ MM_RF_PROTO_YD717, 4, STR_SUBTYPE_YD717, nullptr },
|
||||
{ MM_RF_PROTO_KN, 1, STR_SUBTYPE_KN, nullptr },
|
||||
{ MM_RF_PROTO_SYMAX, 1, STR_SUBTYPE_SYMAX, nullptr },
|
||||
{ MM_RF_PROTO_SLT, 1, STR_SUBTYPE_SLT, nullptr },
|
||||
{ MM_RF_PROTO_CX10, 7, STR_SUBTYPE_CX10, nullptr },
|
||||
{ MM_RF_PROTO_CG023, 2, STR_SUBTYPE_CG023, nullptr },
|
||||
{ MM_RF_PROTO_BAYANG, 1, STR_SUBTYPE_BAYANG, STR_MULTI_TELEMETRY },
|
||||
{ MM_RF_PROTO_MT99XX, 4, STR_SUBTYPE_MT99, nullptr },
|
||||
{ MM_RF_PROTO_MJXQ, 5, STR_SUBTYPE_MJXQ, nullptr },
|
||||
{ MM_RF_PROTO_FY326, 1, STR_SUBTYPE_FY326, nullptr },
|
||||
{ MM_RF_PROTO_SFHSS, 0, NO_SUBTYPE, STR_MULTI_RFTUNE },
|
||||
{ MM_RF_PROTO_HONTAI, 2, STR_SUBTYPE_HONTAI, nullptr },
|
||||
{ MM_RF_PROTO_OLRS, 0, NO_SUBTYPE, STR_MULTI_RFPOWER },
|
||||
{ MM_RF_PROTO_FS_AFHDS2A, 3, STR_SUBTYPE_AFHDS2A, STR_MULTI_SERVOFREQ },
|
||||
{ MM_RF_PROTO_Q2X2, 2, STR_SUBTYPE_Q2X2, nullptr },
|
||||
{ MM_RF_PROTO_WK_2X01, 5, STR_SUBTYPE_WK2x01, nullptr },
|
||||
{ MM_RF_PROTO_Q303, 3, STR_SUBTYPE_Q303, nullptr },
|
||||
{ MM_RF_CUSTOM_SELECTED, 7, STR_SUBTYPE_CUSTOM, STR_MULTI_OPTION },
|
||||
{MM_RF_PROTO_FLYSKY, 4, false, STR_SUBTYPE_FLYSKY, nullptr},
|
||||
{MM_RF_PROTO_HUBSAN, 0, false, NO_SUBTYPE, STR_MULTI_VIDFREQ},
|
||||
{MM_RF_PROTO_FRSKY, 5, false, STR_SUBTYPE_FRSKY, STR_MULTI_RFTUNE},
|
||||
{MM_RF_PROTO_HISKY, 1, false, STR_SUBTYPE_HISKY, nullptr},
|
||||
{MM_RF_PROTO_V2X2, 1, false, STR_SUBTYPE_V2X2, nullptr},
|
||||
{MM_RF_PROTO_DSM2, 3, false, STR_SUBTYPE_DSM, nullptr},
|
||||
{MM_RF_PROTO_YD717, 4, false, STR_SUBTYPE_YD717, nullptr},
|
||||
{MM_RF_PROTO_KN, 1, false, STR_SUBTYPE_KN, nullptr},
|
||||
{MM_RF_PROTO_SYMAX, 1, false, STR_SUBTYPE_SYMAX, nullptr},
|
||||
{MM_RF_PROTO_SLT, 1, false, STR_SUBTYPE_SLT, nullptr},
|
||||
{MM_RF_PROTO_CX10, 7, false, STR_SUBTYPE_CX10, nullptr},
|
||||
{MM_RF_PROTO_CG023, 2, false, STR_SUBTYPE_CG023, nullptr},
|
||||
{MM_RF_PROTO_BAYANG, 2, false, STR_SUBTYPE_BAYANG, STR_MULTI_TELEMETRY},
|
||||
{MM_RF_PROTO_MT99XX, 4, false, STR_SUBTYPE_MT99, nullptr},
|
||||
{MM_RF_PROTO_MJXQ, 5, false, STR_SUBTYPE_MJXQ, nullptr},
|
||||
{MM_RF_PROTO_FY326, 1, false, STR_SUBTYPE_FY326, nullptr},
|
||||
{MM_RF_PROTO_SFHSS, 0, false, NO_SUBTYPE, STR_MULTI_RFTUNE},
|
||||
{MM_RF_PROTO_HONTAI, 2, false, STR_SUBTYPE_HONTAI, nullptr},
|
||||
{MM_RF_PROTO_OLRS, 0, false, NO_SUBTYPE, STR_MULTI_RFPOWER},
|
||||
{MM_RF_PROTO_FS_AFHDS2A, 3, true, STR_SUBTYPE_AFHDS2A, STR_MULTI_SERVOFREQ},
|
||||
{MM_RF_PROTO_Q2X2, 2, false, STR_SUBTYPE_Q2X2, nullptr},
|
||||
{MM_RF_PROTO_WK_2X01, 5, false, STR_SUBTYPE_WK2x01, nullptr},
|
||||
{MM_RF_PROTO_Q303, 3, false, STR_SUBTYPE_Q303, nullptr},
|
||||
{MM_RF_CUSTOM_SELECTED, 7, true, NO_SUBTYPE, STR_MULTI_OPTION},
|
||||
|
||||
// Sentinel and default for protocols not listed above (MM_RF_CUSTOM is 0xff)
|
||||
{ 0xfe, 0, NO_SUBTYPE, nullptr }
|
||||
{ 0xfe, 0, false, NO_SUBTYPE, nullptr}
|
||||
};
|
||||
|
||||
int Multiprotocols::MultiProtocolDefinition::getOptionMin() const {
|
||||
|
|
|
@ -35,12 +35,14 @@ class Multiprotocols
|
|||
struct radio_mm_definition {
|
||||
int protocol;
|
||||
unsigned int maxSubtype;
|
||||
bool hasFailsafe;
|
||||
QStringList protocols;
|
||||
QString optionsstr;
|
||||
};
|
||||
|
||||
struct MultiProtocolDefinition {
|
||||
const int protocol;
|
||||
const bool hasFailsafe;
|
||||
const QStringList subTypeStrings;
|
||||
const QString optionsstr;
|
||||
|
||||
|
@ -55,6 +57,7 @@ class Multiprotocols
|
|||
|
||||
MultiProtocolDefinition(const radio_mm_definition &rd) :
|
||||
protocol(rd.protocol),
|
||||
hasFailsafe(rd.hasFailsafe),
|
||||
subTypeStrings(rd.protocols),
|
||||
optionsstr(rd.optionsstr)
|
||||
{
|
||||
|
|
|
@ -71,6 +71,7 @@ enum MenuModelSetupItems {
|
|||
#if defined(MULTIMODULE)
|
||||
ITEM_MODEL_EXTERNAL_MODULE_SUBTYPE,
|
||||
ITEM_MODEL_EXTERNAL_MODULE_STATUS,
|
||||
ITEM_MODEL_EXTERNAL_MODULE_SYNCSTATUS,
|
||||
#endif
|
||||
ITEM_MODEL_EXTERNAL_MODULE_CHANNELS,
|
||||
ITEM_MODEL_EXTERNAL_MODULE_BIND,
|
||||
|
@ -216,7 +217,7 @@ void menuModelSetup(event_t event)
|
|||
LABEL(ExternalModule),
|
||||
EXTERNAL_MODULE_MODE_ROWS,
|
||||
MULTIMODULE_SUBTYPE_ROWS(EXTERNAL_MODULE)
|
||||
MULTIMODULE_STATUS_ROW
|
||||
MULTIMODULE_STATUS_ROWS
|
||||
EXTERNAL_MODULE_CHANNELS_ROWS,
|
||||
EXTERNAL_MODULE_BIND_ROWS(),
|
||||
OUTPUT_TYPE_ROWS()
|
||||
|
@ -231,7 +232,7 @@ void menuModelSetup(event_t event)
|
|||
LABEL(ExternalModule),
|
||||
EXTERNAL_MODULE_MODE_ROWS,
|
||||
MULTIMODULE_SUBTYPE_ROWS(EXTERNAL_MODULE)
|
||||
MULTIMODULE_STATUS_ROW
|
||||
MULTIMODULE_STATUS_ROWS
|
||||
EXTERNAL_MODULE_CHANNELS_ROWS,
|
||||
EXTERNAL_MODULE_BIND_ROWS(),
|
||||
OUTPUT_TYPE_ROWS()
|
||||
|
@ -1155,7 +1156,14 @@ void menuModelSetup(event_t event)
|
|||
lcdDrawText(MODEL_SETUP_2ND_COLUMN, y, statusText);
|
||||
break;
|
||||
}
|
||||
case ITEM_MODEL_EXTERNAL_MODULE_SYNCSTATUS: {
|
||||
lcdDrawTextAlignedLeft(y, STR_MODULE_SYNC);
|
||||
|
||||
char statusText[64];
|
||||
multiSyncStatus.getRefreshString(statusText);
|
||||
lcdDrawText(MODEL_SETUP_2ND_COLUMN, y, statusText);
|
||||
break;
|
||||
}
|
||||
#endif
|
||||
#endif
|
||||
|
||||
|
|
|
@ -78,6 +78,7 @@ enum MenuModelSetupItems {
|
|||
ITEM_MODEL_EXTERNAL_MODULE_MODE,
|
||||
#if defined (MULTIMODULE)
|
||||
ITEM_MODEL_EXTERNAL_MODULE_STATUS,
|
||||
ITEM_MODEL_EXTERNAL_MODULE_SYNCSTATUS,
|
||||
#endif
|
||||
ITEM_MODEL_EXTERNAL_MODULE_CHANNELS,
|
||||
ITEM_MODEL_EXTERNAL_MODULE_BIND,
|
||||
|
@ -287,7 +288,7 @@ void menuModelSetup(event_t event)
|
|||
IF_INTERNAL_MODULE_ON((IS_MODULE_XJT(INTERNAL_MODULE)) ? FAILSAFE_ROWS(INTERNAL_MODULE) : HIDDEN_ROW),
|
||||
LABEL(ExternalModule),
|
||||
EXTERNAL_MODULE_MODE_ROWS,
|
||||
MULTIMODULE_STATUS_ROW
|
||||
MULTIMODULE_STATUS_ROWS
|
||||
EXTERNAL_MODULE_CHANNELS_ROWS,
|
||||
(IS_MODULE_XJT(EXTERNAL_MODULE) && !HAS_RF_PROTOCOL_FAILSAFE(g_model.moduleData[EXTERNAL_MODULE].rfProtocol)) ? (uint8_t)1 : (IS_MODULE_PPM(EXTERNAL_MODULE) || IS_MODULE_XJT(EXTERNAL_MODULE) || IS_MODULE_DSM2(EXTERNAL_MODULE) || IS_MODULE_MULTIMODULE(EXTERNAL_MODULE)) ? (uint8_t)2 : HIDDEN_ROW,
|
||||
FAILSAFE_ROWS(EXTERNAL_MODULE), EXTERNAL_MODULE_OPTION_ROW, MULTIMODULE_MODULE_ROWS EXTERNAL_MODULE_POWER_ROW,
|
||||
|
@ -303,7 +304,7 @@ void menuModelSetup(event_t event)
|
|||
IF_INTERNAL_MODULE_ON(FAILSAFE_ROWS(INTERNAL_MODULE)),
|
||||
LABEL(ExternalModule),
|
||||
EXTERNAL_MODULE_MODE_ROWS,
|
||||
MULTIMODULE_STATUS_ROW
|
||||
MULTIMODULE_STATUS_ROWS
|
||||
EXTERNAL_MODULE_CHANNELS_ROWS,
|
||||
((IS_MODULE_XJT(EXTERNAL_MODULE) && !HAS_RF_PROTOCOL_FAILSAFE(g_model.moduleData[EXTERNAL_MODULE].rfProtocol))|| IS_MODULE_SBUS(EXTERNAL_MODULE)) ? (uint8_t)1 : (IS_MODULE_PPM(EXTERNAL_MODULE) || IS_MODULE_PXX(EXTERNAL_MODULE) || IS_MODULE_DSM2(EXTERNAL_MODULE) || IS_MODULE_MULTIMODULE(EXTERNAL_MODULE)) ? (uint8_t)2 : HIDDEN_ROW,
|
||||
FAILSAFE_ROWS(EXTERNAL_MODULE), EXTERNAL_MODULE_OPTION_ROW, MULTIMODULE_MODULE_ROWS EXTERNAL_MODULE_POWER_ROW,
|
||||
|
@ -1041,7 +1042,7 @@ void menuModelSetup(event_t event)
|
|||
case ITEM_MODEL_EXTERNAL_MODULE_FAILSAFE:
|
||||
{
|
||||
uint8_t moduleIdx = CURRENT_MODULE_EDITED(k);
|
||||
if (IS_MODULE_PXX(moduleIdx)) {
|
||||
if (IS_MODULE_XJT(moduleIdx) || IS_MODULE_MULTIMODULE(moduleIdx)) {
|
||||
ModuleData & moduleData = g_model.moduleData[moduleIdx];
|
||||
lcdDrawTextAlignedLeft(y, STR_FAILSAFE);
|
||||
lcdDrawTextAtIndex(MODEL_SETUP_2ND_COLUMN, y, STR_VFAILSAFE, moduleData.failsafeMode, menuHorizontalPosition==0 ? attr : 0);
|
||||
|
@ -1064,7 +1065,7 @@ void menuModelSetup(event_t event)
|
|||
}
|
||||
}
|
||||
else {
|
||||
lcdDrawFilledRect(MODEL_SETUP_2ND_COLUMN, y, LCD_W-MODEL_SETUP_2ND_COLUMN-MENUS_SCROLLBAR_WIDTH, 8);
|
||||
lcdDrawFilledRect(MODEL_SETUP_2ND_COLUMN, y, LCD_W - MODEL_SETUP_2ND_COLUMN - MENUS_SCROLLBAR_WIDTH, 8);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
@ -1074,13 +1075,14 @@ void menuModelSetup(event_t event)
|
|||
case ITEM_MODEL_EXTERNAL_MODULE_OPTIONS: {
|
||||
uint8_t moduleIdx = CURRENT_MODULE_EDITED(k);
|
||||
#if defined(MULTIMODULE)
|
||||
|
||||
if (IS_MODULE_MULTIMODULE(moduleIdx)) {
|
||||
int optionValue = g_model.moduleData[moduleIdx].multi.optionValue;
|
||||
|
||||
const uint8_t multi_proto = g_model.moduleData[EXTERNAL_MODULE].getMultiProtocol(true);
|
||||
const mm_protocol_definition *pdef = getMultiProtocolDefinition(multi_proto);
|
||||
if (pdef->optionsstr)
|
||||
lcdDrawTextAlignedLeft(y, pdef->optionsstr);
|
||||
lcdDrawText(INDENT_WIDTH, y, pdef->optionsstr);
|
||||
|
||||
if (multi_proto == MM_RF_PROTO_FS_AFHDS2A)
|
||||
optionValue = 50 + 5 * optionValue;
|
||||
|
@ -1144,6 +1146,14 @@ void menuModelSetup(event_t event)
|
|||
lcdDrawText(MODEL_SETUP_2ND_COLUMN, y, statusText);
|
||||
break;
|
||||
}
|
||||
case ITEM_MODEL_EXTERNAL_MODULE_SYNCSTATUS: {
|
||||
lcdDrawTextAlignedLeft(y, STR_MODULE_SYNC);
|
||||
|
||||
char statusText[64];
|
||||
multiSyncStatus.getRefreshString(statusText);
|
||||
lcdDrawText(MODEL_SETUP_2ND_COLUMN, y, statusText);
|
||||
break;
|
||||
}
|
||||
#endif
|
||||
}
|
||||
}
|
||||
|
|
|
@ -71,6 +71,7 @@ enum MenuModelSetupItems {
|
|||
ITEM_MODEL_EXTERNAL_MODULE_MODE,
|
||||
#if defined(MULTIMODULE)
|
||||
ITEM_MODEL_EXTERNAL_MODULE_STATUS,
|
||||
ITEM_MODEL_EXTERNAL_MODULE_SYNCSTATUS,
|
||||
#endif
|
||||
ITEM_MODEL_EXTERNAL_MODULE_CHANNELS,
|
||||
ITEM_MODEL_EXTERNAL_MODULE_BIND,
|
||||
|
@ -260,7 +261,7 @@ bool menuModelSetup(event_t event)
|
|||
IF_INTERNAL_MODULE_ON(0),
|
||||
LABEL(ExternalModule),
|
||||
EXTERNAL_MODULE_MODE_ROWS,
|
||||
MULTIMODULE_STATUS_ROW
|
||||
MULTIMODULE_STATUS_ROWS
|
||||
EXTERNAL_MODULE_CHANNELS_ROWS,
|
||||
((IS_MODULE_XJT(EXTERNAL_MODULE) && !HAS_RF_PROTOCOL_MODELINDEX(g_model.moduleData[EXTERNAL_MODULE].rfProtocol)) || IS_MODULE_SBUS(EXTERNAL_MODULE)) ? (uint8_t)1 : (IS_MODULE_PPM(EXTERNAL_MODULE) || IS_MODULE_XJT(EXTERNAL_MODULE) || IS_MODULE_DSM2(EXTERNAL_MODULE) || IS_MODULE_MULTIMODULE(EXTERNAL_MODULE)) ? (uint8_t)2 : HIDDEN_ROW,
|
||||
FAILSAFE_ROWS(EXTERNAL_MODULE), EXTERNAL_MODULE_OPTION_ROW, MULTIMODULE_MODULE_ROWS EXTERNAL_MODULE_POWER_ROW,
|
||||
|
@ -1043,6 +1044,14 @@ bool menuModelSetup(event_t event)
|
|||
multiModuleStatus.getStatusString(statusText);
|
||||
lcdDrawText(MODEL_SETUP_2ND_COLUMN, y, statusText);
|
||||
break;
|
||||
case ITEM_MODEL_EXTERNAL_MODULE_SYNCSTATUS: {
|
||||
lcdDrawText(MENUS_MARGIN_LEFT, y, STR_MODULE_SYNC);
|
||||
|
||||
char statusText[64];
|
||||
multiSyncStatus.getRefreshString(statusText);
|
||||
lcdDrawText(MODEL_SETUP_2ND_COLUMN, y, statusText);
|
||||
break;
|
||||
}
|
||||
}
|
||||
#endif
|
||||
}
|
||||
|
|
|
@ -120,39 +120,38 @@ void lcdDrawMMM(coord_t x, coord_t y, LcdFlags flags=0);
|
|||
|
||||
|
||||
#if defined(MULTIMODULE)
|
||||
#define MULTIMODULE_STATUS_ROW IS_MODULE_MULTIMODULE(EXTERNAL_MODULE) ? TITLE_ROW : HIDDEN_ROW,
|
||||
#define MULTIMODULE_STATUS_ROWS IS_MODULE_MULTIMODULE(EXTERNAL_MODULE) ? TITLE_ROW : HIDDEN_ROW, (IS_MODULE_MULTIMODULE(EXTERNAL_MODULE) && multiSyncStatus.isValid()) ? TITLE_ROW : HIDDEN_ROW,
|
||||
#define MULTIMODULE_MODULE_ROWS IS_MODULE_MULTIMODULE(EXTERNAL_MODULE) ? (uint8_t) 0 : HIDDEN_ROW,
|
||||
#define MULTIMODULE_MODE_ROWS(x) (g_model.moduleData[x].multi.customProto) ? (uint8_t) 3 :MULTIMODULE_HAS_SUBTYPE(g_model.moduleData[x].getMultiProtocol(true)) ? (uint8_t)2 : (uint8_t)1
|
||||
#define MULTIMODULE_RFPROTO_ROWS(x) (g_model.moduleData[x].multi.customProto) ? (uint8_t) 1 :MULTIMODULE_HAS_SUBTYPE(g_model.moduleData[x].getMultiProtocol(true)) ? (uint8_t) 0 : HIDDEN_ROW
|
||||
#define MULTIMODULE_SUBTYPE_ROWS(x) IS_MODULE_MULTIMODULE(x) ? MULTIMODULE_RFPROTO_ROWS(x) : HIDDEN_ROW,
|
||||
#define MULTIMODULE_HAS_SUBTYPE(x) (getMultiProtocolDefinition(x)->maxSubtype > 0)
|
||||
#define MULTIMODULE_HASOPTIONS(x) (getMultiProtocolDefinition(x)->optionsstr != nullptr)
|
||||
#define MULTIMODULE_OPTIONS_ROW (IS_MODULE_MULTIMODULE(EXTERNAL_MODULE) && MULTIMODULE_HASOPTIONS(g_model.moduleData[EXTERNAL_MODULE].getMultiProtocol(true))) ? (uint8_t) 0: HIDDEN_ROW
|
||||
#define MULTI_MAX_RX_NUM(x) (g_model.moduleData[x].getMultiProtocol(true) == MM_RF_PROTO_OLRS ? 4 : 15)
|
||||
#define MULTIMODULE_HASFAILSAFE(x) (IS_MODULE_MULTIMODULE(x) && getMultiProtocolDefinition(g_model.moduleData[x].getMultiProtocol(true))->failsafe)
|
||||
#define MULTIMODULE_OPTIONS_ROW (IS_MODULE_MULTIMODULE(EXTERNAL_MODULE) && MULTIMODULE_HASOPTIONS(g_model.moduleData[EXTERNAL_MODULE].getMultiProtocol(true))) ? (uint8_t) 0: HIDDEN_ROW
|
||||
|
||||
// When using packed, the pointer in here end up not being aligned, which clang and gcc complain about
|
||||
// Keep the order of the fields that the so that the size stays small
|
||||
struct mm_protocol_definition {
|
||||
uint8_t protocol;
|
||||
uint8_t maxSubtype;
|
||||
bool failsafe;
|
||||
const pm_char *subTypeString;
|
||||
const char *optionsstr;
|
||||
};
|
||||
|
||||
const mm_protocol_definition *getMultiProtocolDefinition (uint8_t protocol);
|
||||
#else
|
||||
#define MULTIMODULE_STATUS_ROW
|
||||
#define MULTIMODULE_STATUS_ROWS
|
||||
#define MULTIMODULE_MODULE_ROWS
|
||||
#define MULTIMODULE_FAILSAFEROWS(x) HIDDEN_ROW
|
||||
#define MULTIMODULE_HASFAILSAFE(x) false
|
||||
#define MULTIMODULE_SUBTYPE_ROWS(x)
|
||||
#define MULTIMODULE_MODE_ROWS(x) (uint8_t)0
|
||||
#define MULTI_MAX_RX_NUM(x) 15
|
||||
#define MULTIMODULE_OPTIONS_ROW HIDDEN_ROW
|
||||
#endif
|
||||
|
||||
// Multi failsafe is WIP
|
||||
#define MULTIMODULE_HASFAILSAFE(x) false
|
||||
|
||||
|
||||
#define MAX_RX_NUM(x) (IS_MODULE_DSM2(x) ? 20 : IS_MODULE_MULTIMODULE(x) ? MULTI_MAX_RX_NUM(x) : 63)
|
||||
#define IS_D8_RX(x) (g_model.moduleData[x].rfProtocol == RF_PROTO_D8)
|
||||
|
|
|
@ -675,38 +675,39 @@ const pm_char STR_SUBTYPE_WK2x01[] PROGMEM = "\006""WK2801""WK2401""W6_5_1""
|
|||
|
||||
const pm_char STR_SUBTYPE_V2X2[] PROGMEM = "\006""V2x2\0 ""JXD506";
|
||||
|
||||
const pm_char STR_SUBTYPE_BAYANG[] PROGMEM = "\006""Bayang""H8S3D";
|
||||
const pm_char STR_SUBTYPE_BAYANG[] PROGMEM = "\006""Bayang""H8S3D\0""X16 AH\0";
|
||||
|
||||
const pm_char STR_SUBTYPE_FY326[] PROGMEM = "\005""FY326""FY319";
|
||||
|
||||
const mm_protocol_definition multi_protocols[] = {
|
||||
{ MM_RF_PROTO_FLYSKY, 4, STR_SUBTYPE_FLYSKY, nullptr },
|
||||
{ MM_RF_PROTO_HUBSAN, 0, NO_SUBTYPE, STR_MULTI_VIDFREQ },
|
||||
{ MM_RF_PROTO_FRSKY, 5, STR_SUBTYPE_FRSKY, STR_MULTI_RFTUNE },
|
||||
{ MM_RF_PROTO_HISKY, 1, STR_SUBTYPE_HISKY, nullptr },
|
||||
{ MM_RF_PROTO_V2X2, 1, STR_SUBTYPE_V2X2, nullptr },
|
||||
{ MM_RF_PROTO_DSM2, 3, STR_SUBTYPE_DSM, nullptr },
|
||||
{ MM_RF_PROTO_YD717, 4, STR_SUBTYPE_YD717, nullptr },
|
||||
{ MM_RF_PROTO_KN, 1, STR_SUBTYPE_KN, nullptr },
|
||||
{ MM_RF_PROTO_SYMAX, 1, STR_SUBTYPE_SYMAX, nullptr },
|
||||
{ MM_RF_PROTO_SLT, 1, STR_SUBTYPE_SLT, nullptr },
|
||||
{ MM_RF_PROTO_CX10, 7, STR_SUBTYPE_CX10, nullptr },
|
||||
{ MM_RF_PROTO_CG023, 2, STR_SUBTYPE_CG023, nullptr },
|
||||
{ MM_RF_PROTO_BAYANG, 1, STR_SUBTYPE_BAYANG, STR_MULTI_TELEMETRY },
|
||||
{ MM_RF_PROTO_MT99XX, 4, STR_SUBTYPE_MT99, nullptr },
|
||||
{ MM_RF_PROTO_MJXQ, 5, STR_SUBTYPE_MJXQ, nullptr },
|
||||
{ MM_RF_PROTO_FY326, 1, STR_SUBTYPE_FY326, nullptr },
|
||||
{ MM_RF_PROTO_SFHSS, 0, NO_SUBTYPE, STR_MULTI_RFTUNE },
|
||||
{ MM_RF_PROTO_HONTAI, 2, STR_SUBTYPE_HONTAI, nullptr },
|
||||
{ MM_RF_PROTO_OLRS, 0, NO_SUBTYPE, STR_MULTI_RFPOWER },
|
||||
{ MM_RF_PROTO_FS_AFHDS2A, 3, STR_SUBTYPE_AFHDS2A, STR_MULTI_SERVOFREQ },
|
||||
{ MM_RF_PROTO_Q2X2, 2, STR_SUBTYPE_Q2X2, nullptr },
|
||||
{ MM_RF_PROTO_WK_2X01, 5, STR_SUBTYPE_WK2x01, nullptr },
|
||||
{ MM_RF_PROTO_Q303, 3, STR_SUBTYPE_Q303, nullptr },
|
||||
{ MM_RF_CUSTOM_SELECTED, 7, NO_SUBTYPE, STR_MULTI_OPTION },
|
||||
|
||||
{MM_RF_PROTO_FLYSKY, 4, false, STR_SUBTYPE_FLYSKY, nullptr},
|
||||
{MM_RF_PROTO_HUBSAN, 0, false, NO_SUBTYPE, STR_MULTI_VIDFREQ},
|
||||
{MM_RF_PROTO_FRSKY, 5, false, STR_SUBTYPE_FRSKY, STR_MULTI_RFTUNE},
|
||||
{MM_RF_PROTO_HISKY, 1, false, STR_SUBTYPE_HISKY, nullptr},
|
||||
{MM_RF_PROTO_V2X2, 1, false, STR_SUBTYPE_V2X2, nullptr},
|
||||
{MM_RF_PROTO_DSM2, 3, false, STR_SUBTYPE_DSM, nullptr},
|
||||
{MM_RF_PROTO_YD717, 4, false, STR_SUBTYPE_YD717, nullptr},
|
||||
{MM_RF_PROTO_KN, 1, false, STR_SUBTYPE_KN, nullptr},
|
||||
{MM_RF_PROTO_SYMAX, 1, false, STR_SUBTYPE_SYMAX, nullptr},
|
||||
{MM_RF_PROTO_SLT, 1, false, STR_SUBTYPE_SLT, nullptr},
|
||||
{MM_RF_PROTO_CX10, 7, false, STR_SUBTYPE_CX10, nullptr},
|
||||
{MM_RF_PROTO_CG023, 2, false, STR_SUBTYPE_CG023, nullptr},
|
||||
{MM_RF_PROTO_BAYANG, 2, false, STR_SUBTYPE_BAYANG, STR_MULTI_TELEMETRY},
|
||||
{MM_RF_PROTO_MT99XX, 4, false, STR_SUBTYPE_MT99, nullptr},
|
||||
{MM_RF_PROTO_MJXQ, 5, false, STR_SUBTYPE_MJXQ, nullptr},
|
||||
{MM_RF_PROTO_FY326, 1, false, STR_SUBTYPE_FY326, nullptr},
|
||||
{MM_RF_PROTO_SFHSS, 0, false, NO_SUBTYPE, STR_MULTI_RFTUNE},
|
||||
{MM_RF_PROTO_HONTAI, 2, false, STR_SUBTYPE_HONTAI, nullptr},
|
||||
{MM_RF_PROTO_OLRS, 0, false, NO_SUBTYPE, STR_MULTI_RFPOWER},
|
||||
{MM_RF_PROTO_FS_AFHDS2A, 3, true, STR_SUBTYPE_AFHDS2A, STR_MULTI_SERVOFREQ},
|
||||
{MM_RF_PROTO_Q2X2, 2, false, STR_SUBTYPE_Q2X2, nullptr},
|
||||
{MM_RF_PROTO_WK_2X01, 5, false, STR_SUBTYPE_WK2x01, nullptr},
|
||||
{MM_RF_PROTO_Q303, 3, false, STR_SUBTYPE_Q303, nullptr},
|
||||
{MM_RF_CUSTOM_SELECTED, 7, true, NO_SUBTYPE, STR_MULTI_OPTION},
|
||||
|
||||
// Sentinel and default for protocols not listed above (MM_RF_CUSTOM is 0xff)
|
||||
{ 0xfe, 0, NO_SUBTYPE, nullptr }
|
||||
{0xfe, 0, false, NO_SUBTYPE, nullptr}
|
||||
};
|
||||
|
||||
#undef NO_SUBTYPE
|
||||
|
|
|
@ -33,20 +33,126 @@
|
|||
|
||||
#define MULTI_CHANS 16
|
||||
#define MULTI_CHAN_BITS 11
|
||||
|
||||
void sendFrameProtocolHeader(uint8_t port);
|
||||
|
||||
void sendChannels(uint8_t port);
|
||||
|
||||
static void sendSetupFrame()
|
||||
{
|
||||
|
||||
// Old multi firmware will mark config messsages as invalid frame and throw them away
|
||||
sendByteSbus('M');
|
||||
sendByteSbus('P');
|
||||
sendByteSbus(0x80); // Module Configuration
|
||||
sendByteSbus(1); // 1 byte data
|
||||
uint8_t config = 0x1 | 0x2; // inversion + multi_telemetry
|
||||
#if !defined(PPM_PIN_SERIAL)
|
||||
config |= 0x04; //input synchronsisation
|
||||
#endif
|
||||
|
||||
sendByteSbus(config);
|
||||
}
|
||||
|
||||
static void sendFailFailsafeHeader(uint8_t port)
|
||||
{
|
||||
|
||||
// Old multi firmware will mark config messsages as invalid frame and throw them away
|
||||
sendByteSbus('M');
|
||||
sendByteSbus('P');
|
||||
sendByteSbus(0x81); // Failsafe Data
|
||||
sendByteSbus(23); // 22 byte channel + 1 byte mode
|
||||
sendByteSbus(g_model.moduleData->failsafeMode);
|
||||
}
|
||||
|
||||
static void sendFailsafeChannels(uint8_t port)
|
||||
{
|
||||
uint32_t bits = 0;
|
||||
uint8_t bitsavailable = 0;
|
||||
|
||||
for (int i = 0; i < MULTI_CHANS; i++) {
|
||||
int16_t failsafeValue = g_model.moduleData[port].failsafeChannels[i];
|
||||
int pulseValue;
|
||||
if (failsafeValue == FAILSAFE_CHANNEL_HOLD) {
|
||||
pulseValue = 0;
|
||||
}
|
||||
else if (failsafeValue == FAILSAFE_CHANNEL_NOPULSE) {
|
||||
pulseValue = 2048;
|
||||
}
|
||||
else {
|
||||
failsafeValue += 2 * PPM_CH_CENTER(g_model.moduleData[port].channelsStart + i) - 2 * PPM_CENTER;
|
||||
pulseValue = limit(1, (failsafeValue * 800 / 1000) + 1024, 2047);
|
||||
}
|
||||
|
||||
bits |= pulseValue << bitsavailable;
|
||||
bitsavailable += MULTI_CHAN_BITS;
|
||||
while (bitsavailable >= 8) {
|
||||
sendByteSbus((uint8_t) (bits & 0xff));
|
||||
bits >>= 8;
|
||||
bitsavailable -= 8;
|
||||
}
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
void setupPulsesMultimodule(uint8_t port)
|
||||
{
|
||||
static int counter=0;
|
||||
#if defined(PPM_PIN_SERIAL)
|
||||
modulePulsesData[EXTERNAL_MODULE].dsm2.serialByte = 0 ;
|
||||
modulePulsesData[EXTERNAL_MODULE].dsm2.serialBitCount = 0 ;
|
||||
#else
|
||||
modulePulsesData[EXTERNAL_MODULE].dsm2.rest = 18000; // 9ms refresh
|
||||
modulePulsesData[EXTERNAL_MODULE].dsm2.rest = multiSyncStatus.getAdjustedRefreshRate();
|
||||
modulePulsesData[EXTERNAL_MODULE].dsm2.index = 0;
|
||||
#endif
|
||||
|
||||
modulePulsesData[EXTERNAL_MODULE].dsm2.ptr = modulePulsesData[EXTERNAL_MODULE].dsm2.pulses;
|
||||
|
||||
// Every 1000 cycles (=9s) send a config packet that configures the multimodule (inversion, telemetry type)
|
||||
counter++;
|
||||
if (counter % 1000== 500) {
|
||||
sendSetupFrame();
|
||||
} else if (counter % 1000 == 0) {
|
||||
sendFailFailsafeHeader(port);
|
||||
sendFailsafeChannels(port);
|
||||
} else {
|
||||
// Normal Frame
|
||||
sendFrameProtocolHeader(port);
|
||||
sendChannels(port);
|
||||
}
|
||||
|
||||
// byte 1+2, protocol information
|
||||
putDsm2Flush();
|
||||
}
|
||||
|
||||
|
||||
void sendChannels(uint8_t port)
|
||||
{
|
||||
uint32_t bits = 0;
|
||||
uint8_t bitsavailable = 0;
|
||||
|
||||
// byte 4-25, channels 0..2047
|
||||
// Range for pulses (channelsOutputs) is [-1024:+1024] for [-100%;100%]
|
||||
// Multi uses [204;1843] as [-100%;100%]
|
||||
for (int i = 0; i < MULTI_CHANS; i++) {
|
||||
int channel = g_model.moduleData[port].channelsStart + i;
|
||||
int value = channelOutputs[channel] + 2 * PPM_CH_CENTER(channel) - 2 * PPM_CENTER;
|
||||
|
||||
// Scale to 80%
|
||||
value = value * 800 / 1000 + 1024;
|
||||
value = limit(0, value, 2047);
|
||||
|
||||
bits |= value << bitsavailable;
|
||||
bitsavailable += MULTI_CHAN_BITS;
|
||||
while (bitsavailable >= 8) {
|
||||
sendByteSbus((uint8_t) (bits & 0xff));
|
||||
bits >>= 8;
|
||||
bitsavailable -= 8;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void sendFrameProtocolHeader(uint8_t port)
|
||||
{// byte 1+2, protocol information
|
||||
|
||||
// Our enumeration starts at 0
|
||||
int type = g_model.moduleData[port].getMultiProtocol(false) + 1;
|
||||
|
@ -60,10 +166,10 @@ void setupPulsesMultimodule(uint8_t port)
|
|||
protoByte |= MULTI_SEND_RANGECHECK;
|
||||
|
||||
// rfProtocol
|
||||
if (g_model.moduleData[port].getMultiProtocol(true) == MM_RF_PROTO_DSM2){
|
||||
if (g_model.moduleData[port].getMultiProtocol(true) == MM_RF_PROTO_DSM2) {
|
||||
|
||||
// Autobinding should always be done in DSMX 11ms
|
||||
if(g_model.moduleData[port].multi.autoBindMode && moduleFlag[port] == MODULE_BIND)
|
||||
if (g_model.moduleData[port].multi.autoBindMode && moduleFlag[port] == MODULE_BIND)
|
||||
subtype = MM_RF_DSM2_SUBTYPE_AUTO;
|
||||
|
||||
// Multi module in DSM mode wants the number of channels to be used as option value
|
||||
|
@ -81,7 +187,7 @@ void setupPulsesMultimodule(uint8_t port)
|
|||
type = type + 1;
|
||||
|
||||
if (g_model.moduleData[port].getMultiProtocol(true) == MM_RF_PROTO_FRSKY) {
|
||||
if(subtype == MM_RF_FRSKY_SUBTYPE_D8) {
|
||||
if (subtype == MM_RF_FRSKY_SUBTYPE_D8) {
|
||||
//D8
|
||||
type = 3;
|
||||
subtype = 0;
|
||||
|
@ -121,7 +227,7 @@ void setupPulsesMultimodule(uint8_t port)
|
|||
|
||||
// protocol byte
|
||||
protoByte |= (type & 0x1f);
|
||||
if(g_model.moduleData[port].getMultiProtocol(true) != MM_RF_PROTO_DSM2)
|
||||
if (g_model.moduleData[port].getMultiProtocol(true) != MM_RF_PROTO_DSM2)
|
||||
protoByte |= (g_model.moduleData[port].multi.autoBindMode << 6);
|
||||
|
||||
sendByteSbus(protoByte);
|
||||
|
@ -134,27 +240,4 @@ void setupPulsesMultimodule(uint8_t port)
|
|||
|
||||
// byte 3
|
||||
sendByteSbus((uint8_t) optionValue);
|
||||
|
||||
uint32_t bits = 0;
|
||||
uint8_t bitsavailable = 0;
|
||||
|
||||
// byte 4-25, channels 0..2047
|
||||
// Range for pulses (channelsOutputs) is [-1024:+1024] for [-100%;100%]
|
||||
// Multi uses [204;1843] as [-100%;100%]
|
||||
for (int i=0; i<MULTI_CHANS; i++) {
|
||||
int channel = g_model.moduleData[port].channelsStart+i;
|
||||
int value = channelOutputs[channel] + 2*PPM_CH_CENTER(channel) - 2*PPM_CENTER;
|
||||
|
||||
// Scale to 80%
|
||||
value = value*800/1000 + 1024;
|
||||
bits |= limit(0, value, 2047) << bitsavailable;
|
||||
bitsavailable += MULTI_CHAN_BITS;
|
||||
while (bitsavailable >= 8) {
|
||||
sendByteSbus((uint8_t) (bits & 0xff));
|
||||
bits >>= 8;
|
||||
bitsavailable -= 8;
|
||||
}
|
||||
}
|
||||
|
||||
putDsm2Flush();
|
||||
}
|
||||
|
|
|
@ -409,7 +409,7 @@ char * strAppendUnsigned(char * dest, uint32_t value, uint8_t digits, uint8_t ra
|
|||
if (digits == 0) {
|
||||
unsigned int tmp = value;
|
||||
digits = 1;
|
||||
while (tmp >= 10) {
|
||||
while (tmp >= radix) {
|
||||
++digits;
|
||||
tmp /= radix;
|
||||
}
|
||||
|
|
|
@ -19,8 +19,10 @@
|
|||
*/
|
||||
#include "opentx.h"
|
||||
#include "telemetry.h"
|
||||
#include "multi.h"
|
||||
|
||||
MultiModuleStatus multiModuleStatus;
|
||||
MultiModuleSyncStatus multiSyncStatus;
|
||||
uint8_t multiBindStatus = MULTI_NORMAL_OPERATION;
|
||||
|
||||
|
||||
|
@ -31,6 +33,8 @@ enum MultiPacketTypes : uint8_t {
|
|||
SpektrumTelemetry,
|
||||
DSMBindPacket,
|
||||
FlyskyIBusTelemetry,
|
||||
ConfigCommand,
|
||||
InputSync,
|
||||
};
|
||||
|
||||
enum MultiBufferState : uint8_t {
|
||||
|
@ -69,9 +73,23 @@ static void processMultiStatusPacket(const uint8_t *data)
|
|||
|
||||
if (wasBinding && !multiModuleStatus.isBinding() && multiBindStatus == MULTI_BIND_INITIATED)
|
||||
multiBindStatus = MULTI_BIND_FINISHED;
|
||||
|
||||
}
|
||||
|
||||
static void processMultiSyncPacket(const uint8_t *data)
|
||||
{
|
||||
multiSyncStatus.lastUpdate = get_tmr10ms();
|
||||
multiSyncStatus.interval = data[4];
|
||||
multiSyncStatus.target = data[5];
|
||||
auto oldlag = multiSyncStatus.inputLag;
|
||||
|
||||
multiSyncStatus.calcAdjustedRefreshRate(data[0] << 8 | data[1], data[2] << 8 | data[3]);
|
||||
|
||||
TRACE("MP ADJ: rest: %d, lag %04d, diff: %04d target: %d, interval: %d, Refresh: %d, intAdjRefresh: %d, adjRefresh %d\r\n", modulePulsesData[EXTERNAL_MODULE].dsm2.rest,
|
||||
multiSyncStatus.inputLag, oldlag-multiSyncStatus.inputLag, multiSyncStatus.target, multiSyncStatus.interval, multiSyncStatus.refreshRate, multiSyncStatus.adjustedRefreshRate/50,
|
||||
multiSyncStatus.getAdjustedRefreshRate());
|
||||
}
|
||||
|
||||
|
||||
static void processMultiTelemetryPaket(const uint8_t *packet)
|
||||
{
|
||||
uint8_t type = packet[0];
|
||||
|
@ -112,7 +130,16 @@ static void processMultiTelemetryPaket(const uint8_t *packet)
|
|||
if (len >= 4)
|
||||
sportProcessTelemetryPacket(data);
|
||||
else
|
||||
TRACE("[MP] Received sm telemetry len %d < 4", len);
|
||||
TRACE("[MP] Received sport telemetry len %d < 4", len);
|
||||
break;
|
||||
case InputSync:
|
||||
if (len >= 6)
|
||||
processMultiSyncPacket(data);
|
||||
else
|
||||
TRACE("[MP] Received input sync len %d < 6", len);
|
||||
break;
|
||||
case ConfigCommand:
|
||||
// Just an ack to our command, ignore for now
|
||||
break;
|
||||
default:
|
||||
TRACE("[MP] Unkown multi packet type 0x%02X, len %d", type, len);
|
||||
|
@ -122,27 +149,122 @@ static void processMultiTelemetryPaket(const uint8_t *packet)
|
|||
|
||||
// sprintf does not work AVR ARM
|
||||
// use a small helper function
|
||||
static void appendInt(char* buf, uint32_t val)
|
||||
static void appendInt(char *buf, uint32_t val)
|
||||
{
|
||||
while(*buf)
|
||||
while (*buf)
|
||||
buf++;
|
||||
|
||||
int len=1;
|
||||
int32_t tmp = val / 10;
|
||||
while (tmp) {
|
||||
len++;
|
||||
tmp /= 10;
|
||||
}
|
||||
|
||||
buf[len]='\0';
|
||||
for (uint8_t i=1;i<=len; i++) {
|
||||
div_t qr = div(val, 10);
|
||||
char c = qr.rem + '0';
|
||||
buf[len - i] = c;
|
||||
val = qr.quot;
|
||||
}
|
||||
strAppendUnsigned(buf, val);
|
||||
}
|
||||
|
||||
#define MIN_REFRESH_RATE 7000
|
||||
|
||||
void MultiModuleSyncStatus::calcAdjustedRefreshRate(uint16_t newRefreshRate, uint16_t newInputLag)
|
||||
{
|
||||
// Check how far off we are from our target, positive means we are too slow, negative we are too fast
|
||||
int lagDifference = newInputLag - inputLag;
|
||||
|
||||
// The refresh rate that we target
|
||||
// Below is least common multiple of MIN_REFRESH_RATE and requested rate
|
||||
uint16_t targetRefreshRate = (uint16_t) (newRefreshRate * ((MIN_REFRESH_RATE / (newRefreshRate - 1)) + 1));
|
||||
|
||||
// Overflow, reverse sample
|
||||
if (lagDifference < -targetRefreshRate/2)
|
||||
lagDifference= -lagDifference;
|
||||
|
||||
|
||||
// Reset adjusted refresh if rate has changed
|
||||
if (newRefreshRate != refreshRate) {
|
||||
refreshRate = newRefreshRate;
|
||||
adjustedRefreshRate = targetRefreshRate;
|
||||
if (adjustedRefreshRate >= 30000)
|
||||
adjustedRefreshRate /= 2;
|
||||
|
||||
// Our refresh rate in ps
|
||||
adjustedRefreshRate*=1000;
|
||||
return;
|
||||
}
|
||||
|
||||
// Caluclate how many samples went into the reported input Lag (*10)
|
||||
int numsamples = interval * 10000 / targetRefreshRate;
|
||||
|
||||
// Convert lagDifference to ps
|
||||
lagDifference=lagDifference*1000;
|
||||
|
||||
// Calculate the time we intentionally were late/early
|
||||
if (inputLag > target*10 +30)
|
||||
lagDifference += numsamples*500;
|
||||
else if (inputLag < target*10 - 30)
|
||||
lagDifference -= numsamples*500;
|
||||
|
||||
// Caculate the time in ps each frame is to slow (positive), fast(negative)
|
||||
int perframeps = lagDifference*10/ numsamples;
|
||||
|
||||
if (perframeps > 20000)
|
||||
perframeps = 20000;
|
||||
|
||||
if (perframeps < -20000)
|
||||
perframeps = -20000;
|
||||
|
||||
adjustedRefreshRate =(adjustedRefreshRate + perframeps);
|
||||
|
||||
// Safeguards
|
||||
if (adjustedRefreshRate < 6*1000*1000)
|
||||
adjustedRefreshRate = 6*1000*1000;
|
||||
if (adjustedRefreshRate > 30*1000*1000)
|
||||
adjustedRefreshRate = 30*1000*1000;
|
||||
|
||||
inputLag = newInputLag;
|
||||
}
|
||||
|
||||
static uint8_t counter;
|
||||
|
||||
uint16_t MultiModuleSyncStatus::getAdjustedRefreshRate() {
|
||||
if (!isValid() || refreshRate == 0)
|
||||
return 18000;
|
||||
|
||||
|
||||
counter = (uint8_t) (counter + 1 % 10);
|
||||
uint16_t rate = (uint16_t) ((adjustedRefreshRate + counter * 50) / 500);
|
||||
// Check how far off we are from our target, positive means we are too slow, negative we are too fast
|
||||
if (inputLag > target*10 +30)
|
||||
return (uint16_t) (rate - 1);
|
||||
else if (inputLag < target*10 - 30)
|
||||
return (uint16_t) (rate + 1);
|
||||
else
|
||||
return rate;
|
||||
}
|
||||
|
||||
|
||||
static void prependSpaces(char * buf, int val)
|
||||
{
|
||||
while (*buf)
|
||||
buf++;
|
||||
|
||||
int k=10000;
|
||||
while(val/k==0 && k > 0)
|
||||
{
|
||||
*buf=' ';
|
||||
buf++;
|
||||
k/= 10;
|
||||
}
|
||||
*buf='\0';
|
||||
}
|
||||
|
||||
void MultiModuleSyncStatus::getRefreshString(char *statusText)
|
||||
{
|
||||
if (!isValid()) {
|
||||
return;
|
||||
}
|
||||
|
||||
strcpy(statusText, "L ");
|
||||
prependSpaces(statusText, inputLag);
|
||||
appendInt(statusText, inputLag);
|
||||
strcat(statusText, "ns R ");
|
||||
prependSpaces(statusText, adjustedRefreshRate/1000);
|
||||
appendInt(statusText, (uint32_t) (adjustedRefreshRate / 1000));
|
||||
strcat(statusText, "ns");
|
||||
}
|
||||
|
||||
void MultiModuleStatus::getStatusString(char *statusText)
|
||||
{
|
||||
|
@ -158,10 +280,12 @@ void MultiModuleStatus::getStatusString(char *statusText)
|
|||
if (!protocolValid()) {
|
||||
strcpy(statusText, STR_PROTOCOL_INVALID);
|
||||
return;
|
||||
} else if (!serialMode()) {
|
||||
}
|
||||
else if (!serialMode()) {
|
||||
strcpy(statusText, STR_MODULE_NO_SERIAL_MODE);
|
||||
return;
|
||||
} else if (!inputDetected()) {
|
||||
}
|
||||
else if (!inputDetected()) {
|
||||
strcpy(statusText, STR_MODULE_NO_INPUT);
|
||||
return;
|
||||
}
|
||||
|
@ -187,7 +311,8 @@ static void processMultiTelemetryByte(const uint8_t data)
|
|||
{
|
||||
if (telemetryRxBufferCount < TELEMETRY_RX_PACKET_SIZE) {
|
||||
telemetryRxBuffer[telemetryRxBufferCount++] = data;
|
||||
} else {
|
||||
}
|
||||
else {
|
||||
TRACE("[MP] array size %d error", telemetryRxBufferCount);
|
||||
multiTelemetryBufferState = NoProtocolDetected;
|
||||
}
|
||||
|
@ -216,12 +341,14 @@ void processMultiTelemetryData(const uint8_t data)
|
|||
case NoProtocolDetected:
|
||||
if (data == 'M') {
|
||||
multiTelemetryBufferState = MultiFirstByteReceived;
|
||||
} else if (data == 0xAA || data == 0x7e) {
|
||||
}
|
||||
else if (data == 0xAA || data == 0x7e) {
|
||||
multiTelemetryBufferState = guessProtocol();
|
||||
|
||||
// Process the first byte by the protocol
|
||||
processMultiTelemetryData(data);
|
||||
} else {
|
||||
}
|
||||
else {
|
||||
TRACE("[MP] invalid start byte 0x%02X", data);
|
||||
}
|
||||
break;
|
||||
|
@ -262,12 +389,14 @@ void processMultiTelemetryData(const uint8_t data)
|
|||
telemetryRxBufferCount = 0;
|
||||
if (data == 'P') {
|
||||
multiTelemetryBufferState = ReceivingMultiProtocol;
|
||||
} else if (data >= 5 && data <= 10) {
|
||||
}
|
||||
else if (data >= 5 && data <= 10) {
|
||||
// Protocol indented for er9x/ersky9, accept only 5-10 as packet length to have
|
||||
// a bit of validation
|
||||
multiTelemetryBufferState = ReceivingMultiStatus;
|
||||
|
||||
} else {
|
||||
}
|
||||
else {
|
||||
TRACE("[MP] invalid second byte 0x%02X", data);
|
||||
multiTelemetryBufferState = NoProtocolDetected;
|
||||
}
|
||||
|
@ -280,9 +409,9 @@ void processMultiTelemetryData(const uint8_t data)
|
|||
case ReceivingMultiStatus:
|
||||
// Ignore multi status
|
||||
telemetryRxBuffer[telemetryRxBufferCount++] = data;
|
||||
if (telemetryRxBufferCount>5) {
|
||||
if (telemetryRxBufferCount > 5) {
|
||||
processMultiStatusPacket(telemetryRxBuffer);
|
||||
telemetryRxBufferCount=0;
|
||||
telemetryRxBufferCount = 0;
|
||||
multiTelemetryBufferState = NoProtocolDetected;
|
||||
}
|
||||
}
|
||||
|
|
|
@ -86,6 +86,30 @@ Type 0x06 Flysky AFHDS2 telemetry data
|
|||
|
||||
void processMultiTelemetryData(uint8_t data);
|
||||
|
||||
// This should be put into the Module definition if other modules gain this functionality
|
||||
struct MultiModuleSyncStatus {
|
||||
uint32_t adjustedRefreshRate; // in ps
|
||||
tmr10ms_t lastUpdate;
|
||||
uint16_t refreshRate;
|
||||
uint16_t inputLag;
|
||||
uint8_t interval;
|
||||
uint8_t target;
|
||||
|
||||
inline bool isValid() {return (get_tmr10ms() - lastUpdate < 100);}
|
||||
void getRefreshString(char* refreshText);
|
||||
uint16_t getAdjustedRefreshRate();
|
||||
void calcAdjustedRefreshRate(uint16_t newRefreshRate, uint16_t newInputLag);
|
||||
|
||||
MultiModuleSyncStatus() {
|
||||
// Initialise to a valid value
|
||||
adjustedRefreshRate=9000 * 1000;
|
||||
}
|
||||
|
||||
};
|
||||
|
||||
extern MultiModuleSyncStatus multiSyncStatus;
|
||||
|
||||
|
||||
struct MultiModuleStatus {
|
||||
|
||||
uint8_t major;
|
||||
|
@ -98,10 +122,10 @@ struct MultiModuleStatus {
|
|||
|
||||
void getStatusString(char* statusText);
|
||||
|
||||
inline bool isBinding() { return flags & 0x08; }
|
||||
inline bool protocolValid() { return flags & 0x04; }
|
||||
inline bool serialMode() { return flags & 0x02; }
|
||||
inline bool inputDetected() { return flags & 0x01; }
|
||||
inline bool isBinding() { return (bool) (flags & 0x08); }
|
||||
inline bool protocolValid() { return (bool) (flags & 0x04); }
|
||||
inline bool serialMode() { return (bool) (flags & 0x02); }
|
||||
inline bool inputDetected() { return (bool) (flags & 0x01); }
|
||||
};
|
||||
|
||||
extern MultiModuleStatus multiModuleStatus;
|
||||
|
|
|
@ -486,6 +486,7 @@ const pm_char STR_MODULE_NO_TELEMETRY[] PROGMEM = TR_MODULE_NO_TELEMETRY;
|
|||
const pm_char STR_MODULE_BINDING[] PROGMEM = TR_MODULE_BINDING;
|
||||
const pm_char STR_PROTOCOL_INVALID[] PROGMEM = TR_PROTOCOL_INVALID;
|
||||
const pm_char STR_MODULE_STATUS[] PROGMEM = TR_MODULE_STATUS;
|
||||
const pm_char STR_MODULE_SYNC[] PROGMEM = TR_MODULE_SYNC;
|
||||
const pm_char STR_MULTI_SERVOFREQ[] PROGMEM = TR_MULTI_SERVOFREQ;
|
||||
#if LCD_W < 212
|
||||
const pm_char STR_SUBTYPE[] PROGMEM = TR_SUBTYPE;
|
||||
|
|
|
@ -616,6 +616,7 @@ extern const pm_char STR_MODULE_NO_TELEMETRY[];
|
|||
extern const pm_char STR_MODULE_BINDING[];
|
||||
extern const pm_char STR_PROTOCOL_INVALID[];
|
||||
extern const pm_char STR_MODULE_STATUS[];
|
||||
extern const pm_char STR_MODULE_SYNC[];
|
||||
extern const pm_char STR_MULTI_SERVOFREQ[];
|
||||
#if LCD_W < 212
|
||||
extern const pm_char STR_SUBTYPE[];
|
||||
|
|
|
@ -872,6 +872,7 @@
|
|||
#define TR_BINDING_MODE4 "Ch9-16 Telem OFF"
|
||||
#define TR_PROTOCOL_INVALID TR("Prot. invalid", "Protocol invalid")
|
||||
#define TR_MODULE_STATUS TR(INDENT "Status", INDENT "Module Status")
|
||||
#define TR_MODULE_SYNC TR(INDENT "Sync", INDENT "Module Sync")
|
||||
#define TR_MULTI_SERVOFREQ TR(INDENT "Servo rate", INDENT "Servo update rate")
|
||||
#define TR_SYNCMENU "[Sync]"
|
||||
#define TR_LIMIT INDENT"Limit"
|
||||
|
|
|
@ -893,6 +893,7 @@
|
|||
#define TR_BINDING_MODE4 "Ch9-16 Telem OFF"
|
||||
#define TR_PROTOCOL_INVALID TR("Prot. invalid", "Protocol invalid")
|
||||
#define TR_MODULE_STATUS TR(INDENT "Status", INDENT "Module Status")
|
||||
#define TR_MODULE_SYNC TR(INDENT "Sync", INDENT "Module Sync")
|
||||
#define TR_MULTI_SERVOFREQ TR(INDENT "Servo rate", INDENT "Servo update rate")
|
||||
#define TR_SYNCMENU "Sync [MENU]"
|
||||
#define TR_LIMIT INDENT "Grenzen"
|
||||
|
|
|
@ -876,6 +876,7 @@
|
|||
#define TR_BINDING_MODE4 "Ch9-16 Telem OFF"
|
||||
#define TR_PROTOCOL_INVALID TR("Prot. invalid", "Protocol invalid")
|
||||
#define TR_MODULE_STATUS TR(INDENT "Status", INDENT "Module Status")
|
||||
#define TR_MODULE_SYNC TR(INDENT "Sync", INDENT "Module Sync")
|
||||
#define TR_MULTI_SERVOFREQ TR(INDENT "Servo rate", INDENT "Servo update rate")
|
||||
#define TR_SYNCMENU "[Sync]"
|
||||
#define TR_LIMIT INDENT "Limit"
|
||||
|
|
|
@ -852,6 +852,7 @@
|
|||
#define TR_BINDING_MODE4 "Ch9-16 Telem OFF"
|
||||
#define TR_PROTOCOL_INVALID TR("Prot. invalid", "Protocol invalid")
|
||||
#define TR_MODULE_STATUS TR(INDENT "Status", INDENT "Module Status")
|
||||
#define TR_MODULE_SYNC TR(INDENT "Sync", INDENT "Module Sync")
|
||||
#define TR_MULTI_SERVOFREQ TR(INDENT "Servo rate", INDENT "Servo update rate")
|
||||
#define TR_SYNCMENU "Sync " TR_ENTER
|
||||
#define TR_LIMIT INDENT"Limite"
|
||||
|
|
|
@ -848,6 +848,7 @@
|
|||
#define TR_BINDING_MODE4 "Ch9-16 Telem OFF"
|
||||
#define TR_PROTOCOL_INVALID TR("Prot. invalid", "Protocol invalid")
|
||||
#define TR_MODULE_STATUS TR(INDENT "Status", INDENT "Module Status")
|
||||
#define TR_MODULE_SYNC TR(INDENT "Sync", INDENT "Module Sync")
|
||||
#define TR_MULTI_SERVOFREQ TR(INDENT "Servo rate", INDENT "Servo update rate")
|
||||
#define TR_SYNCMENU "[Sync]"
|
||||
#define TR_LIMIT INDENT"Limit"
|
||||
|
|
|
@ -875,6 +875,7 @@
|
|||
#define TR_BINDING_MODE4 "Ch9-16 Telem OFF"
|
||||
#define TR_PROTOCOL_INVALID TR("Sél. invalide", "Protocole invalide")
|
||||
#define TR_MODULE_STATUS TR(INDENT "Etat", INDENT "Etat module")
|
||||
#define TR_MODULE_SYNC TR(INDENT "Sync", INDENT "Module Sync")
|
||||
#define TR_MULTI_SERVOFREQ TR(INDENT "Fréq.servo", INDENT "Fréquence servos")
|
||||
#define TR_SYNCMENU "Sync [MENU]"
|
||||
#define TR_LIMIT INDENT "Limite"
|
||||
|
|
|
@ -877,6 +877,7 @@
|
|||
#define TR_BINDING_MODE4 "Ch9-16 Telem OFF"
|
||||
#define TR_PROTOCOL_INVALID TR("Prot. invalid", "Protocol invalid")
|
||||
#define TR_MODULE_STATUS TR(INDENT "Status", INDENT "Module Status")
|
||||
#define TR_MODULE_SYNC TR(INDENT "Sync", INDENT "Module Sync")
|
||||
#define TR_MULTI_SERVOFREQ TR(INDENT "Servo rate", INDENT "Servo update rate")
|
||||
#define TR_SYNCMENU "[Sync]"
|
||||
#define TR_LIMIT INDENT "Limiti"
|
||||
|
|
|
@ -875,6 +875,7 @@
|
|||
#define TR_BINDING_MODE4 "Ch9-16 Telem OFF"
|
||||
#define TR_PROTOCOL_INVALID TR("Prot. invalid", "Protocol invalid")
|
||||
#define TR_MODULE_STATUS TR(INDENT "Status", INDENT "Module Status")
|
||||
#define TR_MODULE_SYNC TR(INDENT "Sync", INDENT "Module Sync")
|
||||
#define TR_MULTI_SERVOFREQ TR(INDENT "Servo rate", INDENT "Servo update rate")
|
||||
#define TR_SYNCMENU "Sync [MENU]"
|
||||
#define TR_LIMIT INDENT "Grenzen"
|
||||
|
|
|
@ -879,6 +879,7 @@
|
|||
#define TR_BINDING_MODE4 "Ch9-16 Telem OFF"
|
||||
#define TR_PROTOCOL_INVALID TR("Prot. invalid", "Protocol invalid")
|
||||
#define TR_MODULE_STATUS TR(INDENT "Status", INDENT "Module Status")
|
||||
#define TR_MODULE_SYNC TR(INDENT "Sync", INDENT "Module Sync")
|
||||
#define TR_MULTI_SERVOFREQ TR(INDENT "Servo rate", INDENT "Servo update rate")
|
||||
#define TR_SYNCMENU "[Synch]"
|
||||
#define TR_LIMIT INDENT "Limit"
|
||||
|
|
|
@ -854,6 +854,7 @@
|
|||
#define TR_BINDING_MODE4 "Ch9-16 Telem OFF"
|
||||
#define TR_PROTOCOL_INVALID TR("Prot. invalid", "Protocol invalid")
|
||||
#define TR_MODULE_STATUS TR(INDENT "Status", INDENT "Module Status")
|
||||
#define TR_MODULE_SYNC TR(INDENT "Sync", INDENT "Module Sync")
|
||||
#define TR_MULTI_SERVOFREQ TR(INDENT "Servo rate", INDENT "Servo update rate")
|
||||
#define TR_LIMIT INDENT"Limite"
|
||||
#define TR_MINRSSI "Min Rssi"
|
||||
|
|
|
@ -891,6 +891,7 @@
|
|||
#define TR_BINDING_MODE4 "Ch9-16 Telem OFF"
|
||||
#define TR_PROTOCOL_INVALID TR("Prot. invalid", "Protocol invalid")
|
||||
#define TR_MODULE_STATUS TR(INDENT "Status", INDENT "Module Status")
|
||||
#define TR_MODULE_SYNC TR(INDENT "Sync", INDENT "Module Sync")
|
||||
#define TR_MULTI_SERVOFREQ TR(INDENT "Servo rate", INDENT "Servo update rate")
|
||||
#define TR_LIMIT INDENT "Nivå"
|
||||
#define TR_MINRSSI "Min Rssi"
|
||||
|
|
Loading…
Add table
Add a link
Reference in a new issue