chore(dsmp): separate DSMP and DSM2, cleanup code, add DSMP status display and Enable AETR option (#6932)
Co-authored-by: Peter Feerick <peter.feerick@gmail.com>
This commit is contained in:
@@ -357,6 +357,58 @@ static void processMultiProtoDef(uint8_t module, const uint8_t * packet, uint8_t
|
||||
#endif
|
||||
|
||||
|
||||
static void processMultiDSMBindPacket(uint8_t module, const uint8_t *packet)
|
||||
{
|
||||
if (//g_model.moduleData[module].type == MODULE_TYPE_MULTIMODULE &&
|
||||
//g_model.moduleData[module].multi.rfProtocol ==
|
||||
// MODULE_SUBTYPE_MULTI_DSM2 &&
|
||||
g_model.moduleData[module].subType == MM_RF_DSM2_SUBTYPE_AUTO) {
|
||||
// Only sets channel etc when in DSM/AUTO mode
|
||||
int channels = packet[5];
|
||||
if (channels > 12) {
|
||||
channels = 12;
|
||||
} else if (channels < 3) {
|
||||
channels = 3;
|
||||
}
|
||||
|
||||
switch (packet[6]) {
|
||||
case 0xa2:
|
||||
g_model.moduleData[module].subType = MM_RF_DSM2_SUBTYPE_DSMX_22;
|
||||
break;
|
||||
case 0x12:
|
||||
g_model.moduleData[module].subType = MM_RF_DSM2_SUBTYPE_DSM2_11;
|
||||
if (channels == 7) {
|
||||
channels = 12; // change the number of channels if 7
|
||||
}
|
||||
break;
|
||||
case 0x01:
|
||||
case 0x02:
|
||||
g_model.moduleData[module].subType = MM_RF_DSM2_SUBTYPE_DSM2_22;
|
||||
break;
|
||||
default: // 0xb2 or unknown
|
||||
g_model.moduleData[module].subType = MM_RF_DSM2_SUBTYPE_DSMX_11;
|
||||
if (channels == 7) {
|
||||
channels = 12; // change the number of channels if 7
|
||||
}
|
||||
break;
|
||||
}
|
||||
|
||||
g_model.moduleData[module].channelsCount = channels - 8;
|
||||
// clear the 11ms servo refresh rate flag
|
||||
g_model.moduleData[module].multi.optionValue &= 0xFD;
|
||||
|
||||
storageDirty(EE_MODEL);
|
||||
}
|
||||
|
||||
/* Finally stop binding as the rx just told us that it is bound */
|
||||
if (getModuleMode(module) == MODULE_MODE_BIND) {
|
||||
setMultiBindStatus(module, MULTI_BIND_FINISHED);
|
||||
}
|
||||
|
||||
// Continue with the common Spektrum Processing
|
||||
processDSMBindPacket(packet);
|
||||
}
|
||||
|
||||
static void processMultiTelemetryPaket(const uint8_t * packet, uint8_t module)
|
||||
{
|
||||
uint8_t type = packet[0];
|
||||
@@ -372,7 +424,7 @@ static void processMultiTelemetryPaket(const uint8_t * packet, uint8_t module)
|
||||
|
||||
case DSMBindPacket:
|
||||
if (len >= 10)
|
||||
processDSMBindPacket(module, data);
|
||||
processMultiDSMBindPacket(module, data);
|
||||
break;
|
||||
|
||||
case SpektrumTelemetry:
|
||||
|
||||
@@ -849,131 +849,16 @@ void processSpektrumPacket(const uint8_t *packet)
|
||||
LemonRX+Sat+tele 0xb2 07 1
|
||||
|
||||
*/
|
||||
void processDSMBindPacket(uint8_t module, const uint8_t *packet)
|
||||
void processDSMBindPacket(const uint8_t *packet)
|
||||
{
|
||||
uint32_t debugval;
|
||||
|
||||
if (g_model.moduleData[module].type == MODULE_TYPE_LEMON_DSMP) {
|
||||
|
||||
// save flags
|
||||
g_model.moduleData[module].dsmp.flags = packet[0];
|
||||
|
||||
// save number of channels
|
||||
uint8_t channels = packet[2];
|
||||
if (channels > 12) { channels = 12; }
|
||||
g_model.moduleData[module].channelsCount = channels - 8;
|
||||
|
||||
TRACE("[SPK] DSMP bind packet: 0x%X / %i",
|
||||
packet[0] & 0x3F, packet[2]);
|
||||
|
||||
storageDirty(EE_MODEL);
|
||||
|
||||
moduleState[module].mode = MODULE_MODE_NORMAL;
|
||||
restartModuleAsync(module, 50); // ~500ms
|
||||
}
|
||||
#if defined(MULTIMODULE)
|
||||
else if (g_model.moduleData[module].type == MODULE_TYPE_MULTIMODULE &&
|
||||
g_model.moduleData[module].multi.rfProtocol ==
|
||||
MODULE_SUBTYPE_MULTI_DSM2 &&
|
||||
g_model.moduleData[module].subType == MM_RF_DSM2_SUBTYPE_AUTO) {
|
||||
|
||||
// Only sets channel etc when in DSM/AUTO mode
|
||||
int channels = packet[5];
|
||||
if (channels > 12) {
|
||||
channels = 12;
|
||||
}
|
||||
else if (channels < 3) {
|
||||
channels = 3;
|
||||
}
|
||||
|
||||
switch(packet[6]) {
|
||||
case 0xa2:
|
||||
g_model.moduleData[module].subType = MM_RF_DSM2_SUBTYPE_DSMX_22;
|
||||
break;
|
||||
case 0x12:
|
||||
g_model.moduleData[module].subType = MM_RF_DSM2_SUBTYPE_DSM2_11;
|
||||
if (channels == 7) {
|
||||
channels = 12; // change the number of channels if 7
|
||||
}
|
||||
break;
|
||||
case 0x01:
|
||||
case 0x02:
|
||||
g_model.moduleData[module].subType = MM_RF_DSM2_SUBTYPE_DSM2_22;
|
||||
break;
|
||||
default: // 0xb2 or unknown
|
||||
g_model.moduleData[module].subType = MM_RF_DSM2_SUBTYPE_DSMX_11;
|
||||
if (channels == 7) {
|
||||
channels = 12; // change the number of channels if 7
|
||||
}
|
||||
break;
|
||||
}
|
||||
|
||||
g_model.moduleData[module].channelsCount = channels - 8;
|
||||
// clear the 11ms servo refresh rate flag
|
||||
g_model.moduleData[module].multi.optionValue &= 0xFD;
|
||||
|
||||
storageDirty(EE_MODEL);
|
||||
}
|
||||
#endif
|
||||
debugval = packet[7] << 24 | packet[6] << 16 | packet[5] << 8 | packet[4];
|
||||
|
||||
/* log the bind packet as telemetry for quick debugging */
|
||||
setTelemetryValue(PROTOCOL_TELEMETRY_SPEKTRUM, I2C_PSEUDO_TX_BIND, 0, 0,
|
||||
debugval, UNIT_RAW, 0);
|
||||
|
||||
/* Finally stop binding as the rx just told us that it is bound */
|
||||
if (getModuleMode(module) == MODULE_MODE_BIND) {
|
||||
auto module_type = g_model.moduleData[module].type;
|
||||
#if defined(MULTIMODULE)
|
||||
if (module_type == MODULE_TYPE_MULTIMODULE &&
|
||||
g_model.moduleData[module].multi.rfProtocol ==
|
||||
MODULE_SUBTYPE_MULTI_DSM2) {
|
||||
setMultiBindStatus(module, MULTI_BIND_FINISHED);
|
||||
} else
|
||||
#endif
|
||||
if (module_type == MODULE_TYPE_LEMON_DSMP) {
|
||||
setModuleMode(module, MODULE_MODE_NORMAL);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void processSpektrumTelemetryData(uint8_t module, uint8_t data,
|
||||
uint8_t *rxBuffer, uint8_t &rxBufferCount)
|
||||
{
|
||||
if (rxBufferCount == 0 && data != 0xAA) {
|
||||
TRACE("[SPK] invalid start byte 0x%02X", data);
|
||||
return;
|
||||
}
|
||||
|
||||
if (rxBufferCount < TELEMETRY_RX_PACKET_SIZE) {
|
||||
rxBuffer[rxBufferCount++] = data;
|
||||
}
|
||||
else {
|
||||
TRACE("[SPK] array size %d error", rxBufferCount);
|
||||
rxBufferCount = 0;
|
||||
}
|
||||
|
||||
if (rxBuffer[1] == 0x80 && rxBufferCount >= DSM_BIND_PACKET_LENGTH) {
|
||||
processDSMBindPacket(module, rxBuffer+2);
|
||||
rxBufferCount = 0;
|
||||
return;
|
||||
}
|
||||
|
||||
if (rxBufferCount >= SPEKTRUM_TELEMETRY_LENGTH) {
|
||||
// Debug print content of Telemetry to console
|
||||
#if 0
|
||||
debugPrintf("[SPK] Packet 0x%02X rssi 0x%02X: ic2 0x%02x, %02x: ",
|
||||
rxBuffer[0], rxBuffer[1], rxBuffer[2], rxBuffer[3]);
|
||||
for (int i=4; i<SPEKTRUM_TELEMETRY_LENGTH; i+=4) {
|
||||
debugPrintf("%02X%02X %02X%02X ", rxBuffer[i], rxBuffer[i + 1],
|
||||
rxBuffer[i + 2], rxBuffer[i + 3]);
|
||||
}
|
||||
debugPrintf(CRLF);
|
||||
#endif
|
||||
processSpektrumPacket(rxBuffer);
|
||||
rxBufferCount = 0;
|
||||
}
|
||||
}
|
||||
|
||||
const SpektrumSensor *getSpektrumSensor(uint16_t pseudoId)
|
||||
{
|
||||
|
||||
@@ -21,9 +21,11 @@
|
||||
|
||||
#pragma once
|
||||
|
||||
void processSpektrumTelemetryData(uint8_t module, uint8_t data, uint8_t* rxBuffer, uint8_t& rxBufferCount);
|
||||
#define SPEKTRUM_TELEMETRY_LENGTH 18
|
||||
#define DSM_BIND_PACKET_LENGTH 12
|
||||
|
||||
void spektrumSetDefault(int index, uint16_t id, uint8_t subId, uint8_t instance);
|
||||
|
||||
// Used directly by multi telemetry protocol
|
||||
// Used directly by multi and DSMP telemetry protocol
|
||||
void processSpektrumPacket(const uint8_t *packet);
|
||||
void processDSMBindPacket(uint8_t module, const uint8_t *packet);
|
||||
void processDSMBindPacket(const uint8_t *packet);
|
||||
|
||||
Reference in New Issue
Block a user