Schwabe/multi telemetry (#4151)
* Add FY805 protocol * Fix stray space * Fix multi sending 120% instead of 100%. * Implement a custom telemetry for the multi module The custom telemetry allows the module to tell the telemetry protocol to OpenTX and also allows signaling the status of the Module (e.g. invalid protocol) to OpenTX This should also close #3979. * Cosmetics and a few small bugfixes * remove empty if * Move status line directly below the mode to be always visible when selecting a protocol * warn if internal RF module is on * Replace sprint with small helper function to build on AVR ARM (AR9X, 9XPRO, …) * TR in wrong order, RF_PROTO_OFF inverted * Refactor definition of Multimodule protocols to have one central place to define all multi protocol properties * Also update protocols in companion * Update multi version number to display as a.b.c.d
This commit is contained in:
committed by
Bertrand Songis
parent
d1ef22ee47
commit
06b0694f03
@@ -110,13 +110,13 @@ static void processFlySkySensor(const uint8_t *packet)
|
||||
setTelemetryValue(TELEM_PROTO_FLYSKY_IBUS, id, 0, instance, value, UNIT_RAW, 0);
|
||||
}
|
||||
|
||||
static void processFlySkyPacket(const uint8_t *packet)
|
||||
void processFlySkyPacket(const uint8_t *packet)
|
||||
{
|
||||
// Set TX RSSI Value, reverse MULTIs scaling
|
||||
setTelemetryValue(TELEM_PROTO_FLYSKY_IBUS, TX_RSSI_ID, 0, 0, packet[1], UNIT_RAW, 0);
|
||||
setTelemetryValue(TELEM_PROTO_FLYSKY_IBUS, TX_RSSI_ID, 0, 0, packet[0], UNIT_RAW, 0);
|
||||
|
||||
for (int sensor = 0; sensor < 7; sensor++) {
|
||||
int index = 2 + (4 * sensor);
|
||||
int index = 1 + (4 * sensor);
|
||||
processFlySkySensor(packet+index);
|
||||
}
|
||||
telemetryStreaming = TELEMETRY_TIMEOUT10ms;
|
||||
@@ -149,7 +149,7 @@ void processFlySkyTelemetryData(uint8_t data)
|
||||
}
|
||||
debugPrintf("\r\n");
|
||||
#endif
|
||||
processFlySkyPacket(telemetryRxBuffer);
|
||||
processFlySkyPacket(telemetryRxBuffer+1);
|
||||
telemetryRxBufferCount = 0;
|
||||
}
|
||||
}
|
||||
|
||||
@@ -24,4 +24,8 @@
|
||||
void processFlySkyTelemetryData(uint8_t data);
|
||||
void flySkySetDefault(int index, uint16_t id, uint8_t subId, uint8_t instance);
|
||||
|
||||
// Used by multi protocol
|
||||
void processFlySkyPacket(const uint8_t *packet);
|
||||
|
||||
|
||||
#endif
|
||||
|
||||
@@ -409,10 +409,10 @@ typedef enum {
|
||||
// FrSky D Telemetry Protocol
|
||||
void processHubPacket(uint8_t id, int16_t value);
|
||||
void frskyDSendNextAlarm();
|
||||
void frskyDProcessPacket(uint8_t *packet);
|
||||
void frskyDProcessPacket(const uint8_t *packet);
|
||||
|
||||
// FrSky S.PORT Telemetry Protocol
|
||||
void sportProcessTelemetryPacket(uint8_t * packet);
|
||||
void sportProcessTelemetryPacket(const uint8_t * packet);
|
||||
|
||||
void telemetryWakeup();
|
||||
void telemetryReset();
|
||||
|
||||
@@ -229,7 +229,7 @@ void parseTelemWSHowHighByte(uint8_t byte)
|
||||
}
|
||||
#endif
|
||||
|
||||
void frskyDProcessPacket(uint8_t *packet)
|
||||
void frskyDProcessPacket(const uint8_t *packet)
|
||||
{
|
||||
// What type of packet?
|
||||
switch (packet[0])
|
||||
|
||||
@@ -63,7 +63,7 @@ void parseTelemHubByte(uint8_t byte)
|
||||
}
|
||||
#endif // #if defined(FRSKY_HUB)
|
||||
|
||||
void frskyDProcessPacket(uint8_t *packet)
|
||||
void frskyDProcessPacket(const uint8_t *packet)
|
||||
{
|
||||
// What type of packet?
|
||||
switch (packet[0])
|
||||
|
||||
@@ -78,7 +78,7 @@ const FrSkySportSensor * getFrSkySportSensor(uint16_t id, uint8_t subId=0)
|
||||
return result;
|
||||
}
|
||||
|
||||
bool checkSportPacket(uint8_t *packet)
|
||||
bool checkSportPacket(const uint8_t *packet)
|
||||
{
|
||||
short crc = 0;
|
||||
for (int i=1; i<FRSKY_SPORT_PACKET_SIZE; ++i) {
|
||||
@@ -122,7 +122,7 @@ void sportProcessTelemetryPacket(uint16_t id, uint8_t subId, uint8_t instance, u
|
||||
}
|
||||
}
|
||||
|
||||
void sportProcessTelemetryPacket(uint8_t * packet)
|
||||
void sportProcessTelemetryPacket(const uint8_t * packet)
|
||||
{
|
||||
uint8_t physicalId = packet[0] & 0x1F;
|
||||
uint8_t primId = packet[1];
|
||||
|
||||
@@ -0,0 +1,274 @@
|
||||
/*
|
||||
* Copyright (C) OpenTX
|
||||
*
|
||||
* Based on code named
|
||||
* 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"
|
||||
|
||||
MultiModuleStatus multiModuleStatus;
|
||||
uint8_t multiBindStatus = MULTI_NORMAL_OPERATION;
|
||||
|
||||
|
||||
enum MultiPacketTypes : uint8_t {
|
||||
MultiStatus = 1,
|
||||
FrSkySportTelemtry,
|
||||
FrSkyHubTelemetry,
|
||||
SpektrumTelemetry,
|
||||
DSMBindPacket,
|
||||
FlyskyIBusTelemetry,
|
||||
};
|
||||
|
||||
enum MultiBufferState : uint8_t {
|
||||
NoProtocolDetected,
|
||||
MultiFirstByteReceived,
|
||||
ReceivingMultiProtocol,
|
||||
SpektrumTelemetryFallback,
|
||||
FrskyTelemetryFallbackFirstByte,
|
||||
FrskyTelemetryFallbackNextBytes,
|
||||
FlyskyTelemetryFallback
|
||||
};
|
||||
|
||||
MultiBufferState guessProtocol()
|
||||
{
|
||||
if (g_model.moduleData[EXTERNAL_MODULE].getMultiProtocol(false) == MM_RF_PROTO_DSM2)
|
||||
return SpektrumTelemetryFallback;
|
||||
else if (g_model.moduleData[EXTERNAL_MODULE].getMultiProtocol(false) == MM_RF_PROTO_FS_AFHDS2A)
|
||||
return FlyskyTelemetryFallback;
|
||||
else
|
||||
return FrskyTelemetryFallbackFirstByte;
|
||||
}
|
||||
|
||||
static void processMultiStatusPacket(const uint8_t *data)
|
||||
{
|
||||
// At least two status packets without bind flag
|
||||
bool wasBinding = (multiModuleStatus.isBinding());
|
||||
|
||||
multiModuleStatus.flags = data[0];
|
||||
multiModuleStatus.major = data[1];
|
||||
multiModuleStatus.minor = data[2];
|
||||
multiModuleStatus.revision = data[3];
|
||||
multiModuleStatus.patch = data[4];
|
||||
multiModuleStatus.lastUpdate = get_tmr10ms();
|
||||
|
||||
if (wasBinding && !multiModuleStatus.isBinding() && multiBindStatus == MULTI_BIND_INITIATED)
|
||||
multiBindStatus = MULTI_BIND_FINISHED;
|
||||
|
||||
// set moduleFlag to bind status
|
||||
/*
|
||||
if (moduleFlag[EXTERNAL_MODULE] != MODULE_RANGECHECK)
|
||||
// Two times the same status in a row to avoid race conditions
|
||||
if (multiModuleStatus.isBinding() == wasBinding) {
|
||||
multiModuleStatus.isBinding() ? moduleFlag[EXTERNAL_MODULE] = MODULE_BIND : MODULE_NORMAL_MODE;
|
||||
}
|
||||
*/
|
||||
|
||||
}
|
||||
|
||||
static void processMultiTelemetryPaket(const uint8_t *packet)
|
||||
{
|
||||
uint8_t type = packet[0];
|
||||
uint8_t len = packet[1];
|
||||
const uint8_t *data = packet + 2;
|
||||
|
||||
// Switch type
|
||||
switch (type) {
|
||||
case MultiStatus:
|
||||
if (len >= 5)
|
||||
processMultiStatusPacket(data);
|
||||
break;
|
||||
case DSMBindPacket:
|
||||
if (len >= 10)
|
||||
processDSMBindPacket(data);
|
||||
break;
|
||||
case SpektrumTelemetry:
|
||||
// processSpektrumPacket expects data[0] to be the telemetry indicator 0xAA but does not check it,
|
||||
// just send one byte of our header instead
|
||||
if (len >= 17)
|
||||
processSpektrumPacket(data - 1);
|
||||
else
|
||||
TRACE("[MP] Received spektrum telemetry len %d < 17", len);
|
||||
break;
|
||||
case FlyskyIBusTelemetry:
|
||||
if (len >= 28)
|
||||
processFlySkyPacket(data);
|
||||
else
|
||||
TRACE("[MP] Received IBUS telemetry len %d < 28", len);
|
||||
break;
|
||||
case FrSkyHubTelemetry:
|
||||
if (len >= 4)
|
||||
frskyDProcessPacket(data);
|
||||
else
|
||||
TRACE("[MP] Received Frsky HUB telemetry len %d < 4", len);
|
||||
break;
|
||||
case FrSkySportTelemtry:
|
||||
if (len >= 4)
|
||||
sportProcessTelemetryPacket(data);
|
||||
else
|
||||
TRACE("[MP] Received sm telemetry len %d < 4", len);
|
||||
break;
|
||||
default:
|
||||
TRACE("[MP] Unkown multi packet type 0x%02X, len %d", type, len);
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
// sprintf does not work AVR ARM
|
||||
// use a small helper function
|
||||
static void appendInt(char* buf, uint32_t val)
|
||||
{
|
||||
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;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
void MultiModuleStatus::getStatusString(char *statusText)
|
||||
{
|
||||
if (get_tmr10ms() - lastUpdate > 200) {
|
||||
#if defined(PCBTARANIS) || defined(PCBHORUS)
|
||||
if (g_model.moduleData[INTERNAL_MODULE].rfProtocol != RF_PROTO_OFF)
|
||||
strcpy(statusText, STR_DISABLE_INTERNAL);
|
||||
else
|
||||
#endif
|
||||
strcpy(statusText, STR_MODULE_NO_TELEMETRY);
|
||||
return;
|
||||
}
|
||||
if (!protocolValid()) {
|
||||
strcpy(statusText, STR_PROTOCOL_INVALID);
|
||||
return;
|
||||
} else if (!serialMode()) {
|
||||
strcpy(statusText, STR_MODULE_NO_SERIAL_MODE);
|
||||
return;
|
||||
} else if (!inputDetected()) {
|
||||
strcpy(statusText, STR_MODULE_NO_INPUT);
|
||||
return;
|
||||
}
|
||||
|
||||
strcpy(statusText, "V");
|
||||
appendInt(statusText, major);
|
||||
strcat(statusText, ".");
|
||||
appendInt(statusText, minor);
|
||||
strcat(statusText, ".");
|
||||
appendInt(statusText, revision);
|
||||
strcat(statusText, ".");
|
||||
appendInt(statusText, patch);
|
||||
strcat(statusText, " ");
|
||||
|
||||
if (isBinding())
|
||||
strcat(statusText, STR_MODULE_BINDING);
|
||||
}
|
||||
|
||||
|
||||
static MultiBufferState multiTelemetryBufferState;
|
||||
|
||||
static void processMultiTelemetryByte(const uint8_t data)
|
||||
{
|
||||
if (telemetryRxBufferCount < TELEMETRY_RX_PACKET_SIZE) {
|
||||
telemetryRxBuffer[telemetryRxBufferCount++] = data;
|
||||
} else {
|
||||
TRACE("[MP] array size %d error", telemetryRxBufferCount);
|
||||
multiTelemetryBufferState = NoProtocolDetected;
|
||||
}
|
||||
|
||||
// Length field does not count the header
|
||||
if (telemetryRxBufferCount >= 2 && telemetryRxBuffer[1] == telemetryRxBufferCount - 2) {
|
||||
// debug print the content of the packet
|
||||
#if 0
|
||||
debugPrintf("[MP] Packet type %02X len 0x%02X: ",
|
||||
telemetryRxBuffer[0], telemetryRxBuffer[1]);
|
||||
for (int i=0; i<(telemetryRxBufferCount+3)/4; i++) {
|
||||
debugPrintf("[%02X%02X %02X%02X] ", telemetryRxBuffer[i*4+2], telemetryRxBuffer[i*4 + 3],
|
||||
telemetryRxBuffer[i*4 + 4], telemetryRxBuffer[i*4 + 5]);
|
||||
}
|
||||
debugPrintf("\r\n");
|
||||
#endif
|
||||
// Packet is complete, process it
|
||||
processMultiTelemetryPaket(telemetryRxBuffer);
|
||||
multiTelemetryBufferState = NoProtocolDetected;
|
||||
}
|
||||
}
|
||||
|
||||
void processMultiTelemetryData(const uint8_t data)
|
||||
{
|
||||
switch (multiTelemetryBufferState) {
|
||||
case NoProtocolDetected:
|
||||
if (data == 'M') {
|
||||
multiTelemetryBufferState = MultiFirstByteReceived;
|
||||
} else if (data == 0x55 || data == 0x7e) {
|
||||
multiTelemetryBufferState = guessProtocol();
|
||||
if (multiTelemetryBufferState == FrskyTelemetryFallbackFirstByte)
|
||||
processFrskyTelemetryData(data);
|
||||
} else {
|
||||
TRACE("[MP] invalid start byte 0x%02X", data);
|
||||
}
|
||||
break;
|
||||
|
||||
case FrskyTelemetryFallbackFirstByte:
|
||||
multiTelemetryBufferState = FrskyTelemetryFallbackNextBytes;
|
||||
|
||||
case FrskyTelemetryFallbackNextBytes:
|
||||
processFrskyTelemetryData(data);
|
||||
if (data == 0x7e)
|
||||
// might start a new packet
|
||||
multiTelemetryBufferState = FrskyTelemetryFallbackFirstByte;
|
||||
else if (telemetryRxBufferCount == 0 && data != 0x7d)
|
||||
// Should be in a frame (no bytestuff), but the Frsky parser has discarded the byte
|
||||
multiTelemetryBufferState = NoProtocolDetected;
|
||||
break;
|
||||
|
||||
case FlyskyTelemetryFallback:
|
||||
processFlySkyTelemetryData(data);
|
||||
if (telemetryRxBufferCount == 0)
|
||||
multiTelemetryBufferState = NoProtocolDetected;
|
||||
break;
|
||||
|
||||
case SpektrumTelemetryFallback:
|
||||
processSpektrumTelemetryData(data);
|
||||
if (telemetryRxBufferCount == 0)
|
||||
multiTelemetryBufferState = NoProtocolDetected;
|
||||
break;
|
||||
|
||||
case MultiFirstByteReceived:
|
||||
if (data == 'P') {
|
||||
telemetryRxBufferCount = 0;
|
||||
multiTelemetryBufferState = ReceivingMultiProtocol;
|
||||
} else {
|
||||
TRACE("[MP] invalid second byte 0x%02X", data);
|
||||
multiTelemetryBufferState = NoProtocolDetected;
|
||||
}
|
||||
break;
|
||||
|
||||
case ReceivingMultiProtocol:
|
||||
processMultiTelemetryByte(data);
|
||||
break;
|
||||
}
|
||||
|
||||
}
|
||||
@@ -0,0 +1,118 @@
|
||||
/*
|
||||
* Copyright (C) OpenTX
|
||||
*
|
||||
* Based on code named
|
||||
* 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.
|
||||
*/
|
||||
|
||||
#ifndef OPENTX_MULTI_H
|
||||
#define OPENTX_MULTI_H
|
||||
|
||||
/*
|
||||
Multiprotocol telemetry definition
|
||||
|
||||
Serial: 100000 Baud 8e2 (same as input)
|
||||
|
||||
TLV Protocol (type, length, value), allows a TX to ignore unknown messages
|
||||
|
||||
Format: header (4 byte) + data (variable)
|
||||
|
||||
[0] = 'M' (0x4d)
|
||||
[1] = 'P' (0x50)
|
||||
|
||||
|
||||
The first byte is deliberatly chosen to be different from other telemetry protocols
|
||||
(e.g. 0xAA for DSM/Multi, 0xAA for FlySky and 0x7e for Frsky) to allow a TX to detect
|
||||
the telemetry format of older versions
|
||||
|
||||
[2] Type (see below)
|
||||
[3] Length (excluding the 4 header bytes)
|
||||
|
||||
[4-xx] data
|
||||
|
||||
|
||||
Type = 0x01 Multimodule Status:
|
||||
[4] Flags
|
||||
0x01 = Input signal detected
|
||||
0x02 = Serial mode enabled
|
||||
0x04 = protocol is valid
|
||||
0x08 = module is in binding mode
|
||||
[5] major
|
||||
[6] minor
|
||||
[7] revision
|
||||
[8] patchlevel,
|
||||
version of multi code, should be displayed as major.minor.revision.patchlevel
|
||||
|
||||
more information can be added by specifying a longer length of the type, the TX will just ignore these bytes
|
||||
|
||||
|
||||
Type 0x02 Frksy S.port telemetry
|
||||
Type 0x03 Frsky Hub telemetry
|
||||
|
||||
*No* usual frsky byte stuffing and without start/stop byte (0x7e)
|
||||
|
||||
|
||||
Type 0x04 Spektrum telemetry data
|
||||
data[0] RSSI
|
||||
data[1-15] telemetry data
|
||||
|
||||
Type 0x05 DSM bind data
|
||||
data[0-16] DSM bind data
|
||||
|
||||
technically DSM bind data is only 10 bytes but multi send 16
|
||||
like with telemtry, check length field)
|
||||
|
||||
Type 0x06 Flysky AFHDS2 telemetry data
|
||||
length: 29
|
||||
data[0] = RSSI value
|
||||
data[1-28] telemetry data
|
||||
|
||||
*/
|
||||
|
||||
|
||||
|
||||
void processMultiTelemetryData(uint8_t data);
|
||||
|
||||
#define MULTISTATUS_FLAG
|
||||
|
||||
struct MultiModuleStatus {
|
||||
|
||||
uint8_t major;
|
||||
uint8_t minor;
|
||||
uint8_t revision;
|
||||
uint8_t patch;
|
||||
|
||||
uint8_t flags;
|
||||
tmr10ms_t lastUpdate;
|
||||
|
||||
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; }
|
||||
};
|
||||
|
||||
extern MultiModuleStatus multiModuleStatus;
|
||||
enum MultiBindStatus : uint8_t {
|
||||
MULTI_NORMAL_OPERATION,
|
||||
MULTI_BIND_INITIATED,
|
||||
MULTI_BIND_FINISHED,
|
||||
};
|
||||
|
||||
extern uint8_t multiBindStatus;
|
||||
|
||||
#endif //OPENTX_MULTI_H
|
||||
@@ -269,7 +269,6 @@ bool isSpektrumValidValue(int32_t value, const SpektrumDataType type)
|
||||
}
|
||||
}
|
||||
|
||||
bool spektrumBindFinished = 0;
|
||||
void processSpektrumPacket(const uint8_t *packet)
|
||||
{
|
||||
setTelemetryValue(TELEM_PROTO_SPEKTRUM, (I2C_PSEUDO_TX << 8) + 0, 0, 0, packet[1], UNIT_RAW, 0);
|
||||
@@ -350,23 +349,21 @@ void processSpektrumPacket(const uint8_t *packet)
|
||||
}
|
||||
}
|
||||
|
||||
// Parse the DSM2 bind reponse, Fields are as per http://www.rcgroups.com/forums/showpost.php?p=35692146&postcount=5191
|
||||
// Parse the DSM2 bind response, Fields are as per http://www.rcgroups.com/forums/showpost.php?p=35692146&postcount=5191
|
||||
// "I" here means the multi module
|
||||
|
||||
/*
|
||||
0 0xAA -> telemetry start
|
||||
1 0x80 -> bind packet
|
||||
2-5 4 bytes -> Cyrf ID of the TX xor 0xFF but you don't care as I've checked it already...
|
||||
6 1 byte -> RX version but you don't care...
|
||||
7 1 byte -> number of channels, example 0x06=6 channels
|
||||
8 1 byte -> max DSM type allowed:
|
||||
0-3 4 bytes -> Cyrf ID of the TX xor 0xFF but you don't care as I've checked it already...
|
||||
4 1 byte -> RX version but you don't care...
|
||||
5 1 byte -> number of channels, example 0x06=6 channels
|
||||
6 1 byte -> max DSM type allowed:
|
||||
0x01 => 22ms 1024 DSM2 1 packet => number of channels is <8 and no telemetry
|
||||
0x02 => 22ms 1024 DSM2 2 packets => either a number of channel >7 or telemetry enable RX
|
||||
0x12 => 11ms 2048 DSM2 2 packets => can be any number of channels with/without telemetry -> this mode might be supported following Mike's trials, note the channels should be duplicated between the packets which is not the case today
|
||||
0xa2 => 22ms 2048 DSMX 1 packet => number of channels is <8 and no telemetry
|
||||
0xb2 => 11ms 2048 DSMX => can be any number of channels with/without telemetry -> this mode is only half supported since the channels should be duplicated between the packets which is not the case but might be supported following Mike's trials
|
||||
9 0x00: not sure of the use of this byte since I've always seen it at 0...
|
||||
10 2 bytes CRC but you don't care as I've checked it already...
|
||||
7 0x00: not sure of the use of this byte since I've always seen it at 0...
|
||||
8-9 2 bytes CRC but you don't care as I've checked it already...
|
||||
|
||||
Examples: DSM #Chan RXver
|
||||
Inductrix 0xa2 07 1
|
||||
@@ -379,16 +376,16 @@ void processDSMBindPacket(const uint8_t *packet)
|
||||
if (g_model.moduleData[EXTERNAL_MODULE].type == MODULE_TYPE_MULTIMODULE && g_model.moduleData[EXTERNAL_MODULE].getMultiProtocol(true) == MM_RF_PROTO_DSM2
|
||||
&& g_model.moduleData[EXTERNAL_MODULE].multi.autoBindMode) {
|
||||
|
||||
int channels = packet[7];
|
||||
int channels = packet[5];
|
||||
// Only sets channel etc when in DSM multi mode
|
||||
g_model.moduleData[EXTERNAL_MODULE].channelsCount = channels - 8;
|
||||
|
||||
// bool use11ms = (packet[8] & 0x10) ;
|
||||
if (packet[8] >= 0xb2)
|
||||
if (packet[6] >= 0xb2)
|
||||
g_model.moduleData[EXTERNAL_MODULE].subType = MM_RF_DSM2_SUBTYPE_DSMX_11;
|
||||
else if (packet[8] >= 0xa2)
|
||||
else if (packet[6] >= 0xa2)
|
||||
g_model.moduleData[EXTERNAL_MODULE].subType = MM_RF_DSM2_SUBTYPE_DSMX_22;
|
||||
else if (packet[8] >= 0x12)
|
||||
else if (packet[6] >= 0x12)
|
||||
g_model.moduleData[EXTERNAL_MODULE].subType = MM_RF_DSM2_SUBTYPE_DSM2_11;
|
||||
else
|
||||
g_model.moduleData[EXTERNAL_MODULE].subType = MM_RF_DSM2_SUBTYPE_DSM2_22;
|
||||
@@ -397,7 +394,7 @@ void processDSMBindPacket(const uint8_t *packet)
|
||||
|
||||
}
|
||||
|
||||
debugval = packet[9] << 24 | packet[8] << 16 | packet[7] << 8 | packet[6];
|
||||
debugval = packet[7] << 24 | packet[6] << 16 | packet[5] << 8 | packet[4];
|
||||
|
||||
/* log the bind packet as telemetry for quick debugging */
|
||||
setTelemetryValue(TELEM_PROTO_SPEKTRUM, (I2C_PSEUDO_TX << 8) + 4, 0, 0, debugval, UNIT_RAW, 0);
|
||||
@@ -405,7 +402,7 @@ void processDSMBindPacket(const uint8_t *packet)
|
||||
/* Finally stop binding as the rx just told us that it is bound */
|
||||
if (g_model.moduleData[EXTERNAL_MODULE].type == MODULE_TYPE_MULTIMODULE && g_model.moduleData[EXTERNAL_MODULE].getMultiProtocol(true) == MM_RF_PROTO_DSM2
|
||||
&& moduleFlag[EXTERNAL_MODULE] == MODULE_BIND) {
|
||||
spektrumBindFinished=true;
|
||||
multiBindStatus=MULTI_BIND_FINISHED;
|
||||
}
|
||||
}
|
||||
|
||||
@@ -425,13 +422,11 @@ void processSpektrumTelemetryData(uint8_t data)
|
||||
}
|
||||
|
||||
if (telemetryRxBuffer[1] == 0x80 && telemetryRxBufferCount >= DSM_BIND_PACKET_LENGTH) {
|
||||
processDSMBindPacket(telemetryRxBuffer);
|
||||
processDSMBindPacket(telemetryRxBuffer+2);
|
||||
telemetryRxBufferCount = 0;
|
||||
return;
|
||||
}
|
||||
|
||||
|
||||
|
||||
if (telemetryRxBufferCount >= SPEKTRUM_TELEMETRY_LENGTH) {
|
||||
// Debug print content of Telemetry to console
|
||||
#if 0
|
||||
|
||||
@@ -24,5 +24,7 @@
|
||||
void processSpektrumTelemetryData(uint8_t data);
|
||||
void spektrumSetDefault(int index, uint16_t id, uint8_t subId, uint8_t instance);
|
||||
|
||||
extern bool spektrumBindFinished;
|
||||
// Used directly by multi telemetry protocol
|
||||
void processSpektrumPacket(const uint8_t *packet);
|
||||
void processDSMBindPacket(const uint8_t *packet);
|
||||
#endif
|
||||
|
||||
@@ -74,10 +74,12 @@ void processTelemetryData(uint8_t data)
|
||||
if (telemetryProtocol == PROTOCOL_SPEKTRUM) {
|
||||
processSpektrumTelemetryData(data);
|
||||
return;
|
||||
}
|
||||
else if (telemetryProtocol == PROTOCOL_FLYSKY_IBUS) {
|
||||
} else if (telemetryProtocol == PROTOCOL_FLYSKY_IBUS) {
|
||||
processFlySkyTelemetryData(data);
|
||||
return;
|
||||
} else if (telemetryProtocol == PROTOCOL_MULTIMODULE) {
|
||||
processMultiTelemetryData(data);
|
||||
return;
|
||||
}
|
||||
#endif
|
||||
processFrskyTelemetryData(data);
|
||||
@@ -411,10 +413,13 @@ void telemetryReset()
|
||||
// we don't reset the telemetry here as we would also reset the consumption after model load
|
||||
void telemetryInit(uint8_t protocol) {
|
||||
#if defined(MULTIMODULE)
|
||||
// TODO: Is there a better way to communicate this to this function?
|
||||
if (!IS_TELEMETRY_INTERNAL_MODULE && g_model.moduleData[EXTERNAL_MODULE].type == MODULE_TYPE_MULTIMODULE) {
|
||||
if (protocol == PROTOCOL_MULTIMODULE || protocol == PROTOCOL_FLYSKY_IBUS) {
|
||||
// The DIY Multi module always speaks 100000 baud regardless of the telemetry protocol in use
|
||||
telemetryPortInit(MULTIMODULE_BAUDRATE, TELEMETRY_SERIAL_8E2);
|
||||
} else if (protocol == PROTOCOL_SPEKTRUM)
|
||||
{
|
||||
// Spektrum's own small race RX (SPM4648) uses 125000 8N1, use the same since there is no real standard
|
||||
telemetryPortInit(125000, TELEMETRY_SERIAL_8N1);
|
||||
} else
|
||||
#endif
|
||||
if (protocol == PROTOCOL_FRSKY_D) {
|
||||
|
||||
@@ -44,6 +44,7 @@
|
||||
#if defined(MULTIMODULE)
|
||||
#include "spektrum.h"
|
||||
#include "flysky_ibus.h"
|
||||
#include "multi.h"
|
||||
#endif
|
||||
|
||||
extern uint8_t telemetryStreaming; // >0 (true) == data is streaming in. 0 = no data detected for some time
|
||||
@@ -127,7 +128,12 @@ void frskyDSetDefault(int index, uint16_t id);
|
||||
#if defined(CPUARM)
|
||||
extern uint8_t telemetryProtocol;
|
||||
#define IS_FRSKY_D_PROTOCOL() (telemetryProtocol == PROTOCOL_FRSKY_D)
|
||||
#if defined (MULTIMODULE)
|
||||
#define IS_D16_MULTI() ((g_model.moduleData[EXTERNAL_MODULE].getMultiProtocol(false) == MM_RF_PROTO_FRSKY) && (g_model.moduleData[EXTERNAL_MODULE].subType == MM_RF_FRSKY_SUBTYPE_D16 || g_model.moduleData[EXTERNAL_MODULE].subType == MM_RF_FRSKY_SUBTYPE_D16_8CH))
|
||||
#define IS_FRSKY_SPORT_PROTOCOL() (telemetryProtocol == PROTOCOL_FRSKY_SPORT || (telemetryProtocol == PROTOCOL_MULTIMODULE && IS_D16_MULTI()))
|
||||
#else
|
||||
#define IS_FRSKY_SPORT_PROTOCOL() (telemetryProtocol == PROTOCOL_FRSKY_SPORT)
|
||||
#endif
|
||||
#define IS_SPEKTRUM_PROTOCOL() (telemetryProtocol == PROTOCOL_SPEKTRUM)
|
||||
#else
|
||||
#define IS_FRSKY_D_PROTOCOL() (true)
|
||||
@@ -149,15 +155,7 @@ inline uint8_t modelTelemetryProtocol()
|
||||
|
||||
#if defined(MULTIMODULE)
|
||||
if (g_model.moduleData[INTERNAL_MODULE].rfProtocol == RF_PROTO_OFF && g_model.moduleData[EXTERNAL_MODULE].type == MODULE_TYPE_MULTIMODULE) {
|
||||
if (g_model.moduleData[EXTERNAL_MODULE].getMultiProtocol(false) == MM_RF_PROTO_DSM2)
|
||||
return PROTOCOL_SPEKTRUM;
|
||||
else if (g_model.moduleData[EXTERNAL_MODULE].getMultiProtocol(false) == MM_RF_PROTO_FS_AFHDS2A)
|
||||
return PROTOCOL_FLYSKY_IBUS;
|
||||
else if ((g_model.moduleData[EXTERNAL_MODULE].getMultiProtocol(false) == MM_RF_PROTO_FRSKY) &&
|
||||
(g_model.moduleData[EXTERNAL_MODULE].subType == MM_RF_FRSKY_SUBTYPE_D16 || g_model.moduleData[EXTERNAL_MODULE].subType == MM_RF_FRSKY_SUBTYPE_D16_8CH))
|
||||
return PROTOCOL_FRSKY_SPORT;
|
||||
else
|
||||
return PROTOCOL_FRSKY_D;
|
||||
return PROTOCOL_MULTIMODULE;
|
||||
}
|
||||
#endif
|
||||
|
||||
|
||||
Reference in New Issue
Block a user