diff --git a/radio/src/pulses/afhds3.cpp b/radio/src/pulses/afhds3.cpp index 67da700e07a..579e41a546d 100644 --- a/radio/src/pulses/afhds3.cpp +++ b/radio/src/pulses/afhds3.cpp @@ -147,6 +147,13 @@ enum MODULE_POWER_SOURCE EXTERNAL = 0x02, }; +enum DeviceAddress +{ + TRANSMITTER = 0x01, + FRM303 = 0x04, + IRM301 = 0x05, +}; + PACK(struct ModuleVersion { uint32_t productNumber; @@ -183,7 +190,7 @@ class ProtoState * @param moduleIndex index of module one of INTERNAL_MODULE, EXTERNAL_MODULE * @param resetFrameCount flag if current frame count should be reseted */ - void init(uint8_t moduleIndex, void* buffer, etx_module_state_t* mod_st); + void init(uint8_t moduleIndex, void* buffer, etx_module_state_t* mod_st, uint8_t fAddr); /** * Fills DMA buffers with frame to be send depending on actual state @@ -468,10 +475,10 @@ void ProtoState::setupFrame() } } -void ProtoState::init(uint8_t moduleIndex, void* buffer, etx_module_state_t* mod_st) +void ProtoState::init(uint8_t moduleIndex, void* buffer, etx_module_state_t* mod_st, uint8_t fAddr) { module_index = moduleIndex; - trsp.init(buffer, mod_st); + trsp.init(buffer, mod_st, fAddr); //clear local vars because it is member of union moduleData = &g_model.moduleData[module_index]; @@ -853,6 +860,7 @@ static void* initModule(uint8_t module) etx_module_state_t* mod_st = nullptr; etx_serial_init params(_uartParams); uint16_t period = AFHDS3_UART_COMMAND_TIMEOUT * 1000; + uint8_t fAddr = (module == INTERNAL_MODULE ? DeviceAddress::IRM301 : DeviceAddress::FRM303) << 4 | DeviceAddress::TRANSMITTER; params.baudrate = AFHDS3_UART_BAUDRATE; params.polarity = @@ -871,7 +879,7 @@ static void* initModule(uint8_t module) if (!mod_st) return nullptr; auto p_state = &protoState[module]; - p_state->init(module, pulsesGetModuleBuffer(module), mod_st); + p_state->init(module, pulsesGetModuleBuffer(module), mod_st, fAddr); mod_st->user_data = (void*)p_state; mixerSchedulerSetPeriod(module, period); diff --git a/radio/src/pulses/afhds3_transport.cpp b/radio/src/pulses/afhds3_transport.cpp index d23eb0b8912..cc4541a26ed 100644 --- a/radio/src/pulses/afhds3_transport.cpp +++ b/radio/src/pulses/afhds3_transport.cpp @@ -51,18 +51,9 @@ enum AfhdsSpecialChars { // ESC_ESC must be used }; -enum DeviceAddress -{ - TRANSMITTER = 0x01, - // MODULE = 0x03, - MODULE = 0x05, -}; - -//Address used in transmitted frames - it constrains of target address and source address -const uint8_t FrameAddress = DeviceAddress::TRANSMITTER | (DeviceAddress::MODULE << 4); - -void FrameTransport::init(void* buffer) +void FrameTransport::init(void* buffer, uint8_t fAddr) { + frameAddress = fAddr; trsp_buffer = (uint8_t*)buffer; clear(); } @@ -109,7 +100,7 @@ void FrameTransport::putFrame(COMMAND command, FRAME_TYPE frameType, uint8_t* da crc = 0; sendByte(START); - uint8_t buffer[] = {FrameAddress, frameIndex, frameType, command}; + uint8_t buffer[] = {frameAddress, frameIndex, frameType, command}; putBytes(buffer, 4); //payload @@ -223,9 +214,9 @@ void CommandFifo::enqueue(COMMAND command, FRAME_TYPE frameType, bool useData, } } -void Transport::init(void* buffer, etx_module_state_t* mod_st) +void Transport::init(void* buffer, etx_module_state_t* mod_st, uint8_t fAddr) { - trsp.init(buffer); + trsp.init(buffer, fAddr); this->mod_st = mod_st; } diff --git a/radio/src/pulses/afhds3_transport.h b/radio/src/pulses/afhds3_transport.h index e7a649016c2..5383fa0121b 100644 --- a/radio/src/pulses/afhds3_transport.h +++ b/radio/src/pulses/afhds3_transport.h @@ -123,10 +123,11 @@ struct FrameTransport uint8_t* data_ptr; uint8_t crc; + uint8_t frameAddress; // uint8_t timeout; uint8_t esc_state; - void init(void* buffer); + void init(void* buffer, uint8_t fAddr); void clear(); void sendByte(uint8_t b); @@ -175,7 +176,7 @@ class Transport bool handleReply(uint8_t* buffer, uint8_t len); public: - void init(void* buffer, etx_module_state_t* mod_st); + void init(void* buffer, etx_module_state_t* mod_st, uint8_t fAddr); void clear();