From 4547f08cca75588576896c65cda15b33fb37b2c3 Mon Sep 17 00:00:00 2001 From: richardclli Date: Mon, 27 Feb 2023 19:51:45 +0800 Subject: [PATCH] feat: Add support for FRM303 (#2875) * Add FRM303 support. * chore: Formatting --- radio/src/pulses/afhds3.cpp | 101 +++++++++++++------------- radio/src/pulses/afhds3_transport.cpp | 36 ++++----- radio/src/pulses/afhds3_transport.h | 56 ++++++-------- 3 files changed, 89 insertions(+), 104 deletions(-) diff --git a/radio/src/pulses/afhds3.cpp b/radio/src/pulses/afhds3.cpp index 67da700e0..7016c0ae9 100644 --- a/radio/src/pulses/afhds3.cpp +++ b/radio/src/pulses/afhds3.cpp @@ -73,17 +73,15 @@ static uint8_t _phyMode_channels[] = { // EMPTY_DT, // }; -//enum used by command response -> translate to ModuleState -enum MODULE_READY_E -{ +// enum used by command response -> translate to ModuleState +enum MODULE_READY_E { MODULE_STATUS_UNKNOWN = 0x00, MODULE_STATUS_NOT_READY = 0x01, MODULE_STATUS_READY = 0x02 }; -enum ModuleState -{ - STATE_NOT_READY = 0x00, //virtual +enum ModuleState { + STATE_NOT_READY = 0x00, // virtual STATE_HW_ERROR = 0x01, STATE_BINDING = 0x02, STATE_SYNC_RUNNING = 0x03, @@ -94,7 +92,7 @@ enum ModuleState STATE_UPDATING_RX = 0x08, STATE_UPDATING_RX_FAILED = 0x09, STATE_RF_TESTING = 0x0a, - STATE_READY = 0x0b, //virtual + STATE_READY = 0x0b, // virtual STATE_HW_TEST = 0xff, }; @@ -108,14 +106,12 @@ enum MODULE_MODE_E { MODULE_MODE_UNKNOWN = 0xFF }; -enum CMD_RESULT -{ +enum CMD_RESULT { FAILURE = 0x01, SUCCESS = 0x02, }; -enum CHANNELS_DATA_MODE -{ +enum CHANNELS_DATA_MODE { CHANNELS = 0x01, FAIL_SAFE = 0x02, }; @@ -126,14 +122,12 @@ PACK(struct ChannelsData { int16_t data[AFHDS3_MAX_CHANNELS]; }); -union ChannelsData_u -{ +union ChannelsData_u { ChannelsData data; uint8_t buffer[sizeof(ChannelsData)]; }; -PACK(struct TelemetryData -{ +PACK(struct TelemetryData { uint8_t sensorType; uint8_t length; uint8_t type; @@ -141,14 +135,18 @@ PACK(struct TelemetryData uint8_t data[8]; }); -enum MODULE_POWER_SOURCE -{ +enum MODULE_POWER_SOURCE { INTERNAL = 0x01, EXTERNAL = 0x02, }; -PACK(struct ModuleVersion -{ +enum DeviceAddress { + TRANSMITTER = 0x01, + FRM303 = 0x04, + IRM301 = 0x05, +}; + +PACK(struct ModuleVersion { uint32_t productNumber; uint32_t hardwereVersion; uint32_t bootloaderVersion; @@ -156,15 +154,13 @@ PACK(struct ModuleVersion uint32_t rfVersion; }); -PACK(struct CommandResult_s -{ +PACK(struct CommandResult_s { uint16_t command; uint8_t result; uint8_t respLen; }); -union AfhdsFrameData -{ +union AfhdsFrameData { uint8_t value; // Config_s Config; ChannelsData Channels; @@ -183,32 +179,33 @@ 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 - */ - void setupFrame(); + /** + * Fills DMA buffers with frame to be send depending on actual state + */ + void setupFrame(); - /** - * Sends prepared buffers - */ - void sendFrame() { trsp.sendBuffer(); } + /** + * Sends prepared buffers + */ + void sendFrame() { trsp.sendBuffer(); } - /** + /** * Gets actual module status into provided buffer * @param statusText target buffer for status */ - void getStatusString(char * statusText) const; + void getStatusString(char* statusText) const; - /** + /** * Sends stop command to prevent any further module operations */ - void stop(); + void stop(); - Config_u* getConfig() { return &cfg; } + Config_u* getConfig() { return &cfg; } - void applyConfigFromModel(); + void applyConfigFromModel(); protected: @@ -468,10 +465,11 @@ 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 +851,10 @@ 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 +873,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); @@ -885,7 +887,8 @@ static void deinitModule(void* ctx) modulePortDeInit(mod_st); } -static void sendPulses(void* ctx, uint8_t* buffer, int16_t* channels, uint8_t nChannels) +static void sendPulses(void* ctx, uint8_t* buffer, int16_t* channels, + uint8_t nChannels) { (void)buffer; (void)channels; @@ -898,13 +901,11 @@ static void sendPulses(void* ctx, uint8_t* buffer, int16_t* channels, uint8_t nC } etx_proto_driver_t ProtoDriver = { - .protocol = PROTOCOL_CHANNELS_AFHDS3, - .init = initModule, - .deinit = deinitModule, - .sendPulses = sendPulses, - .processData = processTelemetryData, + .protocol = PROTOCOL_CHANNELS_AFHDS3, + .init = initModule, + .deinit = deinitModule, + .sendPulses = sendPulses, + .processData = processTelemetryData, }; -} - - +} // namespace afhds3 diff --git a/radio/src/pulses/afhds3_transport.cpp b/radio/src/pulses/afhds3_transport.cpp index d23eb0b89..2442dfa65 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(); } @@ -100,24 +91,25 @@ void FrameTransport::putBytes(uint8_t* data, int length) } } -void FrameTransport::putFrame(COMMAND command, FRAME_TYPE frameType, uint8_t* data, - uint8_t dataLength, uint8_t frameIndex) +void FrameTransport::putFrame(COMMAND command, FRAME_TYPE frameType, + uint8_t* data, uint8_t dataLength, + uint8_t frameIndex) { - //header + // header data_ptr = trsp_buffer; crc = 0; sendByte(START); - uint8_t buffer[] = {FrameAddress, frameIndex, frameType, command}; + uint8_t buffer[] = {frameAddress, frameIndex, frameType, command}; putBytes(buffer, 4); - //payload + // payload if (dataLength > 0) { putBytes(data, dataLength); } - //footer + // footer uint8_t crcValue = crc ^ 0xff; putBytes(&crcValue, 1); sendByte(END); @@ -223,9 +215,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; } @@ -362,7 +354,8 @@ bool Transport::handleReply(uint8_t* buffer, uint8_t len) bool Transport::processTelemetryData(uint8_t byte, uint8_t* rxBuffer, uint8_t& rxBufferCount, uint8_t maxSize) { - bool has_frame = trsp.processTelemetryData(byte, rxBuffer, rxBufferCount, maxSize); + bool has_frame = + trsp.processTelemetryData(byte, rxBuffer, rxBufferCount, maxSize); if (has_frame && handleReply(rxBuffer, rxBufferCount)) { rxBufferCount = 0; return false; @@ -370,5 +363,4 @@ bool Transport::processTelemetryData(uint8_t byte, uint8_t* rxBuffer, return has_frame; } - -}; +}; // namespace afhds3 diff --git a/radio/src/pulses/afhds3_transport.h b/radio/src/pulses/afhds3_transport.h index e7a649016..aa918efd7 100644 --- a/radio/src/pulses/afhds3_transport.h +++ b/radio/src/pulses/afhds3_transport.h @@ -23,25 +23,22 @@ #include "afhds3_module.h" #include "definitions.h" - #include "hal/module_port.h" namespace afhds3 { - -enum FRAME_TYPE: uint8_t -{ - REQUEST_GET_DATA = 0x01, //Get data response: ACK + DATA - REQUEST_SET_EXPECT_DATA = 0x02, //Set data response: ACK + DATA - REQUEST_SET_EXPECT_ACK = 0x03, //Set data response: ACK - REQUEST_SET_NO_RESP = 0x05, //Set data response: none - RESPONSE_DATA = 0x10, //Response ACK + DATA - RESPONSE_ACK = 0x20, //Response ACK + +enum FRAME_TYPE : uint8_t { + REQUEST_GET_DATA = 0x01, // Get data response: ACK + DATA + REQUEST_SET_EXPECT_DATA = 0x02, // Set data response: ACK + DATA + REQUEST_SET_EXPECT_ACK = 0x03, // Set data response: ACK + REQUEST_SET_NO_RESP = 0x05, // Set data response: none + RESPONSE_DATA = 0x10, // Response ACK + DATA + RESPONSE_ACK = 0x20, // Response ACK NOT_USED = 0xff }; -enum COMMAND: uint8_t -{ +enum COMMAND : uint8_t { MODULE_READY = 0x01, MODULE_STATE = 0x02, MODULE_MODE = 0x03, @@ -53,13 +50,12 @@ enum COMMAND: uint8_t COMMAND_RESULT = 0x0D, MODULE_VERSION = 0x20, MODEL_ID = 0x2F, - VIRTUAL_FAILSAFE = 0x99, // virtual command used to trigger failsafe + VIRTUAL_FAILSAFE = 0x99, // virtual command used to trigger failsafe UNDEFINED = 0xFF }; // one byte frames for request queue -struct Frame -{ +struct Frame { enum COMMAND command; enum FRAME_TYPE frameType; uint8_t payload; @@ -69,7 +65,7 @@ struct Frame }; union AfhdsFrameData; - + PACK(struct AfhdsFrame { uint8_t startByte; uint8_t address; @@ -117,16 +113,16 @@ struct CommandFifo { uint8_t byteContent); }; -struct FrameTransport -{ +struct FrameTransport { uint8_t* trsp_buffer; 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); @@ -136,27 +132,23 @@ struct FrameTransport uint8_t dataLength, uint8_t frameIndex); uint32_t getFrameSize(); - + bool processTelemetryData(uint8_t byte, uint8_t* rxBuffer, uint8_t& rxBufferCount, uint8_t maxSize); }; class Transport { - enum State { - UNKNOWN = 0, - SENDING_COMMAND, - AWAITING_RESPONSE, - IDLE - }; + enum State { UNKNOWN = 0, SENDING_COMMAND, AWAITING_RESPONSE, IDLE }; etx_module_state_t* mod_st; FrameTransport trsp; - CommandFifo fifo; + CommandFifo fifo; /** - * Internal operation state one of UNKNOWN, SENDING_COMMAND, AWAITING_RESPONSE, IDLE + * Internal operation state one of UNKNOWN, SENDING_COMMAND, + * AWAITING_RESPONSE, IDLE * Used to avoid sending commands when not allowed to */ State operationState; @@ -173,12 +165,12 @@ class Transport uint16_t repeatCount; 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(); - + void sendFrame(COMMAND command, FRAME_TYPE frameType, uint8_t* data = nullptr, uint8_t dataLength = 0); @@ -203,4 +195,4 @@ class Transport bool processTelemetryData(uint8_t byte, uint8_t* rxBuffer, uint8_t& rxBufferCount, uint8_t maxSize); }; -}; +}; // namespace afhds3