XiaQingMiao/datalink_nc_mode
1
0
Fork 0
forked from gatsby/datalink_repo
Code Pull Requests 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.
*
* @param dataBuff pointer to data buffer.
* dataLen length of specified data.
* crcType type of crc
* crc a pointer to 32 bit crc container.
* @param dataLen length of specified data.
* @param crcType type of crc
* @param crc a pointer to 32 bit crc container.
*
* @return FAILURE crc type not support.
* 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.
*
* @param dataBuff pointer to data buffer.
* dataLen length of specified data.
* crcPos position of crc insert position
* crcType type of crc
* @param dataLen length of specified data.
* @param crcPos position of crc insert position
* @param crcType type of crc
*
* @return FAILURE crc type not support.
* 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.
*
* @param dataBuff pointer to data buffer.
* dataLen length of specified data.
* crcType type of crc
* @param dataLen length of specified data.
* @param crcType type of crc
*
* @return FAILURE crc type not support.
* 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];
/************************** Function Prototypes ******************************/
// Main
int32_t Linklayer_Init(L2_INST **l2InstPtr, L2_CONFIG *config);
int32_t Linklayer_Proc(L2_INST *l2InstPtr);
int32_t Linklayer_EventHandler(L2_INST *l2InstPtr, L2_EVENT* l2Event);
int32_t Linklayer_PktHandler(L2_INST *l2InstPtr);
int32_t L2_SteerBeam2Node(L2_INST *l2InstPtr, uint8_t nodeId);
int32_t L2_SetupNextSlot(L2_INST *l2InstPtr, L2_TOR_TYPE txrxMode, L2_FRAME_TYPE frameType);
int32_t Linklayer_Init(L2_INST **instPtr, L2_CONFIG *config);
int32_t Linklayer_Proc(L2_INST *instPtr);
int32_t Linklayer_EventHandler(L2_INST *instPtr, L2_EVENT* l2Event);
int32_t Linklayer_PktHandler(L2_INST *instPtr);
int32_t L2_SteerBeam2Node(L2_INST *instPtr, uint8_t nodeId);
int32_t L2_SetupNextSlot(L2_INST *instPtr, L2_TOR_TYPE txrxMode, L2_FRAME_TYPE frameType);
// 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);
int32_t L2_CancelEvent(L2_SCHD_EVENT* schdEvent);
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_ScheduleEventAfter(L2_INST *l2InstPtr, 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_RecheduleEventAfter(L2_INST *l2InstPtr, L2_SCHD_EVENT* schdEvent, uint32_t numSlots);
int32_t L2_ScheduleEventAt(L2_INST *instPtr, L2_SCHD_EVENT* schdEvent, uint32_t frameNo, uint32_t slotNo);
int32_t L2_ScheduleEventAfter(L2_INST *instPtr, L2_SCHD_EVENT* schdEvent, uint32_t numSlots);
int32_t L2_RecheduleEventAt(L2_INST *instPtr, L2_SCHD_EVENT* schdEvent, uint32_t frameNo, uint32_t slotNo);
int32_t L2_RecheduleEventAfter(L2_INST *instPtr, L2_SCHD_EVENT* schdEvent, uint32_t numSlots);
// Framing
void L2_FrameStateUpdate(L2_INST *l2InstPtr);
L2_FRAME_TYPE L2_GetLastRcvFrameType(L2_INST *l2InstPtr);
void L2_FrameStateUpdate(L2_INST *instPtr);
L2_FRAME_TYPE L2_GetLastRcvFrameType(L2_INST *instPtr);
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);
bool L2_CrcCheck(uint8_t* dataBuff, uint32_t dataLen, L2_CRC_TYPE crcType);
// Miscellaneous
L2_NODE_STATE L2_GetMainState(L2_INST *l2InstPtr);
bool L2_isDataAvail(L2_INST *l2InstPtr);
bool L2_isCxn2Net(L2_INST *l2InstPtr);
L2_NODE_STATE L2_GetMainState(L2_INST *instPtr);
bool L2_isDataAvail(L2_INST *instPtr);
bool L2_isCxn2Net(L2_INST *instPtr);
int32_t L2_GetFrameLen(L2_FRAME_TYPE l2FrameType);
uint32_t L2_GetLastFrameNo(L2_INST *l2InstPtr);
uint32_t L2_GetLastSlotNo(L2_INST *l2InstPtr);
uint32_t L2_GetCurrentFrameNo(L2_INST *l2InstPtr);
uint32_t L2_GetCurrentSlotNo(L2_INST *l2InstPtr);
uint32_t L2_GetNextFrameNo(L2_INST *l2InstPtr);
uint32_t L2_GetNextSlotNo(L2_INST *l2InstPtr);
uint32_t L2_GetLastFrameNo(L2_INST *instPtr);
uint32_t L2_GetLastSlotNo(L2_INST *instPtr);
uint32_t L2_GetCurrentFrameNo(L2_INST *instPtr);
uint32_t L2_GetCurrentSlotNo(L2_INST *instPtr);
uint32_t L2_GetNextFrameNo(L2_INST *instPtr);
uint32_t L2_GetNextSlotNo(L2_INST *instPtr);
// Scanning
int32_t L2_Scan_Init(L2_INST *l2InstPtr);
int32_t L2_Scan_PktHandler(L2_INST *l2InstPtr, uint8_t *msg);
int32_t L2_Scan_Proc(L2_INST *l2InstPtr);
int32_t L2_Scan_Routine(L2_INST *l2InstPtr, L2_RTNE_TABLE_ENTRY *schedEntry);
int32_t L2_Scan_Init(L2_INST *instPtr);
int32_t L2_Scan_PktHandler(L2_INST *instPtr, uint8_t *msg);
int32_t L2_Scan_Proc(L2_INST *instPtr);
int32_t L2_Scan_Routine(L2_INST *instPtr, L2_RTNE_TABLE_ENTRY *schedEntry);
// Service
int32_t L2_Service_Init(L2_INST *l2InstPtr);
int32_t L2_Service_PktHandler(L2_INST *l2InstPtr, uint8_t *msg);
int32_t L2_Service_Proc(L2_INST *l2InstPtr);
int32_t L2_Service_Routine(L2_INST *l2InstPtr, L2_RTNE_TABLE_ENTRY *schedEntry);
int32_t L2_Service_Init(L2_INST *instPtr);
int32_t L2_Service_PktHandler(L2_INST *instPtr, uint8_t *msg);
int32_t L2_Service_Proc(L2_INST *instPtr);
int32_t L2_Service_Routine(L2_INST *instPtr, L2_RTNE_TABLE_ENTRY *schedEntry);
// Time Sync
int32_t L2_TimeSync_LocalTimerAdd(L2_INST *l2InstPtr, int64_t adjustment);
int32_t L2_TimeSync_SlotAlign(L2_INST *l2InstPtr);
int32_t L2_TimeSync_GetDeviatoion(L2_INST *l2InstPtr, int64_t *devi);
int32_t L2_TimeSync_LocalTimerAdd(L2_INST *instPtr, int64_t adjustment);
int32_t L2_TimeSync_SlotAlign(L2_INST *instPtr);
int32_t L2_TimeSync_GetDeviatoion(L2_INST *instPtr, int64_t *devi);
#endif /* __LINK_LAYER_H_ */
df_datalink/src/networking/link_layer/scan.c
+63 -63
View File
@@ -33,7 +33,7 @@
/************************** Variable Definitions *****************************/
/************************** 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.
@@ -45,35 +45,35 @@ static int32_t ScanMsg_Packing(L2_INST *l2InstPtr, L2_FRAME_TYPE type);
* SUCCESS
*
******************************************************************************/
static int32_t ScanMsg_Packing(L2_INST *l2InstPtr, L2_FRAME_TYPE type) {
ASSERT(l2InstPtr != NULL);
static int32_t ScanMsg_Packing(L2_INST *instPtr, L2_FRAME_TYPE type) {
ASSERT(instPtr != NULL);
int32_t Status;
uint8_t *buff;
uint32_t packLen;
switch (type) {
case L2_INFO_FRAME:{
packLen = RSP_PACK_LEN;
SysMonitor_GetSysinfo(&(l2InstPtr->sysInfo));
SysMonitor_GetSysinfo(&(instPtr->sysInfo));
buff = malloc(packLen);
if (buff == NULL) {
DBG_ERROR("Not enough space, failed to pack frame...\n");
return ENOMEM;
}
buff[L2_MSG_FRAME_NO] = L2_GetNextFrameNo(l2InstPtr);
buff[L2_MSG_SLOT_NO] = L2_GetNextSlotNo(l2InstPtr);
memcpy(buff+L2_MSG_INFO_POSX, &(l2InstPtr->sysInfo.posX), sizeof(float));
memcpy(buff+L2_MSG_INFO_POSY, &(l2InstPtr->sysInfo.posY), sizeof(float));
memcpy(buff+L2_MSG_INFO_POSZ, &(l2InstPtr->sysInfo.posZ), sizeof(float));
memcpy(buff+L2_MSG_INFO_SPEEDX, &(l2InstPtr->sysInfo.vX), sizeof(float));
memcpy(buff+L2_MSG_INFO_SPEEDY, &(l2InstPtr->sysInfo.vY), sizeof(float));
memcpy(buff+L2_MSG_INFO_SPEEDZ, &(l2InstPtr->sysInfo.vZ), sizeof(float));
buff[L2_MSG_FRAME_NO] = L2_GetNextFrameNo(instPtr);
buff[L2_MSG_SLOT_NO] = L2_GetNextSlotNo(instPtr);
memcpy(buff+L2_MSG_INFO_POSX, &(instPtr->sysInfo.posX), sizeof(float));
memcpy(buff+L2_MSG_INFO_POSY, &(instPtr->sysInfo.posY), sizeof(float));
memcpy(buff+L2_MSG_INFO_POSZ, &(instPtr->sysInfo.posZ), sizeof(float));
memcpy(buff+L2_MSG_INFO_SPEEDX, &(instPtr->sysInfo.vX), sizeof(float));
memcpy(buff+L2_MSG_INFO_SPEEDY, &(instPtr->sysInfo.vY), 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);
if (Status != SUCCESS) {
DBG_ERROR("Fail to append CRC, error code: %ld.\n", Status);
return Status;
}
if(CircQueuePush(l2InstPtr->txQueue, buff) == false){
if(CircQueuePush(instPtr->txQueue, buff) == false){
DBG_ERROR("Cannot push packet to queue..\n");
return EQUEUE;
}
@@ -92,15 +92,15 @@ static int32_t ScanMsg_Packing(L2_INST *l2InstPtr, L2_FRAME_TYPE type) {
/**
* @brief Initialize scan FSM.
*
* @param l2InstPtr
* @param instPtr
*
* @return
*
******************************************************************************/
int32_t L2_Scan_Init(L2_INST *l2InstPtr) {
ASSERT(l2InstPtr != NULL);
l2InstPtr->scanFsm.sacanCnt = 0;
l2InstPtr->scanFsm.beamNo = 0;
int32_t L2_Scan_Init(L2_INST *instPtr) {
ASSERT(instPtr != NULL);
instPtr->scanFsm.sacanCnt = 0;
instPtr->scanFsm.beamNo = 0;
return SUCCESS;
}
@@ -113,32 +113,32 @@ int32_t L2_Scan_Init(L2_INST *l2InstPtr) {
* @return
*
******************************************************************************/
int32_t L2_Scan_PktHandler(L2_INST *l2InstPtr, uint8_t *msg) {
ASSERT(l2InstPtr != NULL);
int32_t L2_Scan_PktHandler(L2_INST *instPtr, uint8_t *msg) {
ASSERT(instPtr != NULL);
ASSERT(msg != NULL);
int32_t Status;
switch (l2InstPtr->frame.last) {
switch (instPtr->frame.last) {
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");
Status = EUNEXP;
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) {
DBG_ERROR("Failed to append node %d, errorCode: %ld.\n", L2_MEMBER_NODE_ID, Status);
}
break;
}
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");
Status = EUNEXP;
break;
}
COORD coordTemp;
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) {
DBG_ERROR("Failed to append node %d, errorCode: %ld.\n", L2_MEMBER_NODE_ID, Status);
break;
@@ -149,7 +149,7 @@ int32_t L2_Scan_PktHandler(L2_INST *l2InstPtr, uint8_t *msg) {
coordTemp.y = tmpVal;
memcpy(&tmpVal, msg+L2_MSG_INFO_POSZ, sizeof(float));
coordTemp.z = tmpVal;
Status = UpdateNodePos(l2InstPtr->nodeInfoList, L2_MEMBER_NODE_ID, &coordTemp);
Status = UpdateNodePos(instPtr->nodeInfoList, L2_MEMBER_NODE_ID, &coordTemp);
if (Status != SUCCESS) {
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;
memcpy(&tmpVal, msg+L2_MSG_INFO_SPEEDZ, sizeof(float));
coordTemp.z = tmpVal;
Status = UpdateNodeSpeed(l2InstPtr->nodeInfoList, L2_MEMBER_NODE_ID, &coordTemp);
Status = UpdateNodeSpeed(instPtr->nodeInfoList, L2_MEMBER_NODE_ID, &coordTemp);
if (Status != SUCCESS) {
DBG_ERROR("Failed to update node %d speed, errorCode: %ld.\n", L2_MEMBER_NODE_ID, Status);
}
// Update Time deviation
int64_t devi, deviTrunc;
Status = L2_TimeSync_GetDeviatoion(l2InstPtr, &devi);
Status = L2_TimeSync_GetDeviatoion(instPtr, &devi);
if (Status != SUCCESS) {
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 {
if(devi > INT32_MAX || devi < INT32_MIN) {
@@ -182,25 +182,25 @@ int32_t L2_Scan_PktHandler(L2_INST *l2InstPtr, uint8_t *msg) {
else {
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;
}
default:
DBG_ERROR("Unsupported frame type: %ld.\n", l2InstPtr->frame.last);
DBG_ERROR("Unsupported frame type: %ld.\n", instPtr->frame.last);
Status = EUNEXP;
break;
}
// Net setup finished.
if(L2_GetMainState(l2InstPtr) == INIT_STATE && Status == SUCCESS){
if(L2_GetMainState(instPtr) == INIT_STATE && Status == SUCCESS){
L2_EVENT l2Event;
L2_RTNE_CONFIG newRtneConfig;
// Assert Net-Setup Done.
l2Event.event = L2_EV_NET_SETUP_DONE;
l2Event.msg = NULL;
AssertL2Event(l2InstPtr, &l2Event);
AssertL2Event(instPtr, &l2Event);
// Assert Routine Table Update...
newRtneConfig.rtneTable = normalTable;
@@ -208,7 +208,7 @@ int32_t L2_Scan_PktHandler(L2_INST *l2InstPtr, uint8_t *msg) {
newRtneConfig.rtneStep = 0;
l2Event.event = L2_ROUTINE_UPDATE;
l2Event.msg = &newRtneConfig;
AssertL2Event(l2InstPtr, &l2Event);
AssertL2Event(instPtr, &l2Event);
}
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.
*
* @param l2InstPtr A pointer to scan net fsm.
* @param instPtr A pointer to scan net fsm.
*
* @return used slot (5ms slot).
*
******************************************************************************/
int32_t L2_Scan_Proc(L2_INST *l2InstPtr) {
int32_t L2_Scan_Proc(L2_INST *instPtr) {
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.
*/
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");
if(l2InstPtr->config.myType != L2_HEAD_NODE) {
Phy_SetBeamAngle(l2InstPtr->config.phyInstRef, 0.0, 0.0);
Phy_SetBeamWidth(l2InstPtr->config.phyInstRef, PHY_ANT_BEAM_120);
Phy_SetMode(l2InstPtr->config.phyInstRef, PHY_MODE_SCHEDULE);
L2_SetupNextSlot(l2InstPtr, L2_TOR_SUSPEND, L2_SCAN_FRAME);
if(instPtr->config.myType != L2_HEAD_NODE) {
Phy_SetBeamAngle(instPtr->config.phyInstRef, 0.0, 0.0);
Phy_SetBeamWidth(instPtr->config.phyInstRef, PHY_ANT_BEAM_120);
Phy_SetMode(instPtr->config.phyInstRef, PHY_MODE_SCHEDULE);
L2_SetupNextSlot(instPtr, L2_TOR_SUSPEND, L2_SCAN_FRAME);
}
else {
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
* it reaches the boundary.
*/
if (l2InstPtr->config.myType == L2_HEAD_NODE) {
if (instPtr->config.myType == L2_HEAD_NODE) {
// Apply a scan procedure
if (l2InstPtr->scanFsm.sacanCnt % 2 == 0){
if (instPtr->scanFsm.sacanCnt % 2 == 0){
// Antenna configure
Phy_SetBeamWidth(l2InstPtr->config.phyInstRef, PHY_ANT_BEAM_8);
Phy_SetBeamNo(l2InstPtr->config.phyInstRef, l2InstPtr->scanFsm.beamNo);
Phy_SetMode(l2InstPtr->config.phyInstRef, PHY_MODE_SCHEDULE);
L2_SetupNextSlot(l2InstPtr, L2_TOR_TX, L2_SCAN_FRAME);
Phy_SetBeamWidth(instPtr->config.phyInstRef, PHY_ANT_BEAM_8);
Phy_SetBeamNo(instPtr->config.phyInstRef, instPtr->scanFsm.beamNo);
Phy_SetMode(instPtr->config.phyInstRef, PHY_MODE_SCHEDULE);
L2_SetupNextSlot(instPtr, L2_TOR_TX, L2_SCAN_FRAME);
}
else {
// Antenna configure
Phy_SetBeamWidth(l2InstPtr->config.phyInstRef, PHY_ANT_BEAM_8);
Phy_SetBeamNo(l2InstPtr->config.phyInstRef, l2InstPtr->scanFsm.beamNo);
Phy_SetMode(l2InstPtr->config.phyInstRef, PHY_MODE_SCHEDULE);
L2_SetupNextSlot(l2InstPtr, L2_TOR_RX, L2_INFO_FRAME);
Phy_SetBeamWidth(instPtr->config.phyInstRef, PHY_ANT_BEAM_8);
Phy_SetBeamNo(instPtr->config.phyInstRef, instPtr->scanFsm.beamNo);
Phy_SetMode(instPtr->config.phyInstRef, PHY_MODE_SCHEDULE);
L2_SetupNextSlot(instPtr, L2_TOR_RX, L2_INFO_FRAME);
// 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.
@@ -278,26 +278,26 @@ int32_t L2_Scan_Proc(L2_INST *l2InstPtr) {
else {
uint8_t *sendData;
// Pack node information.
Status = ScanMsg_Packing(l2InstPtr, L2_INFO_FRAME);
Status = ScanMsg_Packing(instPtr, L2_INFO_FRAME);
if (Status != SUCCESS) {
DBG_INFO("Failed to pack response.\n");
}
if (CircQueuePop(l2InstPtr->txQueue, (void **)(&sendData)) == true) {
memcpy(l2InstPtr->sendPktData, sendData, RSP_PACK_LEN);
if (CircQueuePop(instPtr->txQueue, (void **)(&sendData)) == true) {
memcpy(instPtr->sendPktData, sendData, RSP_PACK_LEN);
free(sendData);
}
else {
DBG_INFO("Failed to pop tx Queue.\n");
}
// Antenna configure
Phy_SetBeamAngle(l2InstPtr->config.phyInstRef, 0.0, 0.0);
Phy_SetBeamWidth(l2InstPtr->config.phyInstRef, PHY_ANT_BEAM_120);
Phy_SetMode(l2InstPtr->config.phyInstRef, PHY_MODE_LISTEN);
L2_SetupNextSlot(l2InstPtr, L2_TOR_RX, L2_SCAN_FRAME);
Phy_SetBeamAngle(instPtr->config.phyInstRef, 0.0, 0.0);
Phy_SetBeamWidth(instPtr->config.phyInstRef, PHY_ANT_BEAM_120);
Phy_SetMode(instPtr->config.phyInstRef, PHY_MODE_LISTEN);
L2_SetupNextSlot(instPtr, L2_TOR_RX, L2_SCAN_FRAME);
}
// Increase scan counter (for debug purpose).
l2InstPtr->scanFsm.sacanCnt ++;
instPtr->scanFsm.sacanCnt ++;
return NUM_SLOTS_SKIP;
}
@@ -311,7 +311,7 @@ int32_t L2_Scan_Proc(L2_INST *l2InstPtr) {
* @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");
ASSERT(false); // Not support.
return 0;
df_datalink/src/networking/link_layer/service.c
+99 -99
View File
@@ -34,7 +34,7 @@
/**************************** Type Definitions *******************************/
/************************** 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 *****************************/
#ifdef DATALINK_ENABLE_BERTEST
@@ -58,9 +58,9 @@ static uint8_t berTestData[SVC_PACK_LEN] = {
* 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
ASSERT(l2InstPtr != NULL);
ASSERT(instPtr != NULL);
int32_t Status;
uint8_t *buff;
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");
return ENOMEM;
}
buff[L2_MSG_FRAME_NO] = L2_GetNextFrameNo(l2InstPtr);
buff[L2_MSG_SLOT_NO] = L2_GetNextSlotNo(l2InstPtr);
buff[L2_MSG_FRAME_NO] = L2_GetNextFrameNo(instPtr);
buff[L2_MSG_SLOT_NO] = L2_GetNextSlotNo(instPtr);
// 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) {
// check if deviation info outdated.
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);
// Pack Seq ID
buff[L2_MSG_SVC_SEQ_ID] = l2InstPtr->serviceFsm.txSeqNum;
buff[L2_MSG_SVC_SEQ_ID] = instPtr->serviceFsm.txSeqNum;
// Pack 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);
if (l2InstPtr->config.myType == L2_HEAD_NODE) {
if (instPtr->config.myType == L2_HEAD_NODE) {
// 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)
? 0 : l2InstPtr->serviceFsm.txSeqNum + 1;
? 0 : instPtr->serviceFsm.txSeqNum + 1;
}
else{
// 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)
? 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;
}
if(CircQueuePush(l2InstPtr->txQueue, buff) == false){
if(CircQueuePush(instPtr->txQueue, buff) == false){
DBG_ERROR("Cannot push packet to queue..\n");
return EQUEUE;
}
@@ -137,27 +137,27 @@ static int32_t Service_Packing(L2_INST *l2InstPtr, L2_FRAME_TYPE type) {
}
case L2_INFO_FRAME:{
packLen = RSP_PACK_LEN;
SysMonitor_GetSysinfo(&(l2InstPtr->sysInfo));
SysMonitor_GetSysinfo(&(instPtr->sysInfo));
buff = malloc(packLen);
if (buff == NULL) {
DBG_ERROR("Not enough space, failed to pack frame...\n");
return ENOMEM;
}
buff[L2_MSG_FRAME_NO] = L2_GetNextFrameNo(l2InstPtr);
buff[L2_MSG_SLOT_NO] = L2_GetNextSlotNo(l2InstPtr);
memcpy(buff+L2_MSG_INFO_POSX, &(l2InstPtr->sysInfo.posX), sizeof(float));
memcpy(buff+L2_MSG_INFO_POSY, &(l2InstPtr->sysInfo.posY), sizeof(float));
memcpy(buff+L2_MSG_INFO_POSZ, &(l2InstPtr->sysInfo.posZ), sizeof(float));
memcpy(buff+L2_MSG_INFO_SPEEDX, &(l2InstPtr->sysInfo.vX), sizeof(float));
memcpy(buff+L2_MSG_INFO_SPEEDY, &(l2InstPtr->sysInfo.vY), sizeof(float));
memcpy(buff+L2_MSG_INFO_SPEEDZ, &(l2InstPtr->sysInfo.vZ), sizeof(float));
buff[L2_MSG_FRAME_NO] = L2_GetNextFrameNo(instPtr);
buff[L2_MSG_SLOT_NO] = L2_GetNextSlotNo(instPtr);
memcpy(buff+L2_MSG_INFO_POSX, &(instPtr->sysInfo.posX), sizeof(float));
memcpy(buff+L2_MSG_INFO_POSY, &(instPtr->sysInfo.posY), sizeof(float));
memcpy(buff+L2_MSG_INFO_POSZ, &(instPtr->sysInfo.posZ), sizeof(float));
memcpy(buff+L2_MSG_INFO_SPEEDX, &(instPtr->sysInfo.vX), sizeof(float));
memcpy(buff+L2_MSG_INFO_SPEEDY, &(instPtr->sysInfo.vY), 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);
if (Status != SUCCESS) {
DBG_ERROR("Fail to append CRC...\n");
return Status;
}
if(CircQueuePush(l2InstPtr->txQueue, buff) == false){
if(CircQueuePush(instPtr->txQueue, buff) == false){
DBG_ERROR("Cannot push packet to queue..\n");
return EQUEUE;
}
@@ -181,22 +181,22 @@ static int32_t Service_Packing(L2_INST *l2InstPtr, L2_FRAME_TYPE type) {
/**
* @brief Initialize scan FSM.
*
* @param l2InstPtr
* @param instPtr
*
* @return
*
******************************************************************************/
int32_t L2_Service_Init(L2_INST *l2InstPtr) {
ASSERT(l2InstPtr != NULL);
l2InstPtr->serviceFsm.txSeqNum = 0;
l2InstPtr->serviceFsm.rxSeqNum = 0;
memset((&l2InstPtr->serviceFsm.rxLog), 0x00, sizeof(PAYLOAD));
memset((&l2InstPtr->serviceFsm.txLog), 0x00, sizeof(PAYLOAD));
l2InstPtr->serviceFsm.pktRecv = 0;
l2InstPtr->serviceFsm.pktRecved = 0;
int32_t L2_Service_Init(L2_INST *instPtr) {
ASSERT(instPtr != NULL);
instPtr->serviceFsm.txSeqNum = 0;
instPtr->serviceFsm.rxSeqNum = 0;
memset((&instPtr->serviceFsm.rxLog), 0x00, sizeof(PAYLOAD));
memset((&instPtr->serviceFsm.txLog), 0x00, sizeof(PAYLOAD));
instPtr->serviceFsm.pktRecv = 0;
instPtr->serviceFsm.pktRecved = 0;
#ifdef DATALINK_ENABLE_BERTEST
l2InstPtr->serviceFsm.bitTotal = 0;
l2InstPtr->serviceFsm.bitError = 0;
instPtr->serviceFsm.bitTotal = 0;
instPtr->serviceFsm.bitError = 0;
#endif
return SUCCESS;
}
@@ -211,13 +211,13 @@ int32_t L2_Service_Init(L2_INST *l2InstPtr) {
* other error occurred.
*
******************************************************************************/
int32_t L2_Service_PktHandler(L2_INST *l2InstPtr, uint8_t *msg) {
ASSERT(l2InstPtr != NULL);
int32_t L2_Service_PktHandler(L2_INST *instPtr, uint8_t *msg) {
ASSERT(instPtr != NULL);
ASSERT(msg != NULL);
int32_t Status;
switch (l2InstPtr->frame.last) {
switch (instPtr->frame.last) {
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");
Status = EUNEXP;
break;
@@ -231,7 +231,7 @@ int32_t L2_Service_PktHandler(L2_INST *l2InstPtr, uint8_t *msg) {
#ifdef DATALINK_ENABLE_AUTOTIMESYNC
l2Event.msg = NULL;
AssertL2Event(l2InstPtr, &l2Event);
AssertL2Event(instPtr, &l2Event);
#else
int64_t timeAdj;
l2Event.msg = &timeAdj;
@@ -241,57 +241,57 @@ int32_t L2_Service_PktHandler(L2_INST *l2InstPtr, uint8_t *msg) {
timeAdj |= (timeAdj & 0x0000000000800000) ? 0xFFFFFFFFFF000000 : 0x0000000000000000;
// FIXME: better approach?
// 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) {
AssertL2Event(l2InstPtr, &l2Event);
if (seqNum % NUM_SERVICE_PRD_SLOTS == 0 || instPtr->serviceFsm.seqNum != seqNum) {
AssertL2Event(instPtr, &l2Event);
}
#endif
#ifndef DATALINK_ENABLE_BERTEST
memcpy(l2InstPtr->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) {
memcpy(instPtr->serviceFsm.rxLog + L2_MSG_SVC_PAYLOAD_LEN*seqNum, msg + L2_MSG_SVC_PAYLOAD, L2_MSG_SVC_PAYLOAD_LEN);
if (instPtr->config.myType == L2_HEAD_NODE) {
// if last packet reset counting
if(seqNum >= (PAYLOAD_FLJ-1)/L2_MSG_SVC_PAYLOAD+1) {
if(seqNum == l2InstPtr->serviceFsm.rxSeqNum) {
SysMonitor_UpdateRecvPayloadLJRaw(l2InstPtr->serviceFsm.rxLog);
if(seqNum == instPtr->serviceFsm.rxSeqNum) {
SysMonitor_UpdateRecvPayloadLJRaw(instPtr->serviceFsm.rxLog);
}
l2InstPtr->serviceFsm.rxSeqNum = 0;
instPtr->serviceFsm.rxSeqNum = 0;
}
else {
if(l2InstPtr->serviceFsm.rxSeqNum == seqNum){
l2InstPtr->serviceFsm.rxSeqNum++;
if(instPtr->serviceFsm.rxSeqNum == seqNum){
instPtr->serviceFsm.rxSeqNum++;
}
}
}
else{
// if last packet reset counting
if(seqNum >= (PAYLOAD_LJ-1)/L2_MSG_SVC_PAYLOAD+1) {
if(seqNum == l2InstPtr->serviceFsm.rxSeqNum) {
SysMonitor_UpdateRecvPayloadFLJRaw(l2InstPtr->serviceFsm.rxLog);
if(seqNum == instPtr->serviceFsm.rxSeqNum) {
SysMonitor_UpdateRecvPayloadFLJRaw(instPtr->serviceFsm.rxLog);
}
l2InstPtr->serviceFsm.rxSeqNum = 0;
instPtr->serviceFsm.rxSeqNum = 0;
}
else {
if(l2InstPtr->serviceFsm.rxSeqNum == seqNum){
l2InstPtr->serviceFsm.rxSeqNum++;
if(instPtr->serviceFsm.rxSeqNum == seqNum){
instPtr->serviceFsm.rxSeqNum++;
}
}
}
#else
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
l2InstPtr->serviceFsm.pktRecved++;
instPtr->serviceFsm.pktRecved++;
// Reset Node TTL
Status = RstNodeTTL(l2InstPtr->nodeInfoList, L2_HEAD_NODE_ID);
Status = RstNodeTTL(instPtr->nodeInfoList, L2_HEAD_NODE_ID);
if (Status != SUCCESS) {
DBG_ERROR("Failed to reset node %d TTL, errorCode: %ld.\n", L2_MEMBER_NODE_ID, Status);
}
break;
}
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");
Status = EUNEXP;
break;
@@ -305,7 +305,7 @@ int32_t L2_Service_PktHandler(L2_INST *l2InstPtr, uint8_t *msg) {
coordTemp.y = tmpVal;
memcpy(&tmpVal, msg+L2_MSG_INFO_POSZ, sizeof(float));
coordTemp.z = tmpVal;
Status = UpdateNodePos(l2InstPtr->nodeInfoList, L2_MEMBER_NODE_ID, &coordTemp);
Status = UpdateNodePos(instPtr->nodeInfoList, L2_MEMBER_NODE_ID, &coordTemp);
if (Status != SUCCESS) {
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;
memcpy(&tmpVal, msg+L2_MSG_INFO_SPEEDZ, sizeof(float));
coordTemp.z = tmpVal;
Status = UpdateNodeSpeed(l2InstPtr->nodeInfoList, L2_MEMBER_NODE_ID, &coordTemp);
Status = UpdateNodeSpeed(instPtr->nodeInfoList, L2_MEMBER_NODE_ID, &coordTemp);
if (Status != SUCCESS) {
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
// Update Time deviation
int64_t devi, deviTrunc;
Status = L2_TimeSync_GetDeviatoion(l2InstPtr, &devi);
Status = L2_TimeSync_GetDeviatoion(instPtr, &devi);
if (Status != SUCCESS) {
DBG_ERROR("Failed to compute slot deviation.\n");
deviTrunc = 0;
@@ -340,28 +340,28 @@ int32_t L2_Service_PktHandler(L2_INST *l2InstPtr, uint8_t *msg) {
else {
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) {
DBG_ERROR("Failed to update node %d time deviation, errorCode: %ld.\n", L2_MEMBER_NODE_ID, Status);
}
#else
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
l2InstPtr->serviceFsm.pktRecved++;
instPtr->serviceFsm.pktRecved++;
// Reset Node TTL
Status = RstNodeTTL(l2InstPtr->nodeInfoList, L2_MEMBER_NODE_ID);
Status = RstNodeTTL(instPtr->nodeInfoList, L2_MEMBER_NODE_ID);
if (Status != SUCCESS) {
DBG_ERROR("Failed to reset node %d ttl, errorCode: %ld.\n", L2_MEMBER_NODE_ID, Status);
}
break;
}
default:
DBG_ERROR("Unsupported frame type: %ld.\n", l2InstPtr->frame.last);
DBG_ERROR("Unsupported frame type: %ld.\n", instPtr->frame.last);
break;
}
return Status;
@@ -371,24 +371,24 @@ int32_t L2_Service_PktHandler(L2_INST *l2InstPtr, uint8_t *msg) {
/**
* @brief Service FSM Routine.
*
* @param l2InstPtr
* @param instPtr
* @param schedEntry
*
* @return Slot has been used.
*
******************************************************************************/
int32_t L2_Service_Routine(L2_INST *l2InstPtr, L2_RTNE_TABLE_ENTRY *schedEntry) {
ASSERT(l2InstPtr != NULL);
int32_t L2_Service_Routine(L2_INST *instPtr, L2_RTNE_TABLE_ENTRY *schedEntry) {
ASSERT(instPtr != NULL);
DBG_INFO("-------- ServiceNet Routine --------\n");
uint8_t *sendData;
int32_t usedSlots;
L2_TOR_TYPE torState;
if(schedEntry->src == l2InstPtr->config.myId){
if(schedEntry->src == instPtr->config.myId){
torState = L2_TOR_TX;
}
else if(schedEntry->dst == l2InstPtr->config.myId){
else if(schedEntry->dst == instPtr->config.myId){
torState = L2_TOR_RX;
l2InstPtr->serviceFsm.pktRecv++;
instPtr->serviceFsm.pktRecv++;
}
else{
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:
if(torState == L2_TOR_TX){
#ifndef DATALINK_ENABLE_BERTEST
if (l2InstPtr->serviceFsm.txSeqNum == 0) {
if(l2InstPtr->config.myType == L2_HEAD_NODE){
SysMonitor_GetSendPayloadFLJRaw(l2InstPtr->serviceFsm.txLog);
if (instPtr->serviceFsm.txSeqNum == 0) {
if(instPtr->config.myType == L2_HEAD_NODE){
SysMonitor_GetSendPayloadFLJRaw(instPtr->serviceFsm.txLog);
}
else {
SysMonitor_GetSendPayloadLJRaw(l2InstPtr->serviceFsm.txLog);
SysMonitor_GetSendPayloadLJRaw(instPtr->serviceFsm.txLog);
}
}
// Packing service data
Service_Packing(l2InstPtr, L2_SVC_FRAME);
if (CircQueuePop(l2InstPtr->txQueue, (void **)(&sendData)) == true) {
memcpy(l2InstPtr->sendPktData, sendData, SVC_PACK_LEN);
Service_Packing(instPtr, L2_SVC_FRAME);
if (CircQueuePop(instPtr->txQueue, (void **)(&sendData)) == true) {
memcpy(instPtr->sendPktData, sendData, SVC_PACK_LEN);
free(sendData);
}
else {
DBG_ERROR("Failed to pop tx queue.\n");
}
#else
memcpy(l2InstPtr->config.phyInstRef->sendFrameBuff, berTestData, SVC_PACK_LEN);
memcpy(instPtr->config.phyInstRef->sendFrameBuff, berTestData, SVC_PACK_LEN);
#endif
}
// FIXME: Steer antenna to point to destination.
Phy_SetBeamAngle(l2InstPtr->config.phyInstRef, 0.0, 0.0);
Phy_SetBeamWidth(l2InstPtr->config.phyInstRef, PHY_ANT_BEAM_8);
Phy_SetMode(l2InstPtr->config.phyInstRef, PHY_MODE_SCHEDULE);
L2_SetupNextSlot(l2InstPtr, torState, L2_SVC_FRAME);
Phy_SetBeamAngle(instPtr->config.phyInstRef, 0.0, 0.0);
Phy_SetBeamWidth(instPtr->config.phyInstRef, PHY_ANT_BEAM_8);
Phy_SetMode(instPtr->config.phyInstRef, PHY_MODE_SCHEDULE);
L2_SetupNextSlot(instPtr, torState, L2_SVC_FRAME);
usedSlots = NUM_SERVICE_SLOTS;
break;
#ifndef DATALINK_ENABLE_BERTEST
case L2_SLOT_ADVERTISE:
if(torState == L2_TOR_TX){
// Packing response data.
Service_Packing(l2InstPtr, L2_INFO_FRAME);
if (CircQueuePop(l2InstPtr->txQueue, (void **)(&sendData)) == true) {
memcpy(l2InstPtr->sendPktData, sendData, RSP_PACK_LEN);
Service_Packing(instPtr, L2_INFO_FRAME);
if (CircQueuePop(instPtr->txQueue, (void **)(&sendData)) == true) {
memcpy(instPtr->sendPktData, sendData, RSP_PACK_LEN);
free(sendData);
}
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.
Phy_SetBeamAngle(l2InstPtr->config.phyInstRef, 0.0, 0.0);
Phy_SetBeamWidth(l2InstPtr->config.phyInstRef, PHY_ANT_BEAM_120);
Phy_SetMode(l2InstPtr->config.phyInstRef, PHY_MODE_SCHEDULE);
L2_SetupNextSlot(l2InstPtr, torState, L2_INFO_FRAME);
Phy_SetBeamAngle(instPtr->config.phyInstRef, 0.0, 0.0);
Phy_SetBeamWidth(instPtr->config.phyInstRef, PHY_ANT_BEAM_120);
Phy_SetMode(instPtr->config.phyInstRef, PHY_MODE_SCHEDULE);
L2_SetupNextSlot(instPtr, torState, L2_INFO_FRAME);
usedSlots = NUM_RESPONSE_SLOTS;
break;
#endif
@@ -454,11 +454,11 @@ int32_t L2_Service_Routine(L2_INST *l2InstPtr, L2_RTNE_TABLE_ENTRY *schedEntry)
#ifdef DATALINK_ENABLE_BERTEST
uint32_t pktLoss;
double errorRate, lossRate;
pktLoss = l2InstPtr->serviceFsm.pktRecv - l2InstPtr->serviceFsm.pktRecved;
lossRate = (l2InstPtr->serviceFsm.pktRecv == 0) ? 0 : (double)pktLoss / (double)l2InstPtr->serviceFsm.pktRecv;
errorRate = (l2InstPtr->serviceFsm.bitTotal == 0) ? 0 : (double)l2InstPtr->serviceFsm.bitError / (double)l2InstPtr->serviceFsm.bitTotal;
DBG_PRINT("Packet Loss Rate -- Total: %lu, Loss: %lu.\n", l2InstPtr->serviceFsm.pktRecv, pktLoss);
DBG_PRINT("Bit Error Rate -- Total:%lu, Error:%lu.\n", l2InstPtr->serviceFsm.bitTotal, l2InstPtr->serviceFsm.bitError);
pktLoss = instPtr->serviceFsm.pktRecv - instPtr->serviceFsm.pktRecved;
lossRate = (instPtr->serviceFsm.pktRecv == 0) ? 0 : (double)pktLoss / (double)instPtr->serviceFsm.pktRecv;
errorRate = (instPtr->serviceFsm.bitTotal == 0) ? 0 : (double)instPtr->serviceFsm.bitError / (double)instPtr->serviceFsm.bitTotal;
DBG_PRINT("Packet Loss Rate -- Total: %lu, Loss: %lu.\n", instPtr->serviceFsm.pktRecv, pktLoss);
DBG_PRINT("Bit Error Rate -- Total:%lu, Error:%lu.\n", instPtr->serviceFsm.bitTotal, instPtr->serviceFsm.bitError);
#endif
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.
*
* @param l2InstPtr.
* @param instPtr.
* @param adjustment
*
* @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
Phy_SetTimeWord(l2InstPtr->config.phyInstRef, adjustment, PHY_TIME_WORD_LOCAL_ADJ);
PhyWrite(l2InstPtr->config.phyInstRef, TM_ADJ_MODE, TM_ADJ_MODE_ADD);
Phy_SetTimeWord(instPtr->config.phyInstRef, adjustment, PHY_TIME_WORD_LOCAL_ADJ);
PhyWrite(instPtr->config.phyInstRef, TM_ADJ_MODE, TM_ADJ_MODE_ADD);
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.
*
* @param l2InstPtr.
* @param instPtr.
*
* @return
*
******************************************************************************/
int32_t L2_TimeSync_SlotAlign(L2_INST *l2InstPtr) {
int32_t L2_TimeSync_SlotAlign(L2_INST *instPtr) {
int32_t Status;
int64_t slotAdj;
Status = L2_TimeSync_GetDeviatoion(l2InstPtr, &slotAdj);
Status = L2_TimeSync_GetDeviatoion(instPtr, &slotAdj);
if (Status != SUCCESS) {
DBG_ERROR("Failed to compute slot deviation.\n");
return Status;
}
return L2_TimeSync_LocalTimerAdd(l2InstPtr, slotAdj);
return L2_TimeSync_LocalTimerAdd(instPtr, slotAdj);
}
/*****************************************************************************/
/**
* @brief Compute slot deviation of received frame.
*
* @param l2InstPtr.
* @param instPtr.
* @param devi
*
* @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
int64_t currentTarget, lastFrameSync, slotMeasure;
currentTarget = PhyRead(l2InstPtr->config.phyInstRef, TM_TARGET_L);
currentTarget |= ((int64_t)PhyRead(l2InstPtr->config.phyInstRef, TM_TARGET_H)) << 32;
lastFrameSync = PhyRead(l2InstPtr->config.phyInstRef, FSTIME_L);
lastFrameSync |= ((int64_t)PhyRead(l2InstPtr->config.phyInstRef, FSTIME_H)) << 32;
currentTarget = PhyRead(instPtr->config.phyInstRef, TM_TARGET_L);
currentTarget |= ((int64_t)PhyRead(instPtr->config.phyInstRef, TM_TARGET_H)) << 32;
lastFrameSync = PhyRead(instPtr->config.phyInstRef, FSTIME_L);
lastFrameSync |= ((int64_t)PhyRead(instPtr->config.phyInstRef, FSTIME_H)) << 32;
slotMeasure = (currentTarget > lastFrameSync) ? (currentTarget - lastFrameSync)
: (currentTarget - lastFrameSync + PHY_LOCAL_CNT_MAX);
switch (Phy_GetLastDetectedFrameType(l2InstPtr->config.phyInstRef)) {
switch (Phy_GetLastDetectedFrameType(instPtr->config.phyInstRef)) {
case L1_2KBPS_20MS_FRAME :
*devi = ((int64_t)PHY_SYNCT2FEND_2K_20MS) - slotMeasure;
break;
@@ -114,7 +114,7 @@ int32_t L2_TimeSync_GetDeviatoion(L2_INST *l2InstPtr, int64_t *devi) {
*devi = ((int64_t)PHY_SYNCT2FEND_2K_100MS) - slotMeasure;
break;
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 SUCCESS;