From 5679de641c674940cab2a06876c1f10453735122 Mon Sep 17 00:00:00 2001 From: Josh Gao Date: Mon, 23 Jun 2025 16:04:01 +0800 Subject: [PATCH] =?UTF-8?q?fix:=20=E4=BF=AE=E5=A4=8D=E6=95=B0=E6=8D=AE?= =?UTF-8?q?=E8=AF=BB=E5=8F=96=E9=80=BB=E8=BE=91,=E5=A2=9E=E5=8A=A0?= =?UTF-8?q?=E5=85=A5=E9=98=9F=E6=8B=B7=E8=B4=9D=E6=8B=B7=E8=B4=9D?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/main.c | 2 +- src/networking/link_layer/service.c | 79 +++++++++++++++++++------- src/networking/phy_layer/phy_antenna.c | 2 +- src/system_monitor.c | 21 ++++--- src/system_monitor.h | 2 +- 5 files changed, 73 insertions(+), 33 deletions(-) diff --git a/src/main.c b/src/main.c index d5ef9d6..ce3513f 100644 --- a/src/main.c +++ b/src/main.c @@ -220,7 +220,7 @@ int32_t SetupInterruptSystem(struct irq_ctrl_desc *descPtr) return Status; } - /* Initialize linklayer layer interrupt. */ + /* Initialize linklayer layer interrupt. */ Status = irq_trigger_level_set(descPtr, PERIODIC_5MS_INTR_ID, IRQ_EDGE_RISING); if (Status != SUCCESS) { diff --git a/src/networking/link_layer/service.c b/src/networking/link_layer/service.c index 1423062..9afae22 100644 --- a/src/networking/link_layer/service.c +++ b/src/networking/link_layer/service.c @@ -196,16 +196,29 @@ static int32_t Service_Packing128K(L2_INST *instPtr, L2_FRAME_TYPE type, uint8_t message->srcSpeedY = instPtr->sysInfo.vY; message->srcSpeedZ = instPtr->sysInfo.vZ; - // TODO: Packing extra payload + // Pack extra payload if (type == L2_FRAME_SVC) { - SYSMON_SVC_PAYLOAD sendPayload; + // Packing extra payload + SYSMON_SVC_PAYLOAD *sendPayload; SysMonitor_GetSendPayload(&sendPayload); - message->common.dst = sendPayload.nodeId; - message->dataId = sendPayload.dataId; - message->dataLen = sendPayload.dataLen; - memset(message->data, 0xAA, maxDataLen); - memcpy(message->data, sendPayload.data, sendPayload.dataLen); + if(sendPayload == NULL) + { + message->common.dst = L2_NONE_ID; + message->dataId = DATA_ID_NONE; + message->dataLen = 0; + memset(message->data, 0xAA, maxDataLen); + } + else + { + message->common.dst = sendPayload->nodeId; + message->dataId = sendPayload->dataId; + message->dataLen = sendPayload->dataLen; + memset(message->data, 0xAA, maxDataLen); + memcpy(message->data, sendPayload->data, (sendPayload->dataLen > maxDataLen) ? + maxDataLen : sendPayload->dataLen); + free(sendPayload); + } } else { @@ -264,14 +277,26 @@ static int32_t Service_Packing512K(L2_INST *instPtr, L2_FRAME_TYPE type, uint8_t message->srcSpeedY = instPtr->sysInfo.vY; message->srcSpeedZ = instPtr->sysInfo.vZ; - // TODO: Packing extra payload - SYSMON_SVC_PAYLOAD sendPayload; + // Pack extra payload + SYSMON_SVC_PAYLOAD *sendPayload; SysMonitor_GetSendPayload(&sendPayload); - message->common.dst = sendPayload.nodeId; - message->dataId = sendPayload.dataId; - message->dataLen = sendPayload.dataLen; - memset(message->data, 0xAA, maxDataLen); - memcpy(message->data, sendPayload.data, sendPayload.dataLen); + if(sendPayload == NULL) + { + message->common.dst = L2_NONE_ID; + message->dataId = DATA_ID_NONE; + message->dataLen = 0; + memset(message->data, 0xAA, maxDataLen); + } + else + { + message->common.dst = sendPayload->nodeId; + message->dataId = sendPayload->dataId; + message->dataLen = sendPayload->dataLen; + memset(message->data, 0xAA, maxDataLen); + memcpy(message->data, sendPayload->data, (sendPayload->dataLen > maxDataLen) ? + maxDataLen : sendPayload->dataLen); + free(sendPayload); + } // Append CRC uint32_t crcVal; @@ -322,14 +347,26 @@ static int32_t Service_Packing2M(L2_INST *instPtr, L2_FRAME_TYPE type, uint8_t d message->srcSpeedY = instPtr->sysInfo.vY; message->srcSpeedZ = instPtr->sysInfo.vZ; - // TODO: Packing extra payload - SYSMON_SVC_PAYLOAD sendPayload; + // Packing extra payload + SYSMON_SVC_PAYLOAD *sendPayload; SysMonitor_GetSendPayload(&sendPayload); - message->common.dst = sendPayload.nodeId; - message->dataId = sendPayload.dataId; - message->dataLen = sendPayload.dataLen; - memset(message->data, 0xAA, maxDataLen); - memcpy(message->data, sendPayload.data, sendPayload.dataLen); + if(sendPayload == NULL) + { + message->common.dst = L2_NONE_ID; + message->dataId = DATA_ID_NONE; + message->dataLen = 0; + memset(message->data, 0xAA, maxDataLen); + } + else + { + message->common.dst = sendPayload->nodeId; + message->dataId = sendPayload->dataId; + message->dataLen = sendPayload->dataLen; + memset(message->data, 0xAA, maxDataLen); + memcpy(message->data, sendPayload->data, (sendPayload->dataLen > maxDataLen) ? + maxDataLen : sendPayload->dataLen); + free(sendPayload); + } // Append CRC uint32_t crcVal; diff --git a/src/networking/phy_layer/phy_antenna.c b/src/networking/phy_layer/phy_antenna.c index f141c8c..6fcf30c 100644 --- a/src/networking/phy_layer/phy_antenna.c +++ b/src/networking/phy_layer/phy_antenna.c @@ -462,7 +462,7 @@ int32_t Phy_SetAntIndex(PHY_INST *instPtr, PHY_ANT_INDEX antIndex) { ASSERT(instPtr != NULL); - if (antIndex < 0 || antIndex >= L1_ANT_NUM) + if (antIndex < PHY_ANT_INDEX_NONE || antIndex > PHY_ANT_INDEX_3) { DBG_ERROR("Antenna index out of range: %" PRIu8 ". \n", antIndex); return EPARAM; diff --git a/src/system_monitor.c b/src/system_monitor.c index 3adb2d6..75e4ff5 100644 --- a/src/system_monitor.c +++ b/src/system_monitor.c @@ -195,9 +195,17 @@ int32_t SysMonitor_UpdateSendPayload(SYSMON_SVC_PAYLOAD *payload) } // push message into send queue - if (CircQueuePush(sysMonitorInst->sendQueue, (void *)payload) == false) + SYSMON_SVC_PAYLOAD *payloadCopy = malloc(sizeof(SYSMON_SVC_PAYLOAD)); + if (payloadCopy == NULL) + { + DBG_ERROR("Allocate memory for send payload failed.\n"); + return ENOMEM; + } + memcpy(payloadCopy, payload, sizeof(SYSMON_SVC_PAYLOAD)); + if (CircQueuePush(sysMonitorInst->sendQueue, (void *)payloadCopy) == false) { DBG_WARNING("SystemMonitor::SEND Queue overflowed!\n"); + free(payloadCopy); return EQUEUE; } @@ -209,19 +217,14 @@ int32_t SysMonitor_UpdateSendPayload(SYSMON_SVC_PAYLOAD *payload) * * @param payload */ -void SysMonitor_GetSendPayload(SYSMON_SVC_PAYLOAD *payload) +void SysMonitor_GetSendPayload(SYSMON_SVC_PAYLOAD **payload) { ASSERT(sysMonitorInst != NULL); ASSERT(payload != NULL); - if (CircQueuePop(sysMonitorInst->sendQueue, (void *)payload) == false) + if (CircQueuePop(sysMonitorInst->sendQueue, (void **)payload) == false) { - payload->dataId = DATA_ID_NONE; - payload->nodeId = 0; - payload->dataLen = 0; - // set payload to be 0xAA to balanced average signal power - memset(payload->data, 0xAA, PAYLOAD_LEN_MAX); - DBG_WARNING("SystemMonitor::Nothing to be sent.\n"); + *payload = NULL; } } diff --git a/src/system_monitor.h b/src/system_monitor.h index 9b04b34..5a94d07 100644 --- a/src/system_monitor.h +++ b/src/system_monitor.h @@ -243,7 +243,7 @@ void SysMonitor_UpdateSysinfo(const SYSINFO *info); void SysMonitor_GetSysinfo(SYSINFO *info); int32_t SysMonitor_UpdateSendPayload(SYSMON_SVC_PAYLOAD *payload); -void SysMonitor_GetSendPayload(SYSMON_SVC_PAYLOAD *payload); +void SysMonitor_GetSendPayload(SYSMON_SVC_PAYLOAD **payload); int32_t SysMonitor_GenRecvRsp(SYSMON_SVC_PAYLOAD *payload); void SysMonitor_UpdateLinklayerLog(const LINKLOG *log);