0xha0/datalink_repo
2
2
Fork 1
Code Issues Pull Requests Packages Projects Releases Wiki Activity

miscellaneous update.

Browse Source
This commit is contained in:
Gatsby Van
2023-12-21 14:48:28 +08:00
parent 97b46fb0e8
commit ebe3ebf71d
6 changed files with 519 additions and 519 deletions
Download Patch File Download Diff File Expand all files Collapse all files
df_datalink/src/networking/link_layer/framing.c
+8 -8
View File
@@ -30,9 +30,9 @@
* @brief Generate crc of specified data. * @brief Generate crc of specified data.
* *
* @param dataBuff pointer to data buffer. * @param dataBuff pointer to data buffer.
* dataLen length of specified data. * @param dataLen length of specified data.
* crcType type of crc * @param crcType type of crc
* crc a pointer to 32 bit crc container. * @param crc a pointer to 32 bit crc container.
* *
* @return FAILURE crc type not support. * @return FAILURE crc type not support.
* SUCCESS * SUCCESS
@@ -63,9 +63,9 @@ static int32_t L2_CrcGen(uint8_t* dataBuff, uint32_t dataLen, L2_CRC_TYPE crcTyp
* @brief Append a crc of specified data, to a specified position. * @brief Append a crc of specified data, to a specified position.
* *
* @param dataBuff pointer to data buffer. * @param dataBuff pointer to data buffer.
* dataLen length of specified data. * @param dataLen length of specified data.
* crcPos position of crc insert position * @param crcPos position of crc insert position
* crcType type of crc * @param crcType type of crc
* *
* @return FAILURE crc type not support. * @return FAILURE crc type not support.
* SUCCESS * SUCCESS
@@ -105,8 +105,8 @@ int32_t L2_CrcAppend(uint8_t* dataBuff, uint32_t dataLen, uint32_t crcPos, L2_CR
* @brief Check data validation by crc. * @brief Check data validation by crc.
* *
* @param dataBuff pointer to data buffer. * @param dataBuff pointer to data buffer.
* dataLen length of specified data. * @param dataLen length of specified data.
* crcType type of crc * @param crcType type of crc
* *
* @return FAILURE crc type not support. * @return FAILURE crc type not support.
* SUCCESS * SUCCESS
df_datalink/src/networking/link_layer/link_layer.c
+300 -300
View File
File diff suppressed because it is too large Load Diff
df_datalink/src/networking/link_layer/link_layer.h
+33 -33
View File
@@ -86,57 +86,57 @@ typedef struct {
extern L2_RTNE_TABLE_ENTRY* normalTable[L2_RTNE_PERIOD]; extern L2_RTNE_TABLE_ENTRY* normalTable[L2_RTNE_PERIOD];
/************************** Function Prototypes ******************************/ /************************** Function Prototypes ******************************/
// Main // Main
int32_t Linklayer_Init(L2_INST **l2InstPtr, L2_CONFIG *config); int32_t Linklayer_Init(L2_INST **instPtr, L2_CONFIG *config);
int32_t Linklayer_Proc(L2_INST *l2InstPtr); int32_t Linklayer_Proc(L2_INST *instPtr);
int32_t Linklayer_EventHandler(L2_INST *l2InstPtr, L2_EVENT* l2Event); int32_t Linklayer_EventHandler(L2_INST *instPtr, L2_EVENT* l2Event);
int32_t Linklayer_PktHandler(L2_INST *l2InstPtr); int32_t Linklayer_PktHandler(L2_INST *instPtr);
int32_t L2_SteerBeam2Node(L2_INST *l2InstPtr, uint8_t nodeId); int32_t L2_SteerBeam2Node(L2_INST *instPtr, uint8_t nodeId);
int32_t L2_SetupNextSlot(L2_INST *l2InstPtr, L2_TOR_TYPE txrxMode, L2_FRAME_TYPE frameType); int32_t L2_SetupNextSlot(L2_INST *instPtr, L2_TOR_TYPE txrxMode, L2_FRAME_TYPE frameType);
// Event // Event
L2_SCHD_EVENT* L2_CreatNewSchdEvent(L2_INST *l2InstPtr, L2_EVENT_TYPE event); L2_SCHD_EVENT* L2_CreatNewSchdEvent(L2_INST *instPtr, L2_EVENT_TYPE event);
bool L2_isEventScheduled(L2_SCHD_EVENT* schdEvent); bool L2_isEventScheduled(L2_SCHD_EVENT* schdEvent);
int32_t L2_CancelEvent(L2_SCHD_EVENT* schdEvent); int32_t L2_CancelEvent(L2_SCHD_EVENT* schdEvent);
int32_t L2_SchdEventAppendMsg(L2_SCHD_EVENT* schdEvent, void* msg); int32_t L2_SchdEventAppendMsg(L2_SCHD_EVENT* schdEvent, void* msg);
int32_t L2_ScheduleEventAt(L2_INST *l2InstPtr, L2_SCHD_EVENT* schdEvent, uint32_t frameNo, uint32_t slotNo); int32_t L2_ScheduleEventAt(L2_INST *instPtr, L2_SCHD_EVENT* schdEvent, uint32_t frameNo, uint32_t slotNo);
int32_t L2_ScheduleEventAfter(L2_INST *l2InstPtr, L2_SCHD_EVENT* schdEvent, uint32_t numSlots); int32_t L2_ScheduleEventAfter(L2_INST *instPtr, L2_SCHD_EVENT* schdEvent, uint32_t numSlots);
int32_t L2_RecheduleEventAt(L2_INST *l2InstPtr, L2_SCHD_EVENT* schdEvent, uint32_t frameNo, uint32_t slotNo); int32_t L2_RecheduleEventAt(L2_INST *instPtr, L2_SCHD_EVENT* schdEvent, uint32_t frameNo, uint32_t slotNo);
int32_t L2_RecheduleEventAfter(L2_INST *l2InstPtr, L2_SCHD_EVENT* schdEvent, uint32_t numSlots); int32_t L2_RecheduleEventAfter(L2_INST *instPtr, L2_SCHD_EVENT* schdEvent, uint32_t numSlots);
// Framing // Framing
void L2_FrameStateUpdate(L2_INST *l2InstPtr); void L2_FrameStateUpdate(L2_INST *instPtr);
L2_FRAME_TYPE L2_GetLastRcvFrameType(L2_INST *l2InstPtr); L2_FRAME_TYPE L2_GetLastRcvFrameType(L2_INST *instPtr);
L1_FRAME_TYPE L2_L2frame2L1frame(L2_FRAME_TYPE l2Frame); L1_FRAME_TYPE L2_L2frame2L1frame(L2_FRAME_TYPE l2Frame);
int32_t L2_CrcAppend(uint8_t* dataBuff, uint32_t dataLen, uint32_t crcPos, L2_CRC_TYPE crcType); int32_t L2_CrcAppend(uint8_t* dataBuff, uint32_t dataLen, uint32_t crcPos, L2_CRC_TYPE crcType);
bool L2_CrcCheck(uint8_t* dataBuff, uint32_t dataLen, L2_CRC_TYPE crcType); bool L2_CrcCheck(uint8_t* dataBuff, uint32_t dataLen, L2_CRC_TYPE crcType);
// Miscellaneous // Miscellaneous
L2_NODE_STATE L2_GetMainState(L2_INST *l2InstPtr); L2_NODE_STATE L2_GetMainState(L2_INST *instPtr);
bool L2_isDataAvail(L2_INST *l2InstPtr); bool L2_isDataAvail(L2_INST *instPtr);
bool L2_isCxn2Net(L2_INST *l2InstPtr); bool L2_isCxn2Net(L2_INST *instPtr);
int32_t L2_GetFrameLen(L2_FRAME_TYPE l2FrameType); int32_t L2_GetFrameLen(L2_FRAME_TYPE l2FrameType);
uint32_t L2_GetLastFrameNo(L2_INST *l2InstPtr); uint32_t L2_GetLastFrameNo(L2_INST *instPtr);
uint32_t L2_GetLastSlotNo(L2_INST *l2InstPtr); uint32_t L2_GetLastSlotNo(L2_INST *instPtr);
uint32_t L2_GetCurrentFrameNo(L2_INST *l2InstPtr); uint32_t L2_GetCurrentFrameNo(L2_INST *instPtr);
uint32_t L2_GetCurrentSlotNo(L2_INST *l2InstPtr); uint32_t L2_GetCurrentSlotNo(L2_INST *instPtr);
uint32_t L2_GetNextFrameNo(L2_INST *l2InstPtr); uint32_t L2_GetNextFrameNo(L2_INST *instPtr);
uint32_t L2_GetNextSlotNo(L2_INST *l2InstPtr); uint32_t L2_GetNextSlotNo(L2_INST *instPtr);
// Scanning // Scanning
int32_t L2_Scan_Init(L2_INST *l2InstPtr); int32_t L2_Scan_Init(L2_INST *instPtr);
int32_t L2_Scan_PktHandler(L2_INST *l2InstPtr, uint8_t *msg); int32_t L2_Scan_PktHandler(L2_INST *instPtr, uint8_t *msg);
int32_t L2_Scan_Proc(L2_INST *l2InstPtr); int32_t L2_Scan_Proc(L2_INST *instPtr);
int32_t L2_Scan_Routine(L2_INST *l2InstPtr, L2_RTNE_TABLE_ENTRY *schedEntry); int32_t L2_Scan_Routine(L2_INST *instPtr, L2_RTNE_TABLE_ENTRY *schedEntry);
// Service // Service
int32_t L2_Service_Init(L2_INST *l2InstPtr); int32_t L2_Service_Init(L2_INST *instPtr);
int32_t L2_Service_PktHandler(L2_INST *l2InstPtr, uint8_t *msg); int32_t L2_Service_PktHandler(L2_INST *instPtr, uint8_t *msg);
int32_t L2_Service_Proc(L2_INST *l2InstPtr); int32_t L2_Service_Proc(L2_INST *instPtr);
int32_t L2_Service_Routine(L2_INST *l2InstPtr, L2_RTNE_TABLE_ENTRY *schedEntry); int32_t L2_Service_Routine(L2_INST *instPtr, L2_RTNE_TABLE_ENTRY *schedEntry);
// Time Sync // Time Sync
int32_t L2_TimeSync_LocalTimerAdd(L2_INST *l2InstPtr, int64_t adjustment); int32_t L2_TimeSync_LocalTimerAdd(L2_INST *instPtr, int64_t adjustment);
int32_t L2_TimeSync_SlotAlign(L2_INST *l2InstPtr); int32_t L2_TimeSync_SlotAlign(L2_INST *instPtr);
int32_t L2_TimeSync_GetDeviatoion(L2_INST *l2InstPtr, int64_t *devi); int32_t L2_TimeSync_GetDeviatoion(L2_INST *instPtr, int64_t *devi);
#endif /* __LINK_LAYER_H_ */ #endif /* __LINK_LAYER_H_ */
df_datalink/src/networking/link_layer/scan.c
+63 -63
View File
@@ -33,7 +33,7 @@
/************************** Variable Definitions *****************************/ /************************** Variable Definitions *****************************/
/************************** Function Prototypes ******************************/ /************************** Function Prototypes ******************************/
static int32_t ScanMsg_Packing(L2_INST *l2InstPtr, L2_FRAME_TYPE type); static int32_t ScanMsg_Packing(L2_INST *instPtr, L2_FRAME_TYPE type);
/*****************************************************************************/ /*****************************************************************************/
/** /**
* @brief Packing Scan message to a bit array. * @brief Packing Scan message to a bit array.
@@ -45,35 +45,35 @@ static int32_t ScanMsg_Packing(L2_INST *l2InstPtr, L2_FRAME_TYPE type);
* SUCCESS * SUCCESS
* *
******************************************************************************/ ******************************************************************************/
static int32_t ScanMsg_Packing(L2_INST *l2InstPtr, L2_FRAME_TYPE type) { static int32_t ScanMsg_Packing(L2_INST *instPtr, L2_FRAME_TYPE type) {
ASSERT(l2InstPtr != NULL); ASSERT(instPtr != NULL);
int32_t Status; int32_t Status;
uint8_t *buff; uint8_t *buff;
uint32_t packLen; uint32_t packLen;
switch (type) { switch (type) {
case L2_INFO_FRAME:{ case L2_INFO_FRAME:{
packLen = RSP_PACK_LEN; packLen = RSP_PACK_LEN;
SysMonitor_GetSysinfo(&(l2InstPtr->sysInfo)); SysMonitor_GetSysinfo(&(instPtr->sysInfo));
buff = malloc(packLen); buff = malloc(packLen);
if (buff == NULL) { if (buff == NULL) {
DBG_ERROR("Not enough space, failed to pack frame...\n"); DBG_ERROR("Not enough space, failed to pack frame...\n");
return ENOMEM; return ENOMEM;
} }
buff[L2_MSG_FRAME_NO] = L2_GetNextFrameNo(l2InstPtr); buff[L2_MSG_FRAME_NO] = L2_GetNextFrameNo(instPtr);
buff[L2_MSG_SLOT_NO] = L2_GetNextSlotNo(l2InstPtr); buff[L2_MSG_SLOT_NO] = L2_GetNextSlotNo(instPtr);
memcpy(buff+L2_MSG_INFO_POSX, &(l2InstPtr->sysInfo.posX), sizeof(float)); memcpy(buff+L2_MSG_INFO_POSX, &(instPtr->sysInfo.posX), sizeof(float));
memcpy(buff+L2_MSG_INFO_POSY, &(l2InstPtr->sysInfo.posY), sizeof(float)); memcpy(buff+L2_MSG_INFO_POSY, &(instPtr->sysInfo.posY), sizeof(float));
memcpy(buff+L2_MSG_INFO_POSZ, &(l2InstPtr->sysInfo.posZ), sizeof(float)); memcpy(buff+L2_MSG_INFO_POSZ, &(instPtr->sysInfo.posZ), sizeof(float));
memcpy(buff+L2_MSG_INFO_SPEEDX, &(l2InstPtr->sysInfo.vX), sizeof(float)); memcpy(buff+L2_MSG_INFO_SPEEDX, &(instPtr->sysInfo.vX), sizeof(float));
memcpy(buff+L2_MSG_INFO_SPEEDY, &(l2InstPtr->sysInfo.vY), sizeof(float)); memcpy(buff+L2_MSG_INFO_SPEEDY, &(instPtr->sysInfo.vY), sizeof(float));
memcpy(buff+L2_MSG_INFO_SPEEDZ, &(l2InstPtr->sysInfo.vZ), sizeof(float)); memcpy(buff+L2_MSG_INFO_SPEEDZ, &(instPtr->sysInfo.vZ), sizeof(float));
Status = L2_CrcAppend(buff, packLen - 2, L2_MSG_CRC, L2_CRC16_1021); Status = L2_CrcAppend(buff, packLen - 2, L2_MSG_CRC, L2_CRC16_1021);
if (Status != SUCCESS) { if (Status != SUCCESS) {
DBG_ERROR("Fail to append CRC, error code: %ld.\n", Status); DBG_ERROR("Fail to append CRC, error code: %ld.\n", Status);
return Status; return Status;
} }
if(CircQueuePush(l2InstPtr->txQueue, buff) == false){ if(CircQueuePush(instPtr->txQueue, buff) == false){
DBG_ERROR("Cannot push packet to queue..\n"); DBG_ERROR("Cannot push packet to queue..\n");
return EQUEUE; return EQUEUE;
} }
@@ -92,15 +92,15 @@ static int32_t ScanMsg_Packing(L2_INST *l2InstPtr, L2_FRAME_TYPE type) {
/** /**
* @brief Initialize scan FSM. * @brief Initialize scan FSM.
* *
* @param l2InstPtr * @param instPtr
* *
* @return * @return
* *
******************************************************************************/ ******************************************************************************/
int32_t L2_Scan_Init(L2_INST *l2InstPtr) { int32_t L2_Scan_Init(L2_INST *instPtr) {
ASSERT(l2InstPtr != NULL); ASSERT(instPtr != NULL);
l2InstPtr->scanFsm.sacanCnt = 0; instPtr->scanFsm.sacanCnt = 0;
l2InstPtr->scanFsm.beamNo = 0; instPtr->scanFsm.beamNo = 0;
return SUCCESS; return SUCCESS;
} }
@@ -113,32 +113,32 @@ int32_t L2_Scan_Init(L2_INST *l2InstPtr) {
* @return * @return
* *
******************************************************************************/ ******************************************************************************/
int32_t L2_Scan_PktHandler(L2_INST *l2InstPtr, uint8_t *msg) { int32_t L2_Scan_PktHandler(L2_INST *instPtr, uint8_t *msg) {
ASSERT(l2InstPtr != NULL); ASSERT(instPtr != NULL);
ASSERT(msg != NULL); ASSERT(msg != NULL);
int32_t Status; int32_t Status;
switch (l2InstPtr->frame.last) { switch (instPtr->frame.last) {
case L2_SCAN_FRAME: { case L2_SCAN_FRAME: {
if (l2InstPtr->config.myType == L2_HEAD_NODE) { if (instPtr->config.myType == L2_HEAD_NODE) {
DBG_ERROR("Unexpected call from head node, Only None-Head Node can handle L2_SVC_FRAME.\n"); DBG_ERROR("Unexpected call from head node, Only None-Head Node can handle L2_SVC_FRAME.\n");
Status = EUNEXP; Status = EUNEXP;
break; break;
} }
Status = AppendNode(l2InstPtr->nodeInfoList, L2_HEAD_NODE_ID, L2_HEAD_NODE); Status = AppendNode(instPtr->nodeInfoList, L2_HEAD_NODE_ID, L2_HEAD_NODE);
if (Status != SUCCESS) { if (Status != SUCCESS) {
DBG_ERROR("Failed to append node %d, errorCode: %ld.\n", L2_MEMBER_NODE_ID, Status); DBG_ERROR("Failed to append node %d, errorCode: %ld.\n", L2_MEMBER_NODE_ID, Status);
} }
break; break;
} }
case L2_INFO_FRAME: { case L2_INFO_FRAME: {
if (l2InstPtr->config.myType != L2_HEAD_NODE) { if (instPtr->config.myType != L2_HEAD_NODE) {
DBG_ERROR("Unexpected call from member node, Only Head Node can handle L2_SVC_FRAME.\n"); DBG_ERROR("Unexpected call from member node, Only Head Node can handle L2_SVC_FRAME.\n");
Status = EUNEXP; Status = EUNEXP;
break; break;
} }
COORD coordTemp; COORD coordTemp;
float tmpVal; float tmpVal;
Status = AppendNode(l2InstPtr->nodeInfoList, L2_MEMBER_NODE_ID, L2_MEMBER_NODE); Status = AppendNode(instPtr->nodeInfoList, L2_MEMBER_NODE_ID, L2_MEMBER_NODE);
if (Status != SUCCESS) { if (Status != SUCCESS) {
DBG_ERROR("Failed to append node %d, errorCode: %ld.\n", L2_MEMBER_NODE_ID, Status); DBG_ERROR("Failed to append node %d, errorCode: %ld.\n", L2_MEMBER_NODE_ID, Status);
break; break;
@@ -149,7 +149,7 @@ int32_t L2_Scan_PktHandler(L2_INST *l2InstPtr, uint8_t *msg) {
coordTemp.y = tmpVal; coordTemp.y = tmpVal;
memcpy(&tmpVal, msg+L2_MSG_INFO_POSZ, sizeof(float)); memcpy(&tmpVal, msg+L2_MSG_INFO_POSZ, sizeof(float));
coordTemp.z = tmpVal; coordTemp.z = tmpVal;
Status = UpdateNodePos(l2InstPtr->nodeInfoList, L2_MEMBER_NODE_ID, &coordTemp); Status = UpdateNodePos(instPtr->nodeInfoList, L2_MEMBER_NODE_ID, &coordTemp);
if (Status != SUCCESS) { if (Status != SUCCESS) {
DBG_ERROR("Failed to update node %d position, errorCode: %ld.\n", L2_MEMBER_NODE_ID, Status); DBG_ERROR("Failed to update node %d position, errorCode: %ld.\n", L2_MEMBER_NODE_ID, Status);
} }
@@ -159,16 +159,16 @@ int32_t L2_Scan_PktHandler(L2_INST *l2InstPtr, uint8_t *msg) {
coordTemp.y = tmpVal; coordTemp.y = tmpVal;
memcpy(&tmpVal, msg+L2_MSG_INFO_SPEEDZ, sizeof(float)); memcpy(&tmpVal, msg+L2_MSG_INFO_SPEEDZ, sizeof(float));
coordTemp.z = tmpVal; coordTemp.z = tmpVal;
Status = UpdateNodeSpeed(l2InstPtr->nodeInfoList, L2_MEMBER_NODE_ID, &coordTemp); Status = UpdateNodeSpeed(instPtr->nodeInfoList, L2_MEMBER_NODE_ID, &coordTemp);
if (Status != SUCCESS) { if (Status != SUCCESS) {
DBG_ERROR("Failed to update node %d speed, errorCode: %ld.\n", L2_MEMBER_NODE_ID, Status); DBG_ERROR("Failed to update node %d speed, errorCode: %ld.\n", L2_MEMBER_NODE_ID, Status);
} }
// Update Time deviation // Update Time deviation
int64_t devi, deviTrunc; int64_t devi, deviTrunc;
Status = L2_TimeSync_GetDeviatoion(l2InstPtr, &devi); Status = L2_TimeSync_GetDeviatoion(instPtr, &devi);
if (Status != SUCCESS) { if (Status != SUCCESS) {
DBG_ERROR("Failed to compute slot deviation.\n"); DBG_ERROR("Failed to compute slot deviation.\n");
UpdateNodeTimeDeviation(l2InstPtr->nodeInfoList, L2_MEMBER_NODE_ID, 0); UpdateNodeTimeDeviation(instPtr->nodeInfoList, L2_MEMBER_NODE_ID, 0);
} }
else { else {
if(devi > INT32_MAX || devi < INT32_MIN) { if(devi > INT32_MAX || devi < INT32_MIN) {
@@ -182,25 +182,25 @@ int32_t L2_Scan_PktHandler(L2_INST *l2InstPtr, uint8_t *msg) {
else { else {
deviTrunc = (int32_t)devi; deviTrunc = (int32_t)devi;
} }
UpdateNodeTimeDeviation(l2InstPtr->nodeInfoList, L2_MEMBER_NODE_ID, (uint32_t)deviTrunc); UpdateNodeTimeDeviation(instPtr->nodeInfoList, L2_MEMBER_NODE_ID, (uint32_t)deviTrunc);
} }
break; break;
} }
default: default:
DBG_ERROR("Unsupported frame type: %ld.\n", l2InstPtr->frame.last); DBG_ERROR("Unsupported frame type: %ld.\n", instPtr->frame.last);
Status = EUNEXP; Status = EUNEXP;
break; break;
} }
// Net setup finished. // Net setup finished.
if(L2_GetMainState(l2InstPtr) == INIT_STATE && Status == SUCCESS){ if(L2_GetMainState(instPtr) == INIT_STATE && Status == SUCCESS){
L2_EVENT l2Event; L2_EVENT l2Event;
L2_RTNE_CONFIG newRtneConfig; L2_RTNE_CONFIG newRtneConfig;
// Assert Net-Setup Done. // Assert Net-Setup Done.
l2Event.event = L2_EV_NET_SETUP_DONE; l2Event.event = L2_EV_NET_SETUP_DONE;
l2Event.msg = NULL; l2Event.msg = NULL;
AssertL2Event(l2InstPtr, &l2Event); AssertL2Event(instPtr, &l2Event);
// Assert Routine Table Update... // Assert Routine Table Update...
newRtneConfig.rtneTable = normalTable; newRtneConfig.rtneTable = normalTable;
@@ -208,7 +208,7 @@ int32_t L2_Scan_PktHandler(L2_INST *l2InstPtr, uint8_t *msg) {
newRtneConfig.rtneStep = 0; newRtneConfig.rtneStep = 0;
l2Event.event = L2_ROUTINE_UPDATE; l2Event.event = L2_ROUTINE_UPDATE;
l2Event.msg = &newRtneConfig; l2Event.msg = &newRtneConfig;
AssertL2Event(l2InstPtr, &l2Event); AssertL2Event(instPtr, &l2Event);
} }
return Status; return Status;
} }
@@ -217,24 +217,24 @@ int32_t L2_Scan_PktHandler(L2_INST *l2InstPtr, uint8_t *msg) {
/** /**
* @brief Execute a member node scan net procedure. * @brief Execute a member node scan net procedure.
* *
* @param l2InstPtr A pointer to scan net fsm. * @param instPtr A pointer to scan net fsm.
* *
* @return used slot (5ms slot). * @return used slot (5ms slot).
* *
******************************************************************************/ ******************************************************************************/
int32_t L2_Scan_Proc(L2_INST *l2InstPtr) { int32_t L2_Scan_Proc(L2_INST *instPtr) {
int32_t Status; int32_t Status;
/* l2InstPtr->nextState == NORMAL_STATE indicates network set up procedure was finished. /* instPtr->nextState == NORMAL_STATE indicates network set up procedure was finished.
* Skip 5ms for nodes for node configuration update. * Skip 5ms for nodes for node configuration update.
*/ */
DBG_INFO("-------- Scan Process --------\n"); DBG_INFO("-------- Scan Process --------\n");
if (l2InstPtr->nextState == NORMAL_STATE) { if (instPtr->nextState == NORMAL_STATE) {
DBG_INFO("NETSET DONE, halt 20ms before normal routine.\n"); DBG_INFO("NETSET DONE, halt 20ms before normal routine.\n");
if(l2InstPtr->config.myType != L2_HEAD_NODE) { if(instPtr->config.myType != L2_HEAD_NODE) {
Phy_SetBeamAngle(l2InstPtr->config.phyInstRef, 0.0, 0.0); Phy_SetBeamAngle(instPtr->config.phyInstRef, 0.0, 0.0);
Phy_SetBeamWidth(l2InstPtr->config.phyInstRef, PHY_ANT_BEAM_120); Phy_SetBeamWidth(instPtr->config.phyInstRef, PHY_ANT_BEAM_120);
Phy_SetMode(l2InstPtr->config.phyInstRef, PHY_MODE_SCHEDULE); Phy_SetMode(instPtr->config.phyInstRef, PHY_MODE_SCHEDULE);
L2_SetupNextSlot(l2InstPtr, L2_TOR_SUSPEND, L2_SCAN_FRAME); L2_SetupNextSlot(instPtr, L2_TOR_SUSPEND, L2_SCAN_FRAME);
} }
else { else {
DBG_ERROR("Illegal call from Head Node.\n"); DBG_ERROR("Illegal call from Head Node.\n");
@@ -249,23 +249,23 @@ int32_t L2_Scan_Proc(L2_INST *l2InstPtr) {
* Increase beam number after a Tx-Rx procedure pair, reset beam number when * Increase beam number after a Tx-Rx procedure pair, reset beam number when
* it reaches the boundary. * it reaches the boundary.
*/ */
if (l2InstPtr->config.myType == L2_HEAD_NODE) { if (instPtr->config.myType == L2_HEAD_NODE) {
// Apply a scan procedure // Apply a scan procedure
if (l2InstPtr->scanFsm.sacanCnt % 2 == 0){ if (instPtr->scanFsm.sacanCnt % 2 == 0){
// Antenna configure // Antenna configure
Phy_SetBeamWidth(l2InstPtr->config.phyInstRef, PHY_ANT_BEAM_8); Phy_SetBeamWidth(instPtr->config.phyInstRef, PHY_ANT_BEAM_8);
Phy_SetBeamNo(l2InstPtr->config.phyInstRef, l2InstPtr->scanFsm.beamNo); Phy_SetBeamNo(instPtr->config.phyInstRef, instPtr->scanFsm.beamNo);
Phy_SetMode(l2InstPtr->config.phyInstRef, PHY_MODE_SCHEDULE); Phy_SetMode(instPtr->config.phyInstRef, PHY_MODE_SCHEDULE);
L2_SetupNextSlot(l2InstPtr, L2_TOR_TX, L2_SCAN_FRAME); L2_SetupNextSlot(instPtr, L2_TOR_TX, L2_SCAN_FRAME);
} }
else { else {
// Antenna configure // Antenna configure
Phy_SetBeamWidth(l2InstPtr->config.phyInstRef, PHY_ANT_BEAM_8); Phy_SetBeamWidth(instPtr->config.phyInstRef, PHY_ANT_BEAM_8);
Phy_SetBeamNo(l2InstPtr->config.phyInstRef, l2InstPtr->scanFsm.beamNo); Phy_SetBeamNo(instPtr->config.phyInstRef, instPtr->scanFsm.beamNo);
Phy_SetMode(l2InstPtr->config.phyInstRef, PHY_MODE_SCHEDULE); Phy_SetMode(instPtr->config.phyInstRef, PHY_MODE_SCHEDULE);
L2_SetupNextSlot(l2InstPtr, L2_TOR_RX, L2_INFO_FRAME); L2_SetupNextSlot(instPtr, L2_TOR_RX, L2_INFO_FRAME);
// Update Scan beam number // Update Scan beam number
l2InstPtr->scanFsm.beamNo = (l2InstPtr->scanFsm.beamNo + 1) % L1_NUM_ANT_NRRW_PATTERN; instPtr->scanFsm.beamNo = (instPtr->scanFsm.beamNo + 1) % L1_NUM_ANT_NRRW_PATTERN;
} }
} }
/* Member node listening the head node pilot signal with wide beam width. /* Member node listening the head node pilot signal with wide beam width.
@@ -278,26 +278,26 @@ int32_t L2_Scan_Proc(L2_INST *l2InstPtr) {
else { else {
uint8_t *sendData; uint8_t *sendData;
// Pack node information. // Pack node information.
Status = ScanMsg_Packing(l2InstPtr, L2_INFO_FRAME); Status = ScanMsg_Packing(instPtr, L2_INFO_FRAME);
if (Status != SUCCESS) { if (Status != SUCCESS) {
DBG_INFO("Failed to pack response.\n"); DBG_INFO("Failed to pack response.\n");
} }
if (CircQueuePop(l2InstPtr->txQueue, (void **)(&sendData)) == true) { if (CircQueuePop(instPtr->txQueue, (void **)(&sendData)) == true) {
memcpy(l2InstPtr->sendPktData, sendData, RSP_PACK_LEN); memcpy(instPtr->sendPktData, sendData, RSP_PACK_LEN);
free(sendData); free(sendData);
} }
else { else {
DBG_INFO("Failed to pop tx Queue.\n"); DBG_INFO("Failed to pop tx Queue.\n");
} }
// Antenna configure // Antenna configure
Phy_SetBeamAngle(l2InstPtr->config.phyInstRef, 0.0, 0.0); Phy_SetBeamAngle(instPtr->config.phyInstRef, 0.0, 0.0);
Phy_SetBeamWidth(l2InstPtr->config.phyInstRef, PHY_ANT_BEAM_120); Phy_SetBeamWidth(instPtr->config.phyInstRef, PHY_ANT_BEAM_120);
Phy_SetMode(l2InstPtr->config.phyInstRef, PHY_MODE_LISTEN); Phy_SetMode(instPtr->config.phyInstRef, PHY_MODE_LISTEN);
L2_SetupNextSlot(l2InstPtr, L2_TOR_RX, L2_SCAN_FRAME); L2_SetupNextSlot(instPtr, L2_TOR_RX, L2_SCAN_FRAME);
} }
// Increase scan counter (for debug purpose). // Increase scan counter (for debug purpose).
l2InstPtr->scanFsm.sacanCnt ++; instPtr->scanFsm.sacanCnt ++;
return NUM_SLOTS_SKIP; return NUM_SLOTS_SKIP;
} }
@@ -311,7 +311,7 @@ int32_t L2_Scan_Proc(L2_INST *l2InstPtr) {
* @return used slot (5ms slot). * @return used slot (5ms slot).
* *
******************************************************************************/ ******************************************************************************/
int32_t L2_Scan_Routine(L2_INST *l2InstPtr, L2_RTNE_TABLE_ENTRY *schedEntry) { int32_t L2_Scan_Routine(L2_INST *instPtr, L2_RTNE_TABLE_ENTRY *schedEntry) {
DBG_INFO("-------- Scan Routine --------\n"); DBG_INFO("-------- Scan Routine --------\n");
ASSERT(false); // Not support. ASSERT(false); // Not support.
return 0; return 0;
df_datalink/src/networking/link_layer/service.c
+99 -99
View File
@@ -34,7 +34,7 @@
/**************************** Type Definitions *******************************/ /**************************** Type Definitions *******************************/
/************************** Function Prototypes ******************************/ /************************** Function Prototypes ******************************/
static int32_t Service_Packing(L2_INST *l2InstPtr, L2_FRAME_TYPE type); static int32_t Service_Packing(L2_INST *instPtr, L2_FRAME_TYPE type);
/************************** Variable Definitions *****************************/ /************************** Variable Definitions *****************************/
#ifdef DATALINK_ENABLE_BERTEST #ifdef DATALINK_ENABLE_BERTEST
@@ -58,9 +58,9 @@ static uint8_t berTestData[SVC_PACK_LEN] = {
* SUCCESS * SUCCESS
* *
******************************************************************************/ ******************************************************************************/
static int32_t Service_Packing(L2_INST *l2InstPtr, L2_FRAME_TYPE type) { static int32_t Service_Packing(L2_INST *instPtr, L2_FRAME_TYPE type) {
#ifndef DATALINK_ENABLE_BERTEST #ifndef DATALINK_ENABLE_BERTEST
ASSERT(l2InstPtr != NULL); ASSERT(instPtr != NULL);
int32_t Status; int32_t Status;
uint8_t *buff; uint8_t *buff;
uint32_t packLen; uint32_t packLen;
@@ -75,11 +75,11 @@ static int32_t Service_Packing(L2_INST *l2InstPtr, L2_FRAME_TYPE type) {
DBG_ERROR("Not enough space, failed to pack frame...\n"); DBG_ERROR("Not enough space, failed to pack frame...\n");
return ENOMEM; return ENOMEM;
} }
buff[L2_MSG_FRAME_NO] = L2_GetNextFrameNo(l2InstPtr); buff[L2_MSG_FRAME_NO] = L2_GetNextFrameNo(instPtr);
buff[L2_MSG_SLOT_NO] = L2_GetNextSlotNo(l2InstPtr); buff[L2_MSG_SLOT_NO] = L2_GetNextSlotNo(instPtr);
// TODO: Check Pack TimeSync info, and better approch?. // TODO: Check Pack TimeSync info, and better approch?.
nodeEntry = GetNode(l2InstPtr->nodeInfoList, L2_MEMBER_NODE_ID); nodeEntry = GetNode(instPtr->nodeInfoList, L2_MEMBER_NODE_ID);
if(nodeEntry != NULL) { if(nodeEntry != NULL) {
// check if deviation info outdated. // check if deviation info outdated.
if(nodeEntry->ttlCnt <= NUM_SERVICE_PRD_SLOTS) { if(nodeEntry->ttlCnt <= NUM_SERVICE_PRD_SLOTS) {
@@ -103,23 +103,23 @@ static int32_t Service_Packing(L2_INST *l2InstPtr, L2_FRAME_TYPE type) {
memcpy(buff+L2_MSG_SVC_TMSYNC, &timeDevi, L2_MSG_SVC_TMSYNC_LEN); memcpy(buff+L2_MSG_SVC_TMSYNC, &timeDevi, L2_MSG_SVC_TMSYNC_LEN);
// Pack Seq ID // Pack Seq ID
buff[L2_MSG_SVC_SEQ_ID] = l2InstPtr->serviceFsm.txSeqNum; buff[L2_MSG_SVC_SEQ_ID] = instPtr->serviceFsm.txSeqNum;
// Pack payload // Pack payload
memcpy(buff+L2_MSG_SVC_PAYLOAD, memcpy(buff+L2_MSG_SVC_PAYLOAD,
l2InstPtr->serviceFsm.txLog + L2_MSG_SVC_PAYLOAD_LEN*l2InstPtr->serviceFsm.txSeqNum, instPtr->serviceFsm.txLog + L2_MSG_SVC_PAYLOAD_LEN*instPtr->serviceFsm.txSeqNum,
L2_MSG_SVC_PAYLOAD_LEN); L2_MSG_SVC_PAYLOAD_LEN);
if (l2InstPtr->config.myType == L2_HEAD_NODE) { if (instPtr->config.myType == L2_HEAD_NODE) {
// if last packet reset counting // if last packet reset counting
l2InstPtr->serviceFsm.txSeqNum = (l2InstPtr->serviceFsm.txSeqNum instPtr->serviceFsm.txSeqNum = (instPtr->serviceFsm.txSeqNum
>= (PAYLOAD_FLJ-1)/L2_MSG_SVC_PAYLOAD+1) >= (PAYLOAD_FLJ-1)/L2_MSG_SVC_PAYLOAD+1)
? 0 : l2InstPtr->serviceFsm.txSeqNum + 1; ? 0 : instPtr->serviceFsm.txSeqNum + 1;
} }
else{ else{
// if last packet reset counting // if last packet reset counting
l2InstPtr->serviceFsm.txSeqNum = (l2InstPtr->serviceFsm.txSeqNum instPtr->serviceFsm.txSeqNum = (instPtr->serviceFsm.txSeqNum
>= (PAYLOAD_LJ-1)/L2_MSG_SVC_PAYLOAD+1) >= (PAYLOAD_LJ-1)/L2_MSG_SVC_PAYLOAD+1)
? 0 : l2InstPtr->serviceFsm.txSeqNum + 1; ? 0 : instPtr->serviceFsm.txSeqNum + 1;
} }
@@ -129,7 +129,7 @@ static int32_t Service_Packing(L2_INST *l2InstPtr, L2_FRAME_TYPE type) {
return Status; return Status;
} }
if(CircQueuePush(l2InstPtr->txQueue, buff) == false){ if(CircQueuePush(instPtr->txQueue, buff) == false){
DBG_ERROR("Cannot push packet to queue..\n"); DBG_ERROR("Cannot push packet to queue..\n");
return EQUEUE; return EQUEUE;
} }
@@ -137,27 +137,27 @@ static int32_t Service_Packing(L2_INST *l2InstPtr, L2_FRAME_TYPE type) {
} }
case L2_INFO_FRAME:{ case L2_INFO_FRAME:{
packLen = RSP_PACK_LEN; packLen = RSP_PACK_LEN;
SysMonitor_GetSysinfo(&(l2InstPtr->sysInfo)); SysMonitor_GetSysinfo(&(instPtr->sysInfo));
buff = malloc(packLen); buff = malloc(packLen);
if (buff == NULL) { if (buff == NULL) {
DBG_ERROR("Not enough space, failed to pack frame...\n"); DBG_ERROR("Not enough space, failed to pack frame...\n");
return ENOMEM; return ENOMEM;
} }
buff[L2_MSG_FRAME_NO] = L2_GetNextFrameNo(l2InstPtr); buff[L2_MSG_FRAME_NO] = L2_GetNextFrameNo(instPtr);
buff[L2_MSG_SLOT_NO] = L2_GetNextSlotNo(l2InstPtr); buff[L2_MSG_SLOT_NO] = L2_GetNextSlotNo(instPtr);
memcpy(buff+L2_MSG_INFO_POSX, &(l2InstPtr->sysInfo.posX), sizeof(float)); memcpy(buff+L2_MSG_INFO_POSX, &(instPtr->sysInfo.posX), sizeof(float));
memcpy(buff+L2_MSG_INFO_POSY, &(l2InstPtr->sysInfo.posY), sizeof(float)); memcpy(buff+L2_MSG_INFO_POSY, &(instPtr->sysInfo.posY), sizeof(float));
memcpy(buff+L2_MSG_INFO_POSZ, &(l2InstPtr->sysInfo.posZ), sizeof(float)); memcpy(buff+L2_MSG_INFO_POSZ, &(instPtr->sysInfo.posZ), sizeof(float));
memcpy(buff+L2_MSG_INFO_SPEEDX, &(l2InstPtr->sysInfo.vX), sizeof(float)); memcpy(buff+L2_MSG_INFO_SPEEDX, &(instPtr->sysInfo.vX), sizeof(float));
memcpy(buff+L2_MSG_INFO_SPEEDY, &(l2InstPtr->sysInfo.vY), sizeof(float)); memcpy(buff+L2_MSG_INFO_SPEEDY, &(instPtr->sysInfo.vY), sizeof(float));
memcpy(buff+L2_MSG_INFO_SPEEDZ, &(l2InstPtr->sysInfo.vZ), sizeof(float)); memcpy(buff+L2_MSG_INFO_SPEEDZ, &(instPtr->sysInfo.vZ), sizeof(float));
Status = L2_CrcAppend(buff, packLen - 2, L2_MSG_CRC, L2_CRC16_1021); Status = L2_CrcAppend(buff, packLen - 2, L2_MSG_CRC, L2_CRC16_1021);
if (Status != SUCCESS) { if (Status != SUCCESS) {
DBG_ERROR("Fail to append CRC...\n"); DBG_ERROR("Fail to append CRC...\n");
return Status; return Status;
} }
if(CircQueuePush(l2InstPtr->txQueue, buff) == false){ if(CircQueuePush(instPtr->txQueue, buff) == false){
DBG_ERROR("Cannot push packet to queue..\n"); DBG_ERROR("Cannot push packet to queue..\n");
return EQUEUE; return EQUEUE;
} }
@@ -181,22 +181,22 @@ static int32_t Service_Packing(L2_INST *l2InstPtr, L2_FRAME_TYPE type) {
/** /**
* @brief Initialize scan FSM. * @brief Initialize scan FSM.
* *
* @param l2InstPtr * @param instPtr
* *
* @return * @return
* *
******************************************************************************/ ******************************************************************************/
int32_t L2_Service_Init(L2_INST *l2InstPtr) { int32_t L2_Service_Init(L2_INST *instPtr) {
ASSERT(l2InstPtr != NULL); ASSERT(instPtr != NULL);
l2InstPtr->serviceFsm.txSeqNum = 0; instPtr->serviceFsm.txSeqNum = 0;
l2InstPtr->serviceFsm.rxSeqNum = 0; instPtr->serviceFsm.rxSeqNum = 0;
memset((&l2InstPtr->serviceFsm.rxLog), 0x00, sizeof(PAYLOAD)); memset((&instPtr->serviceFsm.rxLog), 0x00, sizeof(PAYLOAD));
memset((&l2InstPtr->serviceFsm.txLog), 0x00, sizeof(PAYLOAD)); memset((&instPtr->serviceFsm.txLog), 0x00, sizeof(PAYLOAD));
l2InstPtr->serviceFsm.pktRecv = 0; instPtr->serviceFsm.pktRecv = 0;
l2InstPtr->serviceFsm.pktRecved = 0; instPtr->serviceFsm.pktRecved = 0;
#ifdef DATALINK_ENABLE_BERTEST #ifdef DATALINK_ENABLE_BERTEST
l2InstPtr->serviceFsm.bitTotal = 0; instPtr->serviceFsm.bitTotal = 0;
l2InstPtr->serviceFsm.bitError = 0; instPtr->serviceFsm.bitError = 0;
#endif #endif
return SUCCESS; return SUCCESS;
} }
@@ -211,13 +211,13 @@ int32_t L2_Service_Init(L2_INST *l2InstPtr) {
* other error occurred. * other error occurred.
* *
******************************************************************************/ ******************************************************************************/
int32_t L2_Service_PktHandler(L2_INST *l2InstPtr, uint8_t *msg) { int32_t L2_Service_PktHandler(L2_INST *instPtr, uint8_t *msg) {
ASSERT(l2InstPtr != NULL); ASSERT(instPtr != NULL);
ASSERT(msg != NULL); ASSERT(msg != NULL);
int32_t Status; int32_t Status;
switch (l2InstPtr->frame.last) { switch (instPtr->frame.last) {
case L2_SVC_FRAME: { case L2_SVC_FRAME: {
if (l2InstPtr->config.myType == L2_HEAD_NODE) { if (instPtr->config.myType == L2_HEAD_NODE) {
DBG_ERROR("Unexpected call, Only None-Head Node can handle L2_SVC_FRAME.\n"); DBG_ERROR("Unexpected call, Only None-Head Node can handle L2_SVC_FRAME.\n");
Status = EUNEXP; Status = EUNEXP;
break; break;
@@ -231,7 +231,7 @@ int32_t L2_Service_PktHandler(L2_INST *l2InstPtr, uint8_t *msg) {
#ifdef DATALINK_ENABLE_AUTOTIMESYNC #ifdef DATALINK_ENABLE_AUTOTIMESYNC
l2Event.msg = NULL; l2Event.msg = NULL;
AssertL2Event(l2InstPtr, &l2Event); AssertL2Event(instPtr, &l2Event);
#else #else
int64_t timeAdj; int64_t timeAdj;
l2Event.msg = &timeAdj; l2Event.msg = &timeAdj;
@@ -241,57 +241,57 @@ int32_t L2_Service_PktHandler(L2_INST *l2InstPtr, uint8_t *msg) {
timeAdj |= (timeAdj & 0x0000000000800000) ? 0xFFFFFFFFFF000000 : 0x0000000000000000; timeAdj |= (timeAdj & 0x0000000000800000) ? 0xFFFFFFFFFF000000 : 0x0000000000000000;
// FIXME: better approach? // FIXME: better approach?
// Implement a time sync procedure, first time received a time deviation in a service period. // Implement a time sync procedure, first time received a time deviation in a service period.
if (seqNum % NUM_SERVICE_PRD_SLOTS == 0 || l2InstPtr->serviceFsm.seqNum != seqNum) { if (seqNum % NUM_SERVICE_PRD_SLOTS == 0 || instPtr->serviceFsm.seqNum != seqNum) {
AssertL2Event(l2InstPtr, &l2Event); AssertL2Event(instPtr, &l2Event);
} }
#endif #endif
#ifndef DATALINK_ENABLE_BERTEST #ifndef DATALINK_ENABLE_BERTEST
memcpy(l2InstPtr->serviceFsm.rxLog + L2_MSG_SVC_PAYLOAD_LEN*seqNum, msg + L2_MSG_SVC_PAYLOAD, L2_MSG_SVC_PAYLOAD_LEN); memcpy(instPtr->serviceFsm.rxLog + L2_MSG_SVC_PAYLOAD_LEN*seqNum, msg + L2_MSG_SVC_PAYLOAD, L2_MSG_SVC_PAYLOAD_LEN);
if (l2InstPtr->config.myType == L2_HEAD_NODE) { if (instPtr->config.myType == L2_HEAD_NODE) {
// if last packet reset counting // if last packet reset counting
if(seqNum >= (PAYLOAD_FLJ-1)/L2_MSG_SVC_PAYLOAD+1) { if(seqNum >= (PAYLOAD_FLJ-1)/L2_MSG_SVC_PAYLOAD+1) {
if(seqNum == l2InstPtr->serviceFsm.rxSeqNum) { if(seqNum == instPtr->serviceFsm.rxSeqNum) {
SysMonitor_UpdateRecvPayloadLJRaw(l2InstPtr->serviceFsm.rxLog); SysMonitor_UpdateRecvPayloadLJRaw(instPtr->serviceFsm.rxLog);
} }
l2InstPtr->serviceFsm.rxSeqNum = 0; instPtr->serviceFsm.rxSeqNum = 0;
} }
else { else {
if(l2InstPtr->serviceFsm.rxSeqNum == seqNum){ if(instPtr->serviceFsm.rxSeqNum == seqNum){
l2InstPtr->serviceFsm.rxSeqNum++; instPtr->serviceFsm.rxSeqNum++;
} }
} }
} }
else{ else{
// if last packet reset counting // if last packet reset counting
if(seqNum >= (PAYLOAD_LJ-1)/L2_MSG_SVC_PAYLOAD+1) { if(seqNum >= (PAYLOAD_LJ-1)/L2_MSG_SVC_PAYLOAD+1) {
if(seqNum == l2InstPtr->serviceFsm.rxSeqNum) { if(seqNum == instPtr->serviceFsm.rxSeqNum) {
SysMonitor_UpdateRecvPayloadFLJRaw(l2InstPtr->serviceFsm.rxLog); SysMonitor_UpdateRecvPayloadFLJRaw(instPtr->serviceFsm.rxLog);
} }
l2InstPtr->serviceFsm.rxSeqNum = 0; instPtr->serviceFsm.rxSeqNum = 0;
} }
else { else {
if(l2InstPtr->serviceFsm.rxSeqNum == seqNum){ if(instPtr->serviceFsm.rxSeqNum == seqNum){
l2InstPtr->serviceFsm.rxSeqNum++; instPtr->serviceFsm.rxSeqNum++;
} }
} }
} }
#else #else
for(int i = 0; i < SVC_PACK_LEN; i++) { for(int i = 0; i < SVC_PACK_LEN; i++) {
l2InstPtr->serviceFsm.bitError += count_ones(msg[i]^berTestData[i]); instPtr->serviceFsm.bitError += count_ones(msg[i]^berTestData[i]);
} }
l2InstPtr->serviceFsm.bitTotal += SVC_PACK_LEN*8; instPtr->serviceFsm.bitTotal += SVC_PACK_LEN*8;
#endif #endif
l2InstPtr->serviceFsm.pktRecved++; instPtr->serviceFsm.pktRecved++;
// Reset Node TTL // Reset Node TTL
Status = RstNodeTTL(l2InstPtr->nodeInfoList, L2_HEAD_NODE_ID); Status = RstNodeTTL(instPtr->nodeInfoList, L2_HEAD_NODE_ID);
if (Status != SUCCESS) { if (Status != SUCCESS) {
DBG_ERROR("Failed to reset node %d TTL, errorCode: %ld.\n", L2_MEMBER_NODE_ID, Status); DBG_ERROR("Failed to reset node %d TTL, errorCode: %ld.\n", L2_MEMBER_NODE_ID, Status);
} }
break; break;
} }
case L2_INFO_FRAME: { case L2_INFO_FRAME: {
if (l2InstPtr->config.myType != L2_HEAD_NODE) { if (instPtr->config.myType != L2_HEAD_NODE) {
DBG_ERROR("Unexpected call, Only Head Node can handle L2_SVC_FRAME.\n"); DBG_ERROR("Unexpected call, Only Head Node can handle L2_SVC_FRAME.\n");
Status = EUNEXP; Status = EUNEXP;
break; break;
@@ -305,7 +305,7 @@ int32_t L2_Service_PktHandler(L2_INST *l2InstPtr, uint8_t *msg) {
coordTemp.y = tmpVal; coordTemp.y = tmpVal;
memcpy(&tmpVal, msg+L2_MSG_INFO_POSZ, sizeof(float)); memcpy(&tmpVal, msg+L2_MSG_INFO_POSZ, sizeof(float));
coordTemp.z = tmpVal; coordTemp.z = tmpVal;
Status = UpdateNodePos(l2InstPtr->nodeInfoList, L2_MEMBER_NODE_ID, &coordTemp); Status = UpdateNodePos(instPtr->nodeInfoList, L2_MEMBER_NODE_ID, &coordTemp);
if (Status != SUCCESS) { if (Status != SUCCESS) {
DBG_ERROR("Failed to update node %d position, errorCode: %ld.\n", L2_MEMBER_NODE_ID, Status); DBG_ERROR("Failed to update node %d position, errorCode: %ld.\n", L2_MEMBER_NODE_ID, Status);
} }
@@ -315,7 +315,7 @@ int32_t L2_Service_PktHandler(L2_INST *l2InstPtr, uint8_t *msg) {
coordTemp.y = tmpVal; coordTemp.y = tmpVal;
memcpy(&tmpVal, msg+L2_MSG_INFO_SPEEDZ, sizeof(float)); memcpy(&tmpVal, msg+L2_MSG_INFO_SPEEDZ, sizeof(float));
coordTemp.z = tmpVal; coordTemp.z = tmpVal;
Status = UpdateNodeSpeed(l2InstPtr->nodeInfoList, L2_MEMBER_NODE_ID, &coordTemp); Status = UpdateNodeSpeed(instPtr->nodeInfoList, L2_MEMBER_NODE_ID, &coordTemp);
if (Status != SUCCESS) { if (Status != SUCCESS) {
DBG_ERROR("Failed to update node %d speed, errorCode: %ld.\n", L2_MEMBER_NODE_ID, Status); DBG_ERROR("Failed to update node %d speed, errorCode: %ld.\n", L2_MEMBER_NODE_ID, Status);
} }
@@ -323,7 +323,7 @@ int32_t L2_Service_PktHandler(L2_INST *l2InstPtr, uint8_t *msg) {
// TODO: Check // TODO: Check
// Update Time deviation // Update Time deviation
int64_t devi, deviTrunc; int64_t devi, deviTrunc;
Status = L2_TimeSync_GetDeviatoion(l2InstPtr, &devi); Status = L2_TimeSync_GetDeviatoion(instPtr, &devi);
if (Status != SUCCESS) { if (Status != SUCCESS) {
DBG_ERROR("Failed to compute slot deviation.\n"); DBG_ERROR("Failed to compute slot deviation.\n");
deviTrunc = 0; deviTrunc = 0;
@@ -340,28 +340,28 @@ int32_t L2_Service_PktHandler(L2_INST *l2InstPtr, uint8_t *msg) {
else { else {
deviTrunc = (int32_t)devi; deviTrunc = (int32_t)devi;
} }
UpdateNodeTimeDeviation(l2InstPtr->nodeInfoList, L2_MEMBER_NODE_ID, (uint32_t)deviTrunc); UpdateNodeTimeDeviation(instPtr->nodeInfoList, L2_MEMBER_NODE_ID, (uint32_t)deviTrunc);
} }
Status = UpdateNodeTimeDeviation(l2InstPtr->nodeInfoList, L2_MEMBER_NODE_ID, (uint32_t)deviTrunc); Status = UpdateNodeTimeDeviation(instPtr->nodeInfoList, L2_MEMBER_NODE_ID, (uint32_t)deviTrunc);
if (Status != SUCCESS) { if (Status != SUCCESS) {
DBG_ERROR("Failed to update node %d time deviation, errorCode: %ld.\n", L2_MEMBER_NODE_ID, Status); DBG_ERROR("Failed to update node %d time deviation, errorCode: %ld.\n", L2_MEMBER_NODE_ID, Status);
} }
#else #else
for(int i = 0; i < SVC_PACK_LEN; i++) { for(int i = 0; i < SVC_PACK_LEN; i++) {
l2InstPtr->serviceFsm.bitError += count_ones(msg[i]^berTestData[i]); instPtr->serviceFsm.bitError += count_ones(msg[i]^berTestData[i]);
} }
l2InstPtr->serviceFsm.bitTotal += SVC_PACK_LEN*8; instPtr->serviceFsm.bitTotal += SVC_PACK_LEN*8;
#endif #endif
l2InstPtr->serviceFsm.pktRecved++; instPtr->serviceFsm.pktRecved++;
// Reset Node TTL // Reset Node TTL
Status = RstNodeTTL(l2InstPtr->nodeInfoList, L2_MEMBER_NODE_ID); Status = RstNodeTTL(instPtr->nodeInfoList, L2_MEMBER_NODE_ID);
if (Status != SUCCESS) { if (Status != SUCCESS) {
DBG_ERROR("Failed to reset node %d ttl, errorCode: %ld.\n", L2_MEMBER_NODE_ID, Status); DBG_ERROR("Failed to reset node %d ttl, errorCode: %ld.\n", L2_MEMBER_NODE_ID, Status);
} }
break; break;
} }
default: default:
DBG_ERROR("Unsupported frame type: %ld.\n", l2InstPtr->frame.last); DBG_ERROR("Unsupported frame type: %ld.\n", instPtr->frame.last);
break; break;
} }
return Status; return Status;
@@ -371,24 +371,24 @@ int32_t L2_Service_PktHandler(L2_INST *l2InstPtr, uint8_t *msg) {
/** /**
* @brief Service FSM Routine. * @brief Service FSM Routine.
* *
* @param l2InstPtr * @param instPtr
* @param schedEntry * @param schedEntry
* *
* @return Slot has been used. * @return Slot has been used.
* *
******************************************************************************/ ******************************************************************************/
int32_t L2_Service_Routine(L2_INST *l2InstPtr, L2_RTNE_TABLE_ENTRY *schedEntry) { int32_t L2_Service_Routine(L2_INST *instPtr, L2_RTNE_TABLE_ENTRY *schedEntry) {
ASSERT(l2InstPtr != NULL); ASSERT(instPtr != NULL);
DBG_INFO("-------- ServiceNet Routine --------\n"); DBG_INFO("-------- ServiceNet Routine --------\n");
uint8_t *sendData; uint8_t *sendData;
int32_t usedSlots; int32_t usedSlots;
L2_TOR_TYPE torState; L2_TOR_TYPE torState;
if(schedEntry->src == l2InstPtr->config.myId){ if(schedEntry->src == instPtr->config.myId){
torState = L2_TOR_TX; torState = L2_TOR_TX;
} }
else if(schedEntry->dst == l2InstPtr->config.myId){ else if(schedEntry->dst == instPtr->config.myId){
torState = L2_TOR_RX; torState = L2_TOR_RX;
l2InstPtr->serviceFsm.pktRecv++; instPtr->serviceFsm.pktRecv++;
} }
else{ else{
torState = L2_TOR_SUSPEND; torState = L2_TOR_SUSPEND;
@@ -398,41 +398,41 @@ int32_t L2_Service_Routine(L2_INST *l2InstPtr, L2_RTNE_TABLE_ENTRY *schedEntry)
case L2_SLOT_SERVICE: case L2_SLOT_SERVICE:
if(torState == L2_TOR_TX){ if(torState == L2_TOR_TX){
#ifndef DATALINK_ENABLE_BERTEST #ifndef DATALINK_ENABLE_BERTEST
if (l2InstPtr->serviceFsm.txSeqNum == 0) { if (instPtr->serviceFsm.txSeqNum == 0) {
if(l2InstPtr->config.myType == L2_HEAD_NODE){ if(instPtr->config.myType == L2_HEAD_NODE){
SysMonitor_GetSendPayloadFLJRaw(l2InstPtr->serviceFsm.txLog); SysMonitor_GetSendPayloadFLJRaw(instPtr->serviceFsm.txLog);
} }
else { else {
SysMonitor_GetSendPayloadLJRaw(l2InstPtr->serviceFsm.txLog); SysMonitor_GetSendPayloadLJRaw(instPtr->serviceFsm.txLog);
} }
} }
// Packing service data // Packing service data
Service_Packing(l2InstPtr, L2_SVC_FRAME); Service_Packing(instPtr, L2_SVC_FRAME);
if (CircQueuePop(l2InstPtr->txQueue, (void **)(&sendData)) == true) { if (CircQueuePop(instPtr->txQueue, (void **)(&sendData)) == true) {
memcpy(l2InstPtr->sendPktData, sendData, SVC_PACK_LEN); memcpy(instPtr->sendPktData, sendData, SVC_PACK_LEN);
free(sendData); free(sendData);
} }
else { else {
DBG_ERROR("Failed to pop tx queue.\n"); DBG_ERROR("Failed to pop tx queue.\n");
} }
#else #else
memcpy(l2InstPtr->config.phyInstRef->sendFrameBuff, berTestData, SVC_PACK_LEN); memcpy(instPtr->config.phyInstRef->sendFrameBuff, berTestData, SVC_PACK_LEN);
#endif #endif
} }
// FIXME: Steer antenna to point to destination. // FIXME: Steer antenna to point to destination.
Phy_SetBeamAngle(l2InstPtr->config.phyInstRef, 0.0, 0.0); Phy_SetBeamAngle(instPtr->config.phyInstRef, 0.0, 0.0);
Phy_SetBeamWidth(l2InstPtr->config.phyInstRef, PHY_ANT_BEAM_8); Phy_SetBeamWidth(instPtr->config.phyInstRef, PHY_ANT_BEAM_8);
Phy_SetMode(l2InstPtr->config.phyInstRef, PHY_MODE_SCHEDULE); Phy_SetMode(instPtr->config.phyInstRef, PHY_MODE_SCHEDULE);
L2_SetupNextSlot(l2InstPtr, torState, L2_SVC_FRAME); L2_SetupNextSlot(instPtr, torState, L2_SVC_FRAME);
usedSlots = NUM_SERVICE_SLOTS; usedSlots = NUM_SERVICE_SLOTS;
break; break;
#ifndef DATALINK_ENABLE_BERTEST #ifndef DATALINK_ENABLE_BERTEST
case L2_SLOT_ADVERTISE: case L2_SLOT_ADVERTISE:
if(torState == L2_TOR_TX){ if(torState == L2_TOR_TX){
// Packing response data. // Packing response data.
Service_Packing(l2InstPtr, L2_INFO_FRAME); Service_Packing(instPtr, L2_INFO_FRAME);
if (CircQueuePop(l2InstPtr->txQueue, (void **)(&sendData)) == true) { if (CircQueuePop(instPtr->txQueue, (void **)(&sendData)) == true) {
memcpy(l2InstPtr->sendPktData, sendData, RSP_PACK_LEN); memcpy(instPtr->sendPktData, sendData, RSP_PACK_LEN);
free(sendData); free(sendData);
} }
else { else {
@@ -440,10 +440,10 @@ int32_t L2_Service_Routine(L2_INST *l2InstPtr, L2_RTNE_TABLE_ENTRY *schedEntry)
} }
} }
// FIXME: Steer antenna to point to destination. // FIXME: Steer antenna to point to destination.
Phy_SetBeamAngle(l2InstPtr->config.phyInstRef, 0.0, 0.0); Phy_SetBeamAngle(instPtr->config.phyInstRef, 0.0, 0.0);
Phy_SetBeamWidth(l2InstPtr->config.phyInstRef, PHY_ANT_BEAM_120); Phy_SetBeamWidth(instPtr->config.phyInstRef, PHY_ANT_BEAM_120);
Phy_SetMode(l2InstPtr->config.phyInstRef, PHY_MODE_SCHEDULE); Phy_SetMode(instPtr->config.phyInstRef, PHY_MODE_SCHEDULE);
L2_SetupNextSlot(l2InstPtr, torState, L2_INFO_FRAME); L2_SetupNextSlot(instPtr, torState, L2_INFO_FRAME);
usedSlots = NUM_RESPONSE_SLOTS; usedSlots = NUM_RESPONSE_SLOTS;
break; break;
#endif #endif
@@ -454,11 +454,11 @@ int32_t L2_Service_Routine(L2_INST *l2InstPtr, L2_RTNE_TABLE_ENTRY *schedEntry)
#ifdef DATALINK_ENABLE_BERTEST #ifdef DATALINK_ENABLE_BERTEST
uint32_t pktLoss; uint32_t pktLoss;
double errorRate, lossRate; double errorRate, lossRate;
pktLoss = l2InstPtr->serviceFsm.pktRecv - l2InstPtr->serviceFsm.pktRecved; pktLoss = instPtr->serviceFsm.pktRecv - instPtr->serviceFsm.pktRecved;
lossRate = (l2InstPtr->serviceFsm.pktRecv == 0) ? 0 : (double)pktLoss / (double)l2InstPtr->serviceFsm.pktRecv; lossRate = (instPtr->serviceFsm.pktRecv == 0) ? 0 : (double)pktLoss / (double)instPtr->serviceFsm.pktRecv;
errorRate = (l2InstPtr->serviceFsm.bitTotal == 0) ? 0 : (double)l2InstPtr->serviceFsm.bitError / (double)l2InstPtr->serviceFsm.bitTotal; errorRate = (instPtr->serviceFsm.bitTotal == 0) ? 0 : (double)instPtr->serviceFsm.bitError / (double)instPtr->serviceFsm.bitTotal;
DBG_PRINT("Packet Loss Rate -- Total: %lu, Loss: %lu.\n", l2InstPtr->serviceFsm.pktRecv, pktLoss); DBG_PRINT("Packet Loss Rate -- Total: %lu, Loss: %lu.\n", instPtr->serviceFsm.pktRecv, pktLoss);
DBG_PRINT("Bit Error Rate -- Total:%lu, Error:%lu.\n", l2InstPtr->serviceFsm.bitTotal, l2InstPtr->serviceFsm.bitError); DBG_PRINT("Bit Error Rate -- Total:%lu, Error:%lu.\n", instPtr->serviceFsm.bitTotal, instPtr->serviceFsm.bitError);
#endif #endif
return usedSlots; return usedSlots;
} }
df_datalink/src/networking/link_layer/time_sync.c
+16 -16
View File
@@ -54,16 +54,16 @@
/** /**
* @brief Apply local time adjustment by addition. * @brief Apply local time adjustment by addition.
* *
* @param l2InstPtr. * @param instPtr.
* @param adjustment * @param adjustment
* *
* @return * @return
* *
******************************************************************************/ ******************************************************************************/
inline int32_t L2_TimeSync_LocalTimerAdd(L2_INST *l2InstPtr, int64_t adjustment) { inline int32_t L2_TimeSync_LocalTimerAdd(L2_INST *instPtr, int64_t adjustment) {
// read nextSendTime from physical layer // read nextSendTime from physical layer
Phy_SetTimeWord(l2InstPtr->config.phyInstRef, adjustment, PHY_TIME_WORD_LOCAL_ADJ); Phy_SetTimeWord(instPtr->config.phyInstRef, adjustment, PHY_TIME_WORD_LOCAL_ADJ);
PhyWrite(l2InstPtr->config.phyInstRef, TM_ADJ_MODE, TM_ADJ_MODE_ADD); PhyWrite(instPtr->config.phyInstRef, TM_ADJ_MODE, TM_ADJ_MODE_ADD);
return SUCCESS; return SUCCESS;
} }
@@ -71,42 +71,42 @@ inline int32_t L2_TimeSync_LocalTimerAdd(L2_INST *l2InstPtr, int64_t adjustment)
/** /**
* @brief Align slot, base on physical layer information. * @brief Align slot, base on physical layer information.
* *
* @param l2InstPtr. * @param instPtr.
* *
* @return * @return
* *
******************************************************************************/ ******************************************************************************/
int32_t L2_TimeSync_SlotAlign(L2_INST *l2InstPtr) { int32_t L2_TimeSync_SlotAlign(L2_INST *instPtr) {
int32_t Status; int32_t Status;
int64_t slotAdj; int64_t slotAdj;
Status = L2_TimeSync_GetDeviatoion(l2InstPtr, &slotAdj); Status = L2_TimeSync_GetDeviatoion(instPtr, &slotAdj);
if (Status != SUCCESS) { if (Status != SUCCESS) {
DBG_ERROR("Failed to compute slot deviation.\n"); DBG_ERROR("Failed to compute slot deviation.\n");
return Status; return Status;
} }
return L2_TimeSync_LocalTimerAdd(l2InstPtr, slotAdj); return L2_TimeSync_LocalTimerAdd(instPtr, slotAdj);
} }
/*****************************************************************************/ /*****************************************************************************/
/** /**
* @brief Compute slot deviation of received frame. * @brief Compute slot deviation of received frame.
* *
* @param l2InstPtr. * @param instPtr.
* @param devi * @param devi
* *
* @return * @return
* *
******************************************************************************/ ******************************************************************************/
int32_t L2_TimeSync_GetDeviatoion(L2_INST *l2InstPtr, int64_t *devi) { int32_t L2_TimeSync_GetDeviatoion(L2_INST *instPtr, int64_t *devi) {
// read nextSendTime from physical layer // read nextSendTime from physical layer
int64_t currentTarget, lastFrameSync, slotMeasure; int64_t currentTarget, lastFrameSync, slotMeasure;
currentTarget = PhyRead(l2InstPtr->config.phyInstRef, TM_TARGET_L); currentTarget = PhyRead(instPtr->config.phyInstRef, TM_TARGET_L);
currentTarget |= ((int64_t)PhyRead(l2InstPtr->config.phyInstRef, TM_TARGET_H)) << 32; currentTarget |= ((int64_t)PhyRead(instPtr->config.phyInstRef, TM_TARGET_H)) << 32;
lastFrameSync = PhyRead(l2InstPtr->config.phyInstRef, FSTIME_L); lastFrameSync = PhyRead(instPtr->config.phyInstRef, FSTIME_L);
lastFrameSync |= ((int64_t)PhyRead(l2InstPtr->config.phyInstRef, FSTIME_H)) << 32; lastFrameSync |= ((int64_t)PhyRead(instPtr->config.phyInstRef, FSTIME_H)) << 32;
slotMeasure = (currentTarget > lastFrameSync) ? (currentTarget - lastFrameSync) slotMeasure = (currentTarget > lastFrameSync) ? (currentTarget - lastFrameSync)
: (currentTarget - lastFrameSync + PHY_LOCAL_CNT_MAX); : (currentTarget - lastFrameSync + PHY_LOCAL_CNT_MAX);
switch (Phy_GetLastDetectedFrameType(l2InstPtr->config.phyInstRef)) { switch (Phy_GetLastDetectedFrameType(instPtr->config.phyInstRef)) {
case L1_2KBPS_20MS_FRAME : case L1_2KBPS_20MS_FRAME :
*devi = ((int64_t)PHY_SYNCT2FEND_2K_20MS) - slotMeasure; *devi = ((int64_t)PHY_SYNCT2FEND_2K_20MS) - slotMeasure;
break; break;
@@ -114,7 +114,7 @@ int32_t L2_TimeSync_GetDeviatoion(L2_INST *l2InstPtr, int64_t *devi) {
*devi = ((int64_t)PHY_SYNCT2FEND_2K_100MS) - slotMeasure; *devi = ((int64_t)PHY_SYNCT2FEND_2K_100MS) - slotMeasure;
break; break;
default: default:
DBG_ERROR("Unsupported frame type: %d.\n", Phy_GetLastDetectedFrameType(l2InstPtr->config.phyInstRef)); DBG_ERROR("Unsupported frame type: %d.\n", Phy_GetLastDetectedFrameType(instPtr->config.phyInstRef));
return EIGNORE; return EIGNORE;
} }
return SUCCESS; return SUCCESS;