From cf5ab821ee1a37c04181b6019c9ffc72b66dfb71 Mon Sep 17 00:00:00 2001 From: sneone <133490859+sneone@users.noreply.github.com> Date: Fri, 5 Dec 2025 09:46:13 +0800 Subject: [PATCH] fix(afhds3): modify RF power related issues (#6831) --- .../gui/colorlcd/module/afhds3_settings.cpp | 4 ++ radio/src/pulses/afhds3.cpp | 57 ++++++++++++++++--- radio/src/pulses/afhds3.h | 2 +- radio/src/pulses/afhds3_transport.h | 1 + radio/src/telemetry/flysky_ibus.cpp | 3 + 5 files changed, 59 insertions(+), 8 deletions(-) diff --git a/radio/src/gui/colorlcd/module/afhds3_settings.cpp b/radio/src/gui/colorlcd/module/afhds3_settings.cpp index fc92654aa..97664c2f7 100644 --- a/radio/src/gui/colorlcd/module/afhds3_settings.cpp +++ b/radio/src/gui/colorlcd/module/afhds3_settings.cpp @@ -93,10 +93,14 @@ AFHDS3Settings::AFHDS3Settings(Window* parent, const FlexGridLayout& g, #if defined(RADIO_PL18U) || defined(PCBPA01) hasPowerOption = true; maxPower = AFHDS3_POWER_500; + #if defined(PCBPA01) + md->afhds3.rfPower = afhds3::get_current_rfpower_level(moduleIdx); + #endif #endif } else if (moduleIdx == EXTERNAL_MODULE) { hasPowerOption = true; maxPower = AFHDS3_FRM303_POWER_MAX; + md->afhds3.rfPower = afhds3::get_current_rfpower_level(moduleIdx); } if (hasPowerOption) { diff --git a/radio/src/pulses/afhds3.cpp b/radio/src/pulses/afhds3.cpp index 223599dea..0da31ca41 100644 --- a/radio/src/pulses/afhds3.cpp +++ b/radio/src/pulses/afhds3.cpp @@ -46,6 +46,8 @@ #define MAX_NO_OF_MODELS 20 +extern uint16_t sns_RFCurrentPower; + //get channel value outside of afhds3 namespace int32_t getChannelValue(uint8_t channel); void processFlySkyAFHDS3Sensor(const uint8_t * packet, uint8_t type); @@ -223,6 +225,8 @@ union AfhdsFrameData { CommandResult_s CommandResult; }; +static constexpr uint16_t rfpowerTable[7] = {14*4, 17*4, 20*4, 25*4, 27*4, 30*4, 33*4 }; + #define FRM302_STATUS 0x56 uint8_t receiver_type( unsigned long productnumber ); @@ -263,6 +267,7 @@ class ProtoState void applyConfigFromModel(); bool fifoFull() { return trsp.fifoFull(); } + uint16_t RFCurrentPower; protected: @@ -530,6 +535,26 @@ void ProtoState::setupFrame() return; } + // Sync settings when dirty flag is set + auto *cfg = this->getConfig(); + 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, + (uint8_t)(AFHDS3_POWER[moduleData->afhds3.rfPower]&0xFF), (uint8_t)((AFHDS3_POWER[moduleData->afhds3.rfPower]>>8)&0xFF)}; + trsp.putFrame(COMMAND::SEND_COMMAND, FRAME_TYPE::REQUEST_SET_EXPECT_DATA, data, sizeof(data)); + clearDirtyFlag(DC_RX_CMD_TX_PWR); + RFCurrentPower = (AFHDS3_POWER[moduleData->afhds3.rfPower]&0xFF); + return; + } + else if( EXTERNAL == module_index ) + { + if( !RFCurrentPower ) + { + RFCurrentPower = sns_RFCurrentPower; + } + } + if (isConnected()) { // Sync config, with commands if (syncSettings()) { return; } @@ -542,6 +567,22 @@ void ProtoState::setupFrame() } } +uint8_t get_current_rfpower_level( uint8_t module ) +{ + int16_t diff_min = protoState[module].RFCurrentPower-rfpowerTable[0]; + uint8_t power_level = 0; + for( uint8_t i=1; i<7; i++ ) + { + int16_t diff = protoState[module].RFCurrentPower-rfpowerTable[i]; + + if( abs(diff) < abs(diff_min)) + { + diff_min = diff; + power_level = i; + } + } + return power_level; +} void ProtoState::init(uint8_t moduleIndex, void* buffer, etx_module_state_t* mod_st, uint8_t fAddr) @@ -598,13 +639,6 @@ void ProtoState::setState(ModuleState state) cfg.others.isConnected = isConnected(); cfg.others.lastUpdated = get_tmr10ms(); cfg.others.dirtyFlag = 0U; - - if (state == ModuleState::STATE_SYNC_DONE) - { - // Update power config -// TRACE("Added PWM CMD"); - DIRTY_CMD((&cfg), afhds3::DirtyConfig::DC_RX_CMD_TX_PWR); - } } } @@ -656,6 +690,11 @@ void ProtoState::parseData(uint8_t* rxBuffer, uint8_t rxBufferCount) // version.productNumber, version.hardwareVersion, // version.bootloaderVersion, version.firmwareVersion); break; + case COMMAND::MODULE_RFPOWER: + { uint8_t* value = &responseFrame->value; + RFCurrentPower = (value[1]<<8) + value[0]; + } + break; case COMMAND::MODULE_STATE: // TRACE("AFHDS3 [MODULE_STATE] %02X", responseFrame->value); setState((ModuleState)responseFrame->value); @@ -683,6 +722,10 @@ void ProtoState::parseData(uint8_t* rxBuffer, uint8_t rxBufferCount) if (responseFrame->value != CMD_RESULT::SUCCESS) { setState(ModuleState::STATE_NOT_READY); } + else if( !RFCurrentPower && INTERNAL==module_index ) + { + trsp.enqueue( COMMAND::MODULE_RFPOWER, FRAME_TYPE::REQUEST_GET_DATA ); + } break; case COMMAND::MODULE_SET_CONFIG: if (responseFrame->value != CMD_RESULT::SUCCESS) { diff --git a/radio/src/pulses/afhds3.h b/radio/src/pulses/afhds3.h index 01e6acb1c..b2b66fe76 100644 --- a/radio/src/pulses/afhds3.h +++ b/radio/src/pulses/afhds3.h @@ -69,7 +69,7 @@ typedef PulsesData ExtmoduleData; #if defined(INTERNAL_MODULE_AFHDS3) typedef SerialData IntmoduleData; #endif - +uint8_t get_current_rfpower_level( uint8_t module ); extern etx_proto_driver_t ProtoDriver; void getStatusString(uint8_t module, char* buffer); diff --git a/radio/src/pulses/afhds3_transport.h b/radio/src/pulses/afhds3_transport.h index 59e4fe77e..4595ab791 100644 --- a/radio/src/pulses/afhds3_transport.h +++ b/radio/src/pulses/afhds3_transport.h @@ -48,6 +48,7 @@ enum COMMAND : uint8_t { TELEMETRY_DATA = 0x09, SEND_COMMAND = 0x0C, COMMAND_RESULT = 0x0D, + MODULE_RFPOWER = 0x13, //PA01 Internal RF module only MODULE_VERSION = 0x20, MODEL_ID = 0x2F, VIRTUAL_FAILSAFE = 0x99, // virtual command used to trigger failsafe diff --git a/radio/src/telemetry/flysky_ibus.cpp b/radio/src/telemetry/flysky_ibus.cpp index 77689a387..e7fb87667 100644 --- a/radio/src/telemetry/flysky_ibus.cpp +++ b/radio/src/telemetry/flysky_ibus.cpp @@ -202,6 +202,8 @@ const FlySkySensor flySkySensors[] = { }; // clang-format on +uint16_t sns_RFCurrentPower; + int32_t getALT(uint32_t value); inline int setFlyskyTelemetryValue( int16_t type, uint8_t instance, int32_t value, uint32_t unit, uint32_t prec) { @@ -240,6 +242,7 @@ void processFlySkyAFHDS3Sensor(const uint8_t * packet, uint8_t len ) uint8_t data2[] = { (uint8_t)(SENSOR_TYPE_RF_MODULE_VOL>>8), (uint8_t)SENSOR_TYPE_RF_MODULE_VOL, id, packet[4], packet[5] }; uint8_t data3[] = { (uint8_t)(SENSOR_TYPE_RF_MODULE_POWER>>8), (uint8_t)SENSOR_TYPE_RF_MODULE_POWER, id, packet[8], packet[9] }; // uint8_t data4[] = { (uint8_t)(SENSOR_TYPE_RF_MODULE_RAW>>8), (uint8_t)SENSOR_TYPE_RF_MODULE_RAW, id, packet[6] & 0x07}; + sns_RFCurrentPower = (packet[9]<<8)+packet[8]; processFlySkyAFHDS3Sensor(data1, 1 ); processFlySkyAFHDS3Sensor(data2, 2 );