From 324feee6d708e7efc3173c999dbc830ec1961e19 Mon Sep 17 00:00:00 2001 From: Xy201207 Date: Mon, 10 Jul 2023 12:13:33 +0800 Subject: [PATCH 1/2] Fixed failsafe function and sync output of V0 version --- radio/src/pulses/afhds3.cpp | 52 +++++++++++++++++++++++--------- radio/src/pulses/afhds3_config.h | 3 +- 2 files changed, 39 insertions(+), 16 deletions(-) diff --git a/radio/src/pulses/afhds3.cpp b/radio/src/pulses/afhds3.cpp index 8c52328a0a6..549b869a5c6 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,54 @@ 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 }; + 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; + cfg->others.dirtyFlag |= (uint32_t)1<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.iBusType }; trsp.putFrame(COMMAND::SEND_COMMAND, FRAME_TYPE::REQUEST_SET_EXPECT_DATA, data, sizeof(data)); return true; } diff --git a/radio/src/pulses/afhds3_config.h b/radio/src/pulses/afhds3_config.h index 40da44edd5b..a034fc2346b 100644 --- a/radio/src/pulses/afhds3_config.h +++ b/radio/src/pulses/afhds3_config.h @@ -129,7 +129,8 @@ 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 + uint8_t iBusType; //IBUS-OUT:0; IBUS-IN:1 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 From c95e6b683cf6792865c7b85f48ceccf5242c8562 Mon Sep 17 00:00:00 2001 From: Richard Li Date: Wed, 19 Jul 2023 06:10:19 +0800 Subject: [PATCH 2/2] Make the code more readable. --- radio/src/pulses/afhds3.cpp | 25 ++++++++++++++++++++++--- radio/src/pulses/afhds3_config.h | 13 +++++++------ 2 files changed, 29 insertions(+), 9 deletions(-) diff --git a/radio/src/pulses/afhds3.cpp b/radio/src/pulses/afhds3.cpp index 549b869a5c6..de51464cc0e 100644 --- a/radio/src/pulses/afhds3.cpp +++ b/radio/src/pulses/afhds3.cpp @@ -917,6 +917,23 @@ bool ProtoState::syncSettings() if (checkDirtyFlag(DC_RX_CMD_BUS_TYPE_V0)) { TRACE("AFHDS3 [RX_CMD_BUS_TYPE_V0]"); + 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 }; @@ -926,7 +943,7 @@ bool ProtoState::syncSettings() cfg->others.ExternalBusType = 0;//These RXs only support iBUS-OUT } cfg->others.iBusType = cfg->others.ExternalBusType; - cfg->others.dirtyFlag |= (uint32_t)1<others.ExternalBusType)//SBUS { @@ -934,11 +951,13 @@ bool ProtoState::syncSettings() 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.iBusType }; + 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; } @@ -1026,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 a034fc2346b..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) @@ -130,7 +132,6 @@ PACK(struct sDATA_ConfigV1 { PACK(struct sDATA_Others { uint8_t buffer[sizeof(sDATA_ConfigV1)]; uint8_t ExternalBusType; // eEB_BusType IBUS1:0;IBUS2:1(Not supported yet);SBUS:2 - uint8_t iBusType; //IBUS-OUT:0; IBUS-IN:1 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