From 7e29fc745f7110cc503477aed803866cb9c83487 Mon Sep 17 00:00:00 2001 From: Peter Feerick Date: Fri, 4 Aug 2023 16:10:56 +1000 Subject: [PATCH] fix(AFHDS3): iBus IN and Failsafe settings not updating (#3793) --- radio/src/pulses/afhds3.cpp | 73 +++++++++++++++++++++++++------- radio/src/pulses/afhds3_config.h | 14 +++--- 2 files changed, 65 insertions(+), 22 deletions(-) diff --git a/radio/src/pulses/afhds3.cpp b/radio/src/pulses/afhds3.cpp index 8c52328a0a6..734f8351ebe 100644 --- a/radio/src/pulses/afhds3.cpp +++ b/radio/src/pulses/afhds3.cpp @@ -513,7 +513,6 @@ void ProtoState::setupFrame() trsp.putFrame(COMMAND::CHANNELS_FAILSAFE_DATA, FRAME_TYPE::REQUEST_SET_NO_RESP, (uint8_t*)failSafe, AFHDS3_MAX_CHANNELS * 2 + 2); - return; } else if( isConnected() ){ uint8_t data[AFHDS3_MAX_CHANNELS*2 + 3] = { (uint8_t)(RX_CMD_FAILSAFE_VALUE&0xFF), (uint8_t)((RX_CMD_FAILSAFE_VALUE>>8)&0xFF), (uint8_t)(2*len)}; @@ -524,8 +523,8 @@ void ProtoState::setupFrame() } } else { trsp.putFrame(cmd, FRAME_TYPE::REQUEST_GET_DATA); - return; } + return; } if (isConnected()) { @@ -755,6 +754,7 @@ void ProtoState::parseData(uint8_t* rxBuffer, uint8_t rxBufferCount) } break; case RX_CMD_IBUS_DIRECTION: if(RX_CMDRESULT::RXSUCCESS==*data++) { + clearDirtyFlag(DC_RX_CMD_IBUS_DIRECTION); this->cmd_flg &= ~0x10; if( 0==cfg->version && 2==cfg->v0.ExternalBusType) this->cmd_flg |= 0x20; @@ -827,14 +827,14 @@ bool ProtoState::syncSettings() } else if( 2==receiver_type(rx_version.ProductNumber) ) { - if(this->cmd_flg&0x08) + if(this->cmd_flg&0x08)//IBUS { uint8_t data[] = { (uint8_t)(RX_CMD_BUS_TYPE_V0&0xFF), (uint8_t)((RX_CMD_BUS_TYPE_V0>>8)&0xFF), 0x1, 0 }; trsp.putFrame(COMMAND::SEND_COMMAND, FRAME_TYPE::REQUEST_SET_EXPECT_DATA, data, sizeof(data)); this->cmd_flg |= 0x10; return true; } - else if((this->cmd_flg&0x10)) + else if((this->cmd_flg&0x10))//IBUS-IN IBUS-OUT { if(busdir>=2) busdir = 0; //IBUSOUT uint8_t data1[] = { (uint8_t)(RX_CMD_IBUS_DIRECTION&0xFF), (uint8_t)((RX_CMD_IBUS_DIRECTION>>8)&0xFF), 0x1, busdir }; @@ -851,7 +851,7 @@ bool ProtoState::syncSettings() } // Sync settings when dirty flag is set - if (checkDirtyFlag(DC_RX_CMD_TX_PWR)) + if (checkDirtyFlag(DC_RX_CMD_TX_PWR)) { TRACE("AFHDS3 [RX_CMD_TX_PWR] %d", AFHDS3_POWER[moduleData->afhds3.rfPower] / 4); uint8_t data[] = { (uint8_t)(RX_CMD_TX_PWR&0xFF), (uint8_t)((RX_CMD_TX_PWR>>8)&0xFF), 2, @@ -860,7 +860,7 @@ bool ProtoState::syncSettings() clearDirtyFlag(DC_RX_CMD_TX_PWR); return true; } - if (checkDirtyFlag(DC_RX_CMD_RSSI_CHANNEL_SETUP)) + if (checkDirtyFlag(DC_RX_CMD_RSSI_CHANNEL_SETUP)) { TRACE("AFHDS3 [RX_CMD_RSSI_CHANNEL_SETUP]"); uint8_t data[] = { (uint8_t)(RX_CMD_RSSI_CHANNEL_SETUP&0xFF), (uint8_t)((RX_CMD_RSSI_CHANNEL_SETUP>>8)&0xFF), 1, cfg->v1.SignalStrengthRCChannelNb }; @@ -874,15 +874,16 @@ bool ProtoState::syncSettings() trsp.putFrame(COMMAND::SEND_COMMAND, FRAME_TYPE::REQUEST_SET_EXPECT_DATA, data, sizeof(data)); return true; } - if (checkDirtyFlag(DC_RX_CMD_FREQUENCY_V0)) + if (checkDirtyFlag(DC_RX_CMD_FREQUENCY_V0)) { TRACE("AFHDS3 [RX_CMD_FREQUENCY_V0]"); + uint16_t Frequency = ((cfg->v0.PWMFrequency.Synchronized<<15)| cfg->v0.PWMFrequency.Frequency); uint8_t data[] = { (uint8_t)(RX_CMD_FREQUENCY_V0&0xFF), (uint8_t)((RX_CMD_FREQUENCY_V0>>8)&0xFF), 2, - (uint8_t)(cfg->v0.PWMFrequency.Frequency&0xFF), (uint8_t)((cfg->v0.PWMFrequency.Frequency>>8)&0xFF) }; + (uint8_t)(Frequency&0xFF), (uint8_t)((Frequency>>8)&0xFF) }; trsp.putFrame(COMMAND::SEND_COMMAND, FRAME_TYPE::REQUEST_SET_EXPECT_DATA, data, sizeof(data)); return true; } - if (checkDirtyFlag(DC_RX_CMD_PORT_TYPE_V1)) + if (checkDirtyFlag(DC_RX_CMD_PORT_TYPE_V1)) { TRACE("AFHDS3 [RX_CMD_PORT_TYPE_V1]"); uint8_t data[] = { (uint8_t)(RX_CMD_PORT_TYPE_V1&0xFF), (uint8_t)((RX_CMD_PORT_TYPE_V1>>8)&0xFF), 4, 0, 0, 0, 0 }; @@ -890,33 +891,73 @@ bool ProtoState::syncSettings() trsp.putFrame(COMMAND::SEND_COMMAND, FRAME_TYPE::REQUEST_SET_EXPECT_DATA, data, sizeof(data)); return true; } - if (checkDirtyFlag(DC_RX_CMD_FREQUENCY_V1)) + if (checkDirtyFlag(DC_RX_CMD_FREQUENCY_V1)) { TRACE("AFHDS3 [RX_CMD_FREQUENCY_V1]"); uint8_t data[32 + 3 + 3] = { (uint8_t)(RX_CMD_FREQUENCY_V1&0xFF), (uint8_t)((RX_CMD_FREQUENCY_V1>>8)&0xFF), 32+3}; data[3] = 0; std::memcpy(&data[4], &cfg->v1.PWMFrequenciesV1.PWMFrequencies[0], 32); data[36] = cfg->v1.PWMFrequenciesV1.Synchronized & 0xff; - data[37] = (cfg->v1.PWMFrequenciesV1.Synchronized>>8) & 0xff; + data[37] = (cfg->v1.PWMFrequenciesV1.Synchronized>>8) & 0xff; trsp.putFrame(COMMAND::SEND_COMMAND, FRAME_TYPE::REQUEST_SET_EXPECT_DATA, data, sizeof(data)); DIRTY_CMD(cfg, DC_RX_CMD_FREQUENCY_V1_2); return true; } - if (checkDirtyFlag(DC_RX_CMD_FREQUENCY_V1_2)) + if (checkDirtyFlag(DC_RX_CMD_FREQUENCY_V1_2)) { TRACE("AFHDS3 [RX_CMD_FREQUENCY_V1_2]"); uint8_t data[32 + 3 + 3] = { (uint8_t)(RX_CMD_FREQUENCY_V1_2&0xFF), (uint8_t)((RX_CMD_FREQUENCY_V1_2>>8)&0xFF), 32+3}; data[3] = 1; std::memcpy(&data[4], &cfg->v1.PWMFrequenciesV1.PWMFrequencies[16], 32); data[36] = (cfg->v1.PWMFrequenciesV1.Synchronized>>16) & 0xff; - data[37] = (cfg->v1.PWMFrequenciesV1.Synchronized>>24) & 0xff; + data[37] = (cfg->v1.PWMFrequenciesV1.Synchronized>>24) & 0xff; trsp.putFrame(COMMAND::SEND_COMMAND, FRAME_TYPE::REQUEST_SET_EXPECT_DATA, data, sizeof(data)); return true; } - if (checkDirtyFlag(DC_RX_CMD_BUS_TYPE_V0)) + if (checkDirtyFlag(DC_RX_CMD_BUS_TYPE_V0)) { TRACE("AFHDS3 [RX_CMD_BUS_TYPE_V0]"); - uint8_t data[] = { (uint8_t)(RX_CMD_BUS_TYPE_V0&0xFF), (uint8_t)((RX_CMD_BUS_TYPE_V0>>8)&0xFF), 1, cfg->others.ExternalBusType }; + bool onlySupportIBUSOut = (1==receiver_type(rx_version.ProductNumber)); + + if (onlySupportIBUSOut && cfg->others.ExternalBusType == EB_BT_IBUS1_IN) + { + cfg->others.ExternalBusType = EB_BT_IBUS1_OUT; + } + uint8_t data[] = { (uint8_t)(RX_CMD_BUS_TYPE_V0&0xFF), (uint8_t)((RX_CMD_BUS_TYPE_V0>>8)&0xFF), 1, + cfg->others.ExternalBusType == EB_BT_SBUS1 ? EB_BT_SBUS1 : EB_BT_IBUS1}; + trsp.putFrame(COMMAND::SEND_COMMAND, FRAME_TYPE::REQUEST_SET_EXPECT_DATA, data, sizeof(data)); + + if (!onlySupportIBUSOut) + { + DIRTY_CMD(cfg, DC_RX_CMD_IBUS_DIRECTION); + } + return true; + +/* + if(1>=cfg->others.ExternalBusType) //IBUS1 + { + uint8_t data[] = { (uint8_t)(RX_CMD_BUS_TYPE_V0&0xFF), (uint8_t)((RX_CMD_BUS_TYPE_V0>>8)&0xFF), 1, 0 }; + trsp.putFrame(COMMAND::SEND_COMMAND, FRAME_TYPE::REQUEST_SET_EXPECT_DATA, data, sizeof(data)); + if( 1==receiver_type(rx_version.ProductNumber)) + { + cfg->others.ExternalBusType = 0;//These RXs only support iBUS-OUT + } + cfg->others.iBusType = cfg->others.ExternalBusType; + DIRTY_CMD(cfg, DC_RX_CMD_IBUS_DIRECTION); + } + else if(2==cfg->others.ExternalBusType)//SBUS + { + uint8_t data[] = { (uint8_t)(RX_CMD_BUS_TYPE_V0&0xFF), (uint8_t)((RX_CMD_BUS_TYPE_V0>>8)&0xFF), 1, cfg->others.ExternalBusType }; + trsp.putFrame(COMMAND::SEND_COMMAND, FRAME_TYPE::REQUEST_SET_EXPECT_DATA, data, sizeof(data)); + } + return true; +*/ + } + if (checkDirtyFlag(DC_RX_CMD_IBUS_DIRECTION)) + { + TRACE("AFHDS3 [RX_CMD_IBUS_DIRECTION]"); + uint8_t data[] = { (uint8_t)(RX_CMD_IBUS_DIRECTION&0xFF), (uint8_t)((RX_CMD_IBUS_DIRECTION>>8)&0xFF), 1, + cfg->others.ExternalBusType == EB_BT_IBUS1_OUT ? EB_BT_IBUS1_OUT : EB_BT_IBUS1_IN}; trsp.putFrame(COMMAND::SEND_COMMAND, FRAME_TYPE::REQUEST_SET_EXPECT_DATA, data, sizeof(data)); return true; } @@ -1004,7 +1045,7 @@ void ProtoState::applyConfigFromModel() cfg.v0.EMIStandard = moduleData->afhds3.emi; cfg.v0.IsTwoWay = moduleData->afhds3.telemetry; cfg.v0.PhyMode = moduleData->afhds3.phyMode; - cfg.v0.ExternalBusType = cfg.others.ExternalBusType==1?0:cfg.others.ExternalBusType; + cfg.v0.ExternalBusType = cfg.others.ExternalBusType==EB_BT_SBUS1 ? EB_BT_SBUS1 : EB_BT_IBUS1; // Failsafe setFailSafe(cfg.v0.FailSafe); if (moduleData->failsafeMode != FAILSAFE_NOPULSES) { diff --git a/radio/src/pulses/afhds3_config.h b/radio/src/pulses/afhds3_config.h index 40da44edd5b..e21dd079ce6 100644 --- a/radio/src/pulses/afhds3_config.h +++ b/radio/src/pulses/afhds3_config.h @@ -37,17 +37,19 @@ enum eSES_PA_SetAnalogOutput { SES_ANALOG_OUTPUT_PPM }; -enum eEB_BusType { +enum eEB_BusType : uint8_t { EB_BT_IBUS1=0, + EB_BT_IBUS1_OUT = EB_BT_IBUS1, EB_BT_IBUS2, + EB_BT_IBUS1_IN = EB_BT_IBUS2, EB_BT_SBUS1 }; -typedef enum +enum IBUS1_DIR { IBUS1_OUT, - IBUS1_IN, //Not yet supported -} eIBUS1_DIR; + IBUS1_IN, +}; // 48 bytes PACK(struct sDATA_ConfigV0 { @@ -61,7 +63,7 @@ PACK(struct sDATA_ConfigV0 { uint8_t FailsafeOutputMode; //TRUE Or FALSE sSES_PWMFrequencyV0 PWMFrequency; uint8_t AnalogOutput; // eSES_PA_SetAnalogOutput - uint8_t ExternalBusType; // eEB_BusType + eEB_BusType ExternalBusType; // eEB_BusType }); #define SES_NB_MAX_CHANNELS (32) @@ -129,7 +131,7 @@ PACK(struct sDATA_ConfigV1 { PACK(struct sDATA_Others { uint8_t buffer[sizeof(sDATA_ConfigV1)]; - uint8_t ExternalBusType; // eEB_BusType + uint8_t ExternalBusType; // eEB_BusType IBUS1:0;IBUS2:1(Not supported yet);SBUS:2 tmr10ms_t lastUpdated; // last updated time bool isConnected; // specify if receiver is connected uint32_t dirtyFlag; // mapped to commands that need to be issued to sync settings