diff --git a/radio/src/gui/colorlcd/flysky_settings.cpp b/radio/src/gui/colorlcd/flysky_settings.cpp index 946698fc2..58ed89ee0 100644 --- a/radio/src/gui/colorlcd/flysky_settings.cpp +++ b/radio/src/gui/colorlcd/flysky_settings.cpp @@ -90,12 +90,12 @@ void FlySkySettings::update() { clear(); - // RX options: - auto line = newLine(&grid); - new StaticText(line, rect_t{}, STR_OPTIONS, 0, COLOR_THEME_PRIMARY1); - #if defined(AFHDS2) if (isModuleAFHDS2A(moduleIdx)) { + // RX options: + auto line = newLine(&grid); + new StaticText(line, rect_t{}, STR_OPTIONS, 0, COLOR_THEME_PRIMARY1); + new FSProtoOpts( line, [=]() { return md->flysky.mode; }, [=](uint8_t v) { md->flysky.mode = v; }); @@ -119,7 +119,7 @@ void FlySkySettings::update() if (isModuleAFHDS3(moduleIdx)) { // Status - line = newLine(&grid); + auto line = newLine(&grid); new StaticText(line, rect_t{}, STR_MODULE_STATUS, 0, COLOR_THEME_PRIMARY1); new DynamicText(line, rect_t{}, [=] { char msg[64] = ""; diff --git a/radio/src/pulses/afhds3.cpp b/radio/src/pulses/afhds3.cpp index dd6c210f1..61e675065 100644 --- a/radio/src/pulses/afhds3.cpp +++ b/radio/src/pulses/afhds3.cpp @@ -337,13 +337,6 @@ class ProtoState */ void getStatusString(char * statusText) const; - /** - * Gets actual power source and voltage - */ - void getPowerStatus(char* buffer) const; - - RUN_POWER actualRunPower() const; - /** * Sends stop command to prevent any further module operations */ @@ -371,21 +364,14 @@ class ProtoState void clearFrameData(); - void processTelemetryData(uint8_t byte, uint8_t* rxBuffer, uint8_t& rxBufferCount, uint8_t maxSize); + void processTelemetryData(uint8_t byte, uint8_t* rxBuffer, + uint8_t& rxBufferCount, uint8_t maxSize); //friendship declaration - use for passing telemetry friend void processTelemetryData(uint8_t data, uint8_t module); - /** - * Returns max power that currently can be set - use it to validate before synchronization of settings - */ - RUN_POWER getMaxRunPower() const; - - RUN_POWER getRunPower() const; - - bool isConnectedUnicast(); - - bool isConnectedMulticast(); + bool isConnected(); + bool hasTelemetry(); Transport trsp; @@ -399,11 +385,6 @@ class ProtoState */ ModuleState state; - /** - * Target mode to be set to the module one of MODULE_MODE_E - */ - uint8_t requestedModuleMode; - /** * Command count used for counting actual number of commands sent in run mode */ @@ -414,12 +395,6 @@ class ProtoState */ uint32_t cmdIndex; - /** - * Actual power source of the module - should be requested time to time - * Currently requested once - */ - enum MODULE_POWER_SOURCE powerSource; - /** * Pointer to module config - it is making operations easier and faster */ @@ -441,7 +416,7 @@ static const char* const moduleStateText[] = "Not ready", "HW Error", "Binding", - "Connecting", + "Disconnected", "Connected", "Standby", "Waiting for update", @@ -453,17 +428,9 @@ static const char* const moduleStateText[] = "HW test" }; -static const char* const powerSourceText[] = -{ - "Unknown", - "Internal", - "External" -}; - static const COMMAND periodicRequestCommands[] = { COMMAND::MODULE_STATE, - // COMMAND::MODULE_POWER_STATUS, COMMAND::MODULE_GET_CONFIG, COMMAND::VIRTUAL_FAILSAFE }; @@ -476,17 +443,6 @@ void getStatusString(uint8_t module, char* buffer) return protoState[module].getStatusString(buffer); } -void getPowerStatus(uint8_t module, char* buffer) -{ - return protoState[module].getPowerStatus(buffer); -} - -RUN_POWER getActualRunPower(uint8_t module) -{ - // return protoState[module].actualRunPower(); - return (RUN_POWER)0; -} - //friends function that can access telemetry parsing method void processTelemetryData(uint8_t data, uint8_t module) { @@ -504,12 +460,8 @@ void ProtoState::getStatusString(char* buffer) const : "Unknown"); } -void ProtoState::getPowerStatus(char* buffer) const -{ - strcpy(buffer, this->powerSource <= MODULE_POWER_SOURCE::EXTERNAL ? powerSourceText[this->powerSource] : "Unknown"); -} - -void ProtoState::processTelemetryData(uint8_t byte, uint8_t* rxBuffer, uint8_t& rxBufferCount, uint8_t maxSize) +void ProtoState::processTelemetryData(uint8_t byte, uint8_t* rxBuffer, + uint8_t& rxBufferCount, uint8_t maxSize) { if (!trsp.processTelemetryData(byte, rxBuffer, rxBufferCount, maxSize)) return; @@ -518,17 +470,17 @@ void ProtoState::processTelemetryData(uint8_t byte, uint8_t* rxBuffer, uint8_t& rxBufferCount = 0; } -bool ProtoState::isConnectedUnicast() +bool ProtoState::isConnected() { - // return cfg.config.telemetry == TELEMETRY::TELEMETRY_ENABLED && return this->state == ModuleState::STATE_SYNC_DONE; } -bool ProtoState::isConnectedMulticast() +bool ProtoState::hasTelemetry() { - // return cfg.config.telemetry == TELEMETRY::TELEMETRY_DISABLED && - // this->state == ModuleState::STATE_SYNC_RUNNING; - return false; + if (cfg.version == 0) + return cfg.v0.IsTwoWay; + else + return cfg.v1.IsTwoWay; } void ProtoState::setupFrame() @@ -556,70 +508,48 @@ void ProtoState::setupFrame() ::ModuleSettingsMode moduleMode = getModuleMode(module_index); if (moduleMode == ::ModuleSettingsMode::MODULE_MODE_BIND) { - switch (state) { - case STATE_BINDING: - // TODO: poll status? handle timeout? - // requestedModuleMode = MODULE_MODE_E::BIND; - // trsp.sendFrame(COMMAND::MODULE_MODE, FRAME_TYPE::REQUEST_SET_EXPECT_DATA, - // &requestedModuleMode, 1); - // return; - break; + if (state != STATE_BINDING) { + TRACE("AFHDS3 [BIND]"); + setConfigFromModel(); - default: - // case STATE_STANDBY: - TRACE("AFHDS3 [BIND]"); - setConfigFromModel(); + trsp.sendFrame(COMMAND::MODULE_SET_CONFIG, + FRAME_TYPE::REQUEST_SET_EXPECT_DATA, cfg.buffer, + cfg.version == 0 ? sizeof(cfg.v0) : sizeof(cfg.v1)); - trsp.sendFrame(COMMAND::MODULE_SET_CONFIG, - FRAME_TYPE::REQUEST_SET_EXPECT_DATA, cfg.buffer, - cfg.version == 0 ? sizeof(cfg.v0) : sizeof(cfg.v1)); - - requestedModuleMode = MODULE_MODE_E::BIND; - trsp.enqueue(COMMAND::MODULE_MODE, FRAME_TYPE::REQUEST_SET_EXPECT_DATA, true, - requestedModuleMode); - return; - - // default: - // // switch to standby mode first - // stop(); - // return; + trsp.enqueue(COMMAND::MODULE_MODE, FRAME_TYPE::REQUEST_SET_EXPECT_DATA, true, + (uint8_t)MODULE_MODE_E::BIND); + return; } } - // else if (moduleMode == ::ModuleSettingsMode::MODULE_MODE_RANGECHECK) { - // if (cfg.config.runPower != RUN_POWER::RUN_POWER_FIRST) { - // TRACE("AFHDS3 [RANGE CHECK]"); - // cfg.config.runPower = RUN_POWER::RUN_POWER_FIRST; - // uint8_t data[] = { 0x13, 0x20, 0x02, cfg.config.runPower, 0 }; - // TRACE("AFHDS3 SET TX POWER %d", moduleData->afhds3.runPower); - // trsp.sendFrame(COMMAND::SEND_COMMAND, FRAME_TYPE::REQUEST_SET_EXPECT_DATA, - // data, sizeof(data)); - // return; - // } - // } + else if (moduleMode == ::ModuleSettingsMode::MODULE_MODE_RANGECHECK) { + TRACE("AFHDS3 [RANGE CHECK] not supported"); + } else if (moduleMode == ::ModuleSettingsMode::MODULE_MODE_NORMAL) { // if module is ready but not started - if (this->state == ModuleState::STATE_READY || - this->state == ModuleState::STATE_STANDBY) { + if (this->state == ModuleState::STATE_READY) { + auto mode = (uint8_t)MODULE_MODE_E::STANDBY; + trsp.sendFrame(COMMAND::MODULE_MODE, FRAME_TYPE::REQUEST_SET_EXPECT_DATA, &mode, 1); + return; + } + if (this->state == ModuleState::STATE_STANDBY) { cmdCount = 0; requestInfoAndRun(true); return; } - + // exit bind - if (state == STATE_BINDING) { + if (this->state == STATE_BINDING) { TRACE("AFHDS3 [EXIT BIND]"); - requestedModuleMode = MODULE_MODE_E::RUN; - trsp.sendFrame(COMMAND::MODULE_MODE, FRAME_TYPE::REQUEST_SET_EXPECT_DATA, - &requestedModuleMode, 1); + auto mode = (uint8_t)MODULE_MODE_E::RUN; + trsp.sendFrame(COMMAND::MODULE_MODE, FRAME_TYPE::REQUEST_SET_EXPECT_DATA, &mode, 1); return; } } - bool isConnected = isConnectedUnicast() || isConnectedMulticast(); - if (cmdCount++ >= 150) { + cmdCount = 0; if (cmdIndex >= sizeof(periodicRequestCommands)) { cmdIndex = 0; @@ -627,8 +557,7 @@ void ProtoState::setupFrame() COMMAND cmd = periodicRequestCommands[cmdIndex++]; if (cmd == COMMAND::VIRTUAL_FAILSAFE) { - if (isConnected) { - if (isConnectedMulticast()) { + if (isConnected() && !hasTelemetry()) { TRACE("AFHDS ONE WAY FAILSAFE"); uint16_t failSafe[AFHDS3_MAX_CHANNELS + 1] = { ((AFHDS3_MAX_CHANNELS << 8) | CHANNELS_DATA_MODE::FAIL_SAFE), 0}; @@ -636,21 +565,15 @@ void ProtoState::setupFrame() trsp.sendFrame(COMMAND::CHANNELS_FAILSAFE_DATA, FRAME_TYPE::REQUEST_SET_NO_RESP, (uint8_t*)failSafe, AFHDS3_MAX_CHANNELS * 2 + 2); - } else { - TRACE("AFHDS TWO WAYS FAILSAFE"); - uint8_t failSafe[3 + AFHDS3_MAX_CHANNELS * 2] = { - 0x11, 0x60, AFHDS3_MAX_CHANNELS * 2}; - setFailSafe((int16_t*)(failSafe + 3)); - trsp.sendFrame(COMMAND::SEND_COMMAND, FRAME_TYPE::REQUEST_SET_EXPECT_DATA, - failSafe, 3 + AFHDS3_MAX_CHANNELS * 2); - } - } else { - trsp.sendFrame(COMMAND::MODULE_STATE, FRAME_TYPE::REQUEST_GET_DATA); + return; } } else { trsp.sendFrame(cmd, FRAME_TYPE::REQUEST_GET_DATA); + return; } - } else if (isConnected) { + } + + if (isConnected()) { sendChannelsData(); } else { //default frame - request state @@ -725,19 +648,15 @@ void ProtoState::setState(ModuleState state) void ProtoState::requestInfoAndRun(bool send) { - if (!send) { - trsp.enqueue(COMMAND::MODULE_VERSION, FRAME_TYPE::REQUEST_GET_DATA); - } + // set model ID + trsp.enqueue(COMMAND::MODEL_ID, FRAME_TYPE::REQUEST_SET_EXPECT_DATA, true, + g_model.header.modelId[module_index]); - // trsp.enqueue(COMMAND::MODULE_POWER_STATUS, FRAME_TYPE::REQUEST_GET_DATA); - - requestedModuleMode = MODULE_MODE_E::RUN; + // RUN trsp.enqueue(COMMAND::MODULE_MODE, FRAME_TYPE::REQUEST_SET_EXPECT_DATA, true, - (uint8_t)requestedModuleMode); + (uint8_t)MODULE_MODE_E::RUN); - if (send) { - trsp.sendFrame(COMMAND::MODULE_VERSION, FRAME_TYPE::REQUEST_GET_DATA); - } + if (send) { trsp.processQueue(); } } void ProtoState::parseData(uint8_t* rxBuffer, uint8_t rxBufferCount) @@ -749,7 +668,7 @@ void ProtoState::parseData(uint8_t* rxBuffer, uint8_t rxBufferCount) TRACE("AFHDS3 [MODULE_READY] %02X", responseFrame->value); if (responseFrame->value == MODULE_STATUS_READY) { setState(ModuleState::STATE_READY); - requestInfoAndRun(); + // requestInfoAndRun(); } else { setState(ModuleState::STATE_NOT_READY); @@ -770,10 +689,6 @@ void ProtoState::parseData(uint8_t* rxBuffer, uint8_t rxBufferCount) version.productNumber, version.hardwereVersion, version.bootloaderVersion, version.firmwareVersion); break; - // case COMMAND::MODULE_POWER_STATUS: - // powerSource = (enum MODULE_POWER_SOURCE) responseFrame->value; - // TRACE("AFHDS3 [MODULE_POWER_STATUS], %d", powerSource); - // break; case COMMAND::MODULE_STATE: TRACE("AFHDS3 [MODULE_STATE] %02X", responseFrame->value); setState((ModuleState)responseFrame->value); @@ -783,13 +698,6 @@ void ProtoState::parseData(uint8_t* rxBuffer, uint8_t rxBufferCount) if (responseFrame->value != CMD_RESULT::SUCCESS) { setState(ModuleState::STATE_NOT_READY); } - else { - if (requestedModuleMode == MODULE_MODE_E::RUN) { - trsp.enqueue(COMMAND::MODULE_GET_CONFIG, FRAME_TYPE::REQUEST_GET_DATA); - trsp.enqueue(COMMAND::MODULE_STATE, FRAME_TYPE::REQUEST_GET_DATA); - } - requestedModuleMode = MODULE_MODE_UNKNOWN; - } break; case COMMAND::MODULE_SET_CONFIG: if (responseFrame->value != CMD_RESULT::SUCCESS) { @@ -864,35 +772,6 @@ inline bool isPWM(uint8_t mode) return !(mode & 2); } -RUN_POWER ProtoState::getMaxRunPower() const -{ - if (powerSource == MODULE_POWER_SOURCE::EXTERNAL) { - return RUN_POWER::PLUS_33dBm; - } - - return RUN_POWER::PLUS_27dbm; -} - -RUN_POWER ProtoState::actualRunPower() const -{ - // uint8_t actualRfPower = cfg.config.runPower; - // if (getMaxRunPower() < actualRfPower) { - // actualRfPower = getMaxRunPower(); - // } - // return (RUN_POWER) actualRfPower; - return (RUN_POWER)0; -} - -RUN_POWER ProtoState::getRunPower() const -{ - // RUN_POWER targetPower = (RUN_POWER) moduleData->afhds3.runPower; - // if (getMaxRunPower() < targetPower) { - // targetPower = getMaxRunPower(); - // } - // return targetPower; - return (RUN_POWER)0; -} - bool ProtoState::syncSettings() { // RUN_POWER targetPower = getRunPower(); @@ -989,28 +868,12 @@ void ProtoState::sendChannelsData() void ProtoState::stop() { TRACE("AFHDS3 STOP"); - requestedModuleMode = MODULE_MODE_E::STANDBY; - trsp.sendFrame(COMMAND::MODULE_MODE, FRAME_TYPE::REQUEST_SET_EXPECT_DATA, - &requestedModuleMode, 1); + auto mode = (uint8_t)MODULE_MODE_E::STANDBY; + trsp.sendFrame(COMMAND::MODULE_MODE, FRAME_TYPE::REQUEST_SET_EXPECT_DATA, &mode, 1); } void ProtoState::setConfigFromModel() { - // cfg.config.bindPower = moduleData->afhds3.bindPower; - // cfg.config.runPower = getRunPower(); - // cfg.config.emiStandard = EMI_STANDARD::FCC; - // cfg.config.telemetry = moduleData->afhds3.telemetry; //always use bidirectional mode - // cfg.config.pwmFreq = moduleData->afhds3.rxFreq(); - // cfg.config.serialMode = isSbus(moduleData->afhds3.mode) - // ? SERIAL_MODE::SBUS_MODE - // : SERIAL_MODE::IBUS; - // cfg.config.pulseMode = isPWM(moduleData->afhds3.mode) ? PULSE_MODE::PWM_MODE - // : PULSE_MODE::PPM_MODE; - // // use max channels - because channel count can not be changed after bind - // cfg.config.channelCount = AFHDS3_MAX_CHANNELS; - // cfg.config.failSafeTimout = moduleData->afhds3.failsafeTimeout; - // setFailSafe(cfg.config.failSafeMode); - if (moduleData->afhds3.phyMode < ROUTINE_FLCR1_18CH) { cfg.version = 0; } else { diff --git a/radio/src/pulses/afhds3.h b/radio/src/pulses/afhds3.h index b78ff425f..b035df0f3 100644 --- a/radio/src/pulses/afhds3.h +++ b/radio/src/pulses/afhds3.h @@ -77,22 +77,7 @@ extern etx_module_driver_t externalDriver; extern etx_module_driver_t internalDriver; #endif -enum RUN_POWER -{ - PLUS_15bBm = 0x00, - RUN_POWER_FIRST = PLUS_15bBm, - PLUS_20bBm = 0x01, - PLUS_27dbm = 0x02, - PLUS_30dBm = 0x03, - PLUS_33dBm = 0x04, - RUN_POWER_LAST = PLUS_33dBm, -}; - void getStatusString(uint8_t module, char* buffer); -void getPowerStatus(uint8_t module, char* buffer); - -RUN_POWER getActualRunPower(uint8_t module); - void processTelemetryData(uint8_t data, uint8_t module); } // namespace afhds3 diff --git a/radio/src/pulses/afhds3_module.h b/radio/src/pulses/afhds3_module.h index 93c51e260..03fa9562a 100644 --- a/radio/src/pulses/afhds3_module.h +++ b/radio/src/pulses/afhds3_module.h @@ -47,6 +47,8 @@ #define AFHDS3_SOFTSERIAL_COMMAND_TIMEOUT 15 #endif +#define AFHDS3_MAX_MODEL_ID 19 + namespace afhds3 { diff --git a/radio/src/pulses/afhds3_transport.cpp b/radio/src/pulses/afhds3_transport.cpp index 83c35910b..1ecff9865 100644 --- a/radio/src/pulses/afhds3_transport.cpp +++ b/radio/src/pulses/afhds3_transport.cpp @@ -416,13 +416,17 @@ bool Transport::handleReply(uint8_t* buffer, uint8_t len) return true; } - TRACE("AFHDS3 [QUEUE ACK] cmd %02X type %02X number %02X", + TRACE("AFHDS3 [SEND ACK] cmd %02X type %02X number %02X", responseFrame->command, responseFrame->frameType, responseFrame->frameNumber); + // TODO: send ACK immediately !!! + // trsp.putFrame(command, FRAME_TYPE::RESPONSE_ACK, nullptr, 0, + // responseFrame->frameNumber); + // return true; + fifo.enqueueACK((enum COMMAND)responseFrame->command, responseFrame->frameNumber); - // TODO: send ACK immediately !!! } else if (responseFrame->frameType == FRAME_TYPE::RESPONSE_DATA || responseFrame->frameType == FRAME_TYPE::RESPONSE_ACK) { diff --git a/radio/src/pulses/afhds3_transport.h b/radio/src/pulses/afhds3_transport.h index c507ccb1a..3d16611cd 100644 --- a/radio/src/pulses/afhds3_transport.h +++ b/radio/src/pulses/afhds3_transport.h @@ -60,9 +60,8 @@ enum COMMAND: uint8_t TELEMETRY_DATA = 0x09, SEND_COMMAND = 0x0C, COMMAND_RESULT = 0x0D, - // MODULE_POWER_STATUS = 0x0F, - // MODULE_VERSION = 0x1F, MODULE_VERSION = 0x20, + MODEL_ID = 0x2F, VIRTUAL_FAILSAFE = 0x99, // virtual command used to trigger failsafe UNDEFINED = 0xFF }; diff --git a/radio/src/pulses/modules_helpers.h b/radio/src/pulses/modules_helpers.h index 7b357bc6e..350f18304 100644 --- a/radio/src/pulses/modules_helpers.h +++ b/radio/src/pulses/modules_helpers.h @@ -36,6 +36,10 @@ extern uint32_t NV14internalModuleFwVersion; #endif +#if defined(AFHDS3) +#include "pulses/afhds3_module.h" +#endif + #define CROSSFIRE_CHANNELS_COUNT 16 #define GHOST_CHANNELS_COUNT 16 @@ -584,6 +588,9 @@ inline bool isModuleModelIndexAvailable(uint8_t idx) if (isModuleCrossfire(idx)) return true; + if (isModuleAFHDS3(idx)) + return true; + return false; } @@ -670,6 +677,10 @@ inline uint8_t getMaxRxNum(uint8_t idx) } #endif +#if defined(AFHDS3) + if (isModuleAFHDS3(idx)) return AFHDS3_MAX_MODEL_ID; +#endif + return MAX_RXNUM; } diff --git a/radio/src/pulses/pulses.cpp b/radio/src/pulses/pulses.cpp index dd7adc1de..7a057687a 100755 --- a/radio/src/pulses/pulses.cpp +++ b/radio/src/pulses/pulses.cpp @@ -192,27 +192,13 @@ void getModuleSyncStatusString(uint8_t moduleIdx, char * statusText) getModuleSyncStatus(moduleIdx).getRefreshString(statusText); } #endif -#if defined(AFHDS3) - if (isModuleAFHDS3(moduleIdx)) { - afhds3::getPowerStatus(moduleIdx, statusText); - } -#endif } -#if defined(AFHDS3) -uint8_t actualAfhdsRunPower(int moduleIndex) -{ - if (isModuleAFHDS3(moduleIndex)) { - return (uint8_t)afhds3::getActualRunPower(moduleIndex); - } - return 0; -} -#endif - ModuleSettingsMode getModuleMode(int moduleIndex) { return (ModuleSettingsMode)moduleState[moduleIndex].mode; } + void setModuleMode(int moduleIndex, ModuleSettingsMode mode) { moduleState[moduleIndex].mode = mode;