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

rewrite rtt fsm, simplified logic and enhanced code coherence.

Browse Source
This commit is contained in:
Gatsby Van
2023-12-20 19:57:10 +08:00
parent f1a34a16bc
commit fcb9876675
2 changed files with 257 additions and 317 deletions
Download Patch File Download Diff File Expand all files Collapse all files
datalink814_ge/src/linklayer/rtt.h
+16 -47
View File
@@ -22,51 +22,30 @@
#include <stdint.h> #include <stdint.h>
#include "linklayer814.h" #include "linklayer814.h"
#include "linklayer814_types.h" #include "linklayer814_types.h"
#include "../time_ctrl/time_ctrl.h"
/************************** Constant Definitions *****************************/ /************************** Constant Definitions *****************************/
#define MAX_FREQ_EST_NUM 128 // 32bit word
#define SVC_FREQ_EST_NUM 512 // byte
#define MGMT_FREQ_EST_NUM 512 // byte
#define BISAR_FREQ_EST_NUM 480 // byte
#define CXR_FREQ 3800000000.0 // Hz
#define LIGHT_SPPED_AIR 299702547.0 // m/s
#define LIGHT_SPPED_VAC 299792458.0 // m/s
#define RCV2FSYNC_CLOCKS 0 // clocks from frame sync time to receive time
#define PHYRSP_DELAY_CLOCKS 0// clocks from frame sync time to receive time
//#define PHY_SYNCT2FEND_200K 904052L
//#define PHY_SYNCT2FEND_200K 924232L
#define PHY_SYNCT2FEND_200K 905052L
#define RTT_TIME_NONE UINT32_MAX
#define RTT_DOPPLER_NONE UINT32_MAX
/***************** Macros (Inline Functions) Definitions *********************/ /***************** Macros (Inline Functions) Definitions *********************/
/**************************** Type Definitions *******************************/ /**************************** Type Definitions *******************************/
// TODO: Multi-node RTT // TODO: Multi-node RTT
typedef struct { typedef struct {
L2_TIME_WORD Tt1; TIME_WORD Tt1;
L2_TIME_WORD Tr1; TIME_WORD Tr1;
L2_TIME_WORD Tt2; TIME_WORD Tt2;
L2_TIME_WORD Tr2; TIME_WORD Tr2;
L2_TIME_WORD Tt3; TIME_WORD Tt3;
L2_TIME_WORD Tr3; TIME_WORD Tr3;
L2_TIME_WORD Tt4; TIME_WORD Tt4;
L2_TIME_WORD Tr4; TIME_WORD Tr4;
int32_t dopplerR1; int32_t dopplerR1;
int32_t dopplerR2; int32_t dopplerR2;
int32_t dopplerR3; int32_t dopplerR3;
int32_t dopplerR4; int32_t dopplerR4;
L2_TIME_WORD TroundA; TIME_WORD TroundA;
L2_TIME_WORD TreplyA; TIME_WORD TreplyA;
L2_TIME_WORD TroundB; TIME_WORD TroundB;
L2_TIME_WORD TreplyB; TIME_WORD TreplyB;
int32_t rttAdj;
} RTT_RECORD; } RTT_RECORD;
typedef enum { typedef enum {
@@ -78,25 +57,15 @@ typedef enum {
} L2_RTT_SATGE; } L2_RTT_SATGE;
typedef struct { typedef struct {
L2_RCD rttStatge; L2_RTT_SATGE rttStatge;
bool isR1Valid; bool isR1Valid;
bool isR2Valid; bool isR2Valid;
bool isR3Valid; bool isR3Valid;
bool isR4Valid; bool isR4Valid;
RTT_RECORD rttRecord; RTT_RECORD rttRecord;
TIME_WORD timeStamp;
uint32_t doppler;
} L2_RTT_FSM; } L2_RTT_FSM;
typedef enum {
RTT_NONE = 0,
RTT_SEND = 1,
RTT_RCV = 2,
} L2_RTT_TOR;
/************************** Function Prototypes ******************************/ /************************** Function Prototypes ******************************/
// RTT
L2_RTT_SATGE RTT_GetLastStage(L2_RTT_FSM* rttFsmPtr);
L2_RTT_SATGE RTT_GetCurrentStage(L2_RTT_FSM* rttFsmPtr);
L2_RTT_SATGE RTT_GetNextStage(L2_RTT_FSM* rttFsmPtr);
#endif /* RTT_H_ */ #endif /* RTT_H_ */
datalink814_ge/src/linklayer/time_sync.c
+241 -270
View File
@@ -26,14 +26,22 @@
#include "../common.h" #include "../common.h"
/************************** Constant Definitions *****************************/ /************************** Constant Definitions *****************************/
#define LIGHT_SPPED_AIR 299702547.0 // m/s
#define LIGHT_SPPED_VAC 299792458.0 // m/s
#define RTT_DOPPLER_NONE UINT32_MAX
const TIME_WORD RTT_TIME_NONE = {
.int_part = UINT32_MAX,
.frac_part = UINT32_MAX
};
/***************** Macros (Inline Functions) Definitions *********************/ /***************** Macros (Inline Functions) Definitions *********************/
/**************************** Type Definitions *******************************/ /**************************** Type Definitions *******************************/
/************************** Variable Definitions *****************************/ /************************** Variable Definitions *****************************/
/************************** Function Prototypes ******************************/ /************************** Function Prototypes ******************************/
static void RTT_StageUpdate(L2_RTT_FSM* rttFsmPtr, L2_RTT_SATGE newSatge); static void RTT_StageUpdate(L2_RTT_FSM* rttFsmPtr, L2_RTT_SATGE newSatge);
static void RTT_ResetRecord(RTT_RECORD* recordPtr); static void RTT_GetFrameStart(L2_INST* instPtr, TIME_WORD *tmStamp, L2_TOR_TYPE torType);
static int32_t RTT_GetCounter10nsDeviation(PHY_INST *phyRef); void RTT_CalculateTimeGap(const TIME_WORD *tmA, const TIME_WORD *tmB, TIME_WORD *tRound);
/*****************************************************************************/ /*****************************************************************************/
@@ -70,48 +78,18 @@ int32_t L2_TimeAlign(L2_INST* instPtr, uint32_t frameNum, uint32_t slotNum){
timeDiff = (int64_t)(expctTime - currTime); timeDiff = (int64_t)(expctTime - currTime);
slotTimerAdjmet.int_part = (uint32_t)(timeDiff/COUNTE_FRAC_MAX); slotTimerAdjmet.int_part = (uint32_t)(timeDiff / COUNTE_FRAC_MAX);
slotTimerAdjmet.frac_part = (uint32_t)(timeDiff%COUNTE_FRAC_MAX); slotTimerAdjmet.frac_part = (uint32_t)(timeDiff % COUNTE_FRAC_MAX);
TimeCtrl_TimeAdjust(instPtr->config.tmcInstPtr, &slotTimerAdjmet, TM_ADJ_ADD); TimeCtrl_TimeAdjust(instPtr->config.tmcInstPtr, &slotTimerAdjmet, TM_ADJ_ADD);
return SUCCESS; return SUCCESS;
} }
/**
* @brief Get last RTT stage of a specified RTT descriptor.
*/
inline L2_RTT_SATGE RTT_GetLastStage(L2_RTT_FSM* rttFsmPtr){
ASSERT(rttFsmPtr != NULL);
return rttFsmPtr->rttStatge.last;
}
/**
* @brief Get current RTT stage of a specified RTT descriptor.
*/
inline L2_RTT_SATGE RTT_GetCurrentStage(L2_RTT_FSM* rttFsmPtr){
ASSERT(rttFsmPtr != NULL);
return rttFsmPtr->rttStatge.curr;
}
/**
* @brief Get next RTT stage of a specified RTT descriptor.
*/
inline L2_RTT_SATGE RTT_GetNextStage(L2_RTT_FSM* rttFsmPtr){
ASSERT(rttFsmPtr != NULL);
return rttFsmPtr->rttStatge.next;
}
/**
* @brief Get next RTT stage of a specified RTT descriptor.
*/
static inline void RTT_ResetRecord(RTT_RECORD* recordPtr){
ASSERT(recordPtr != NULL);
memset(recordPtr, 0x00, sizeof(RTT_RECORD));
return;
}
/*****************************************************************************/ /*****************************************************************************/
/** /**
* @brief Update corrent time-sync for RTT stage * @brief Update corrent time-sync for RTT stage
* *
* @param scanFsmPtr A pointer to scan net fsm. * @param scanFsmPtr A pointer to scan net fsm.
* newSatge New state. * @param newSatge New state.
* *
* @return NONE. * @return NONE.
* *
@@ -119,49 +97,114 @@ static inline void RTT_ResetRecord(RTT_RECORD* recordPtr){
static void RTT_StageUpdate(L2_RTT_FSM* rttFsmPtr, L2_RTT_SATGE newSatge) { static void RTT_StageUpdate(L2_RTT_FSM* rttFsmPtr, L2_RTT_SATGE newSatge) {
ASSERT(rttFsmPtr != NULL); ASSERT(rttFsmPtr != NULL);
ASSERT(newSatge >= RTT_NONE_STAGE && newSatge <= RTT_RSP2); ASSERT(newSatge >= RTT_NONE_STAGE && newSatge <= RTT_RSP2);
DBG_INFO("RTT current Stage: %u -> RTT new Stage: %u, Stage: arranged.%d.\n", DBG_INFO("RTT Stage Update: %d -> %d.\n", rttFsmPtr->rttStatge, newSatge);
rttFsmPtr->rttStatge.curr, rttFsmPtr->rttStatge.next, newSatge);
L2_RcdShift(&(rttFsmPtr->rttStatge), newSatge);
return;
} }
/*****************************************************************************/ /*****************************************************************************/
/** /**
* @brief Reset RTT systerm . * @brief Reset RTT FSM
* *
* @param rttFsmPtr * @param rttFsmPtr a pointer to rtt FSM instance.
* *
* @return SUCCESS Initialization successful. * @return NONE
* EMAC_MEMORY_ALLOC_ERROR Memory allocate error.
* *
******************************************************************************/ ******************************************************************************/
void RTT_Reset(L2_RTT_FSM* rttFsmPtr){ void RTT_Reset(L2_RTT_FSM* rttFsmPtr){
ASSERT(rttFsmPtr != NULL); ASSERT(rttFsmPtr != NULL);
RTT_ResetRecord(&(rttFsmPtr->rttRecord)); memset(&(rttFsmPtr->rttRecord), 0x00, sizeof(RTT_RECORD));
rttFsmPtr->rttStatge.last = RTT_NONE_STAGE; rttFsmPtr->rttStatge = RTT_NONE_STAGE;
rttFsmPtr->rttStatge.curr = RTT_NONE_STAGE;
rttFsmPtr->rttStatge.next = RTT_NONE_STAGE;
rttFsmPtr->isR1Valid = false; rttFsmPtr->isR1Valid = false;
rttFsmPtr->isR2Valid = false; rttFsmPtr->isR2Valid = false;
rttFsmPtr->isR3Valid = false; rttFsmPtr->isR3Valid = false;
rttFsmPtr->isR4Valid = false; rttFsmPtr->isR4Valid = false;
rttFsmPtr->timeStamp.int_part = 0;
rttFsmPtr->timeStamp.frac_part = 0;
rttFsmPtr->doppler = 0;
} }
/*****************************************************************************/
/**
* @brief Destroy RTT FSM, free allocated memory.
*
* @param rttFsmPtr a pointer to rtt FSM instance.
*
* @return NONE
*
******************************************************************************/
void RTT_Destroy(L2_RTT_FSM* rttFsmPtr){ void RTT_Destroy(L2_RTT_FSM* rttFsmPtr){
free(rttFsmPtr); free(rttFsmPtr);
} }
static int32_t RTT_GetCounter10nsDeviation(PHY_INST *phyRef) { /*****************************************************************************/
TIME_WORD timeStamp; /**
int64_t lastFrameSyncT, lastSlotStartT; * @brief Get the frame start time. If torType == L2_TOR_TX, get the next
Phy_GetFrameSyncTimeStamp(phyRef, &timeStamp); * slot start time, if torType == L2_TOR_RX get last frame start
lastFrameSyncT = (uint64_t) timeStamp.frac_part; * time.
lastFrameSyncT += ((uint64_t)timeStamp.int_part) * COUNTE_FRAC_MAX; *
Phy_GetLastSlotStartTimeStamp(phyRef, &timeStamp); * @param instPtr a pointer to linklayer layer instance
lastSlotStartT = (uint64_t) timeStamp.frac_part; * @param tmStamp a pointer to store frame start time
lastSlotStartT += ((uint64_t)timeStamp.int_part) * COUNTE_FRAC_MAX; * @param torType tx/rx mode
return (PHY_SYNCT2FEND_200K - (lastSlotStartT - lastFrameSyncT)); *
* @return NONE
*
******************************************************************************/
void RTT_GetFrameStart(L2_INST* instPtr, TIME_WORD *tmStamp, L2_TOR_TYPE torType){
ASSERT(instPtr != NULL);
ASSERT(tmStamp != NULL);
// Get the next frame start time
if(torType == L2_TOR_TX) {
tmStamp->int_part = L2_GetNextFrameNum(instPtr)*L2_SLOTS_PER_FRAME + L2_GetNextSlotNum(instPtr);
tmStamp->frac_part = 0 ;
}
// Get the last reiceive frame start time
else if(torType == L2_TOR_RX) {
PHY_INST* phyRef = instPtr->config.phyInstPtr;
TIME_WORD timeStamp;
int64_t timeDiff;
Phy_GetFrameSyncTimeStamp(phyRef, &timeStamp);
timeDiff = (uint64_t) timeStamp.frac_part;
timeDiff += ((uint64_t)timeStamp.int_part) * COUNTE_FRAC_MAX;
Phy_GetLastSlotStartTimeStamp(phyRef, tmStamp);
timeDiff -= ((uint64_t) tmStamp->int_part) * COUNTE_FRAC_MAX;
timeDiff -= (uint64_t) tmStamp->frac_part;
timeDiff = (int64_t)Phy_GetTimeComp(L2_L2frame2L1frame(L2_GetLastFrameType(instPtr))) - timeDiff;
if (timeDiff < 0) {
timeDiff += COUNTE_FRAC_MAX;
tmStamp->int_part--;
_nop_();
}
tmStamp->frac_part = (TIME_FRAC)timeDiff;
}
else {
*tmStamp = RTT_TIME_NONE;
DBG_ERROR("Unexpected tx/rx mode, setting dummy values.\n");
}
}
/*****************************************************************************/
/**
* @brief Calculate tmA - tmA.
*
* @param tmA time stamp A
* @param tmA time stamp B
* @param tRound t round
*
* @return NONE
*
******************************************************************************/
void RTT_CalculateTimeGap(const TIME_WORD *tmA, const TIME_WORD *tmB, TIME_WORD *tRound){
ASSERT(tmA != NULL);
ASSERT(tmB != NULL);
ASSERT(tRound != NULL);
uint64_t timeDiff;
timeDiff = (uint64_t) tmB->frac_part;
timeDiff += ((uint64_t)tmB->int_part) * COUNTE_FRAC_MAX;
timeDiff -= ((uint64_t)tmA->int_part) * COUNTE_FRAC_MAX;
timeDiff -= (uint64_t) tmA->frac_part;
tRound->frac_part = (TIME_FRAC)(timeDiff % COUNTE_FRAC_MAX);
tRound->frac_part = (TIME_INT)(timeDiff / COUNTE_FRAC_MAX);
} }
/*****************************************************************************/ /*****************************************************************************/
@@ -186,338 +229,266 @@ int32_t RTT_Init(L2_RTT_FSM** rttFsmPtr){
/*****************************************************************************/ /*****************************************************************************/
/** /**
* @brief Execute a Time-Sync Process. * @brief Update RTT FSM accordingly.
* *
* @param instPtr a pointer to linklayer layer instance * @param instPtr a pointer to linklayer layer instance
* @param torType tx/rx mode type
* *
* @return SUCCESS Time-Sync Process Done. * @return SUCCESS
* FAILURE Time-Sync Process end with error. * error code
* *
******************************************************************************/ ******************************************************************************/
int32_t RTT_Proc(L2_INST* instPtr) { int32_t RTT_FsmUpdate(L2_INST* instPtr, L2_TOR_TYPE torType) {
ASSERT(instPtr != NULL); ASSERT(instPtr != NULL);
L2_RTT_FSM* rttFsmPtr = L2_GetRttDesc(instPtr); L2_RTT_FSM* rttFsmPtr = L2_GetRttFsm(instPtr);
RTT_RECORD* rttRecord = &(rttFsmPtr->rttRecord); RTT_RECORD* rttRecord = &(rttFsmPtr->rttRecord);
PHY_INST *phyRef = instPtr->config.phyInstPtr; PHY_INST *phyRef = instPtr->config.phyInstPtr;
int32_t Status = SUCCESS;
L2_RTT_TOR lastTor, nextTor;
if(instPtr->rtneConfig.last != NULL && instPtr->rtneConfig.last->slotType == SLOT_INTRA_TSYNC){
// Get last tor state
if(instPtr->rtneConfig.last->src0 == instPtr->myInfo.nodeId){
lastTor = RTT_SEND;
}
else if(instPtr->rtneConfig.last->dst0 == instPtr->myInfo.nodeId){
lastTor = RTT_RCV;
}
else{
lastTor = RTT_NONE;
}
}
else{
lastTor = RTT_NONE;
}
if(instPtr->rtneConfig.next != NULL && instPtr->rtneConfig.next->slotType == SLOT_INTRA_TSYNC){ DBG_INFO("-------- RTT FSM Update --------\n");
// Get next tor state
if(instPtr->rtneConfig.next->src0 == instPtr->myInfo.nodeId){
nextTor = RTT_SEND;
}
else if(instPtr->rtneConfig.next->dst0 == instPtr->myInfo.nodeId){
nextTor = RTT_RCV;
}
else{
nextTor = RTT_NONE;
}
}
else{
nextTor = RTT_NONE;
}
DBG_INFO("-------- RTT Procedure --------\n");
// Stage trans // Stage trans
// For Head Node: Tx->Rx->Tx->Rx // For Head Node: Tx->Rx->Tx->Rx
if(instPtr->myInfo.nodeType == HEAD_NODE){ if(L2_GetNodeType(instPtr) == HEAD_NODE){
switch(rttFsmPtr->rttStatge.next) { switch(rttFsmPtr->rttStatge) {
case RTT_NONE_STAGE :
DBG_INFO("RTT procedure start...\n");
// TODO
// RTT_ResetRecord(rttRecord);
RTT_StageUpdate(rttFsmPtr, RTT_REQ1);
break;
case RTT_REQ1 : case RTT_REQ1 :
if(nextTor == RTT_SEND){ if(torType == L2_TOR_TX){
DBG_INFO("Prepare data for RTT REQ-1\n"); DBG_INFO("Prepare data for RTT REQ-1\n");
rttRecord->Tt1.timeH = L2_GetNextFrameNum(instPtr)*L2_SLOTS_PER_FRAME + L2_GetNextSlotNum(instPtr); // Update Tt1
rttRecord->Tt1.timeL = 0 ; RTT_GetFrameStart(instPtr, &(rttRecord->Tt1), torType);
DBG_INFO("Tt1-timeL:%u.\n", rttRecord->Tt1.timeL); DBG_INFO("Tt1-timeL:%u.\n", rttRecord->Tt1.int_part);
DBG_INFO("Tt1-timeH:%u.\n", rttRecord->Tt1.timeH); DBG_INFO("Tt1-timeH:%u.\n", rttRecord->Tt1.frac_part);
// Prepare data for sending...
rttFsmPtr->timeStamp = rttRecord->Tt1;
rttFsmPtr->doppler = RTT_DOPPLER_NONE;
RTT_StageUpdate(rttFsmPtr, RTT_RSP1); RTT_StageUpdate(rttFsmPtr, RTT_RSP1);
} }
else{ else{
DBG_INFO("Skip RTT slot, [Stage: %u].\n", rttFsmPtr->rttStatge.next); DBG_INFO("RTT FSM remain current state [%d].\n", rttFsmPtr->rttStatge);
} }
break; break;
case RTT_RSP1 : case RTT_RSP1 :
if(lastTor == RTT_RCV){ if(torType == L2_TOR_RX){
if(L2_isDataAvail(instPtr) == true){ if(L2_isDataAvail(instPtr) == true){
DBG_INFO("RTT RSP-1 have been received.\n"); DBG_INFO("RTT RSP-1 have been received.\n");
DBG_INFO("Preparing data for RTT REQ-2.\n"); DBG_INFO("Preparing data for RTT REQ-2.\n");
rttFsmPtr->isR2Valid = true; rttFsmPtr->isR2Valid = true;
// Calculate doppler // Update doppler
double doppler = Phy_GetCoarseFreqEst(phyRef); rttRecord->dopplerR2 = (int32_t)Phy_GetCoarseFreqEst(phyRef);
rttRecord->dopplerR2 = (int32_t)doppler; // Update Tr2
DBG_INFO("rttRecord->dopplerR2: %u.\n", rttRecord->dopplerR2); RTT_GetFrameStart(instPtr, &(rttRecord->Tr2), torType);
// Calculate last frame receive time.
int32_t counter10ns = RTT_GetCounter10nsDeviation(phyRef);
// response for rtt Adj
counter10ns += 10000;
counter10ns -= rttRecord->rttAdj;
if ( counter10ns < 0) {
counter10ns += COUNTE_FRAC_MAX;
rttRecord->Tr2.timeH -- ;
_nop_();
}
rttRecord->Tr2.timeH = L2_GetLastFrameNum(instPtr)*L2_SLOTS_PER_FRAME + L2_GetLastSlotNum(instPtr);
rttRecord->Tr2.timeL = (uint32_t)counter10ns;
DBG_INFO("Tr2-timeL:%u.\n", rttRecord->Tr2.timeL);
DBG_INFO("Tr2-timeH:%u.\n", rttRecord->Tr2.timeH);
// Calculate TroundA // Calculate TroundA
rttRecord->TroundA.timeL = rttRecord->Tr2.timeL; RTT_CalculateTimeGap(&(rttRecord->Tt1), &(rttRecord->Tr2), &(rttRecord->TroundA));
rttRecord->TroundA.timeH = rttRecord->Tr2.timeH - rttRecord->Tt1.timeH;
DBG_INFO("TroundA-timeL:%u.\n", rttRecord->TroundA.timeL);
DBG_INFO("TroundA-timeH:%u.\n", rttRecord->TroundA.timeH);
} }
else{ else{
rttFsmPtr->isR2Valid = false; rttFsmPtr->isR2Valid = false;
rttRecord->dopplerR2 = RTT_DOPPLER_NONE; rttRecord->dopplerR2 = RTT_DOPPLER_NONE;
rttRecord->TroundA.timeL = RTT_TIME_NONE; rttRecord->Tr2 = RTT_TIME_NONE;
rttRecord->TroundA.timeH = RTT_TIME_NONE; rttRecord->TroundA = RTT_TIME_NONE;
DBG_WARNING("RTT RSP-1 have not been received [Stage: %u].\n", rttFsmPtr->rttStatge.next); DBG_WARNING("RTT RSP-1 had not been received [Stage: %d].\n", rttFsmPtr->rttStatge);
DBG_WARNING("Set dopplerR2 and TroundA to dummy value.\n"); DBG_WARNING("Set Tr2 dopplerR2 and TroundA to dummy value.\n");
DBG_INFO("dopplerR2:%08X.\n", rttRecord->dopplerR2);
DBG_INFO("TroundA-timeL:%08X.\n", rttRecord->TroundA.timeL);
DBG_INFO("TroundA-timeH:%08X.\n", rttRecord->TroundA.timeH);
} }
DBG_INFO("dopplerR2:%08X.\n", rttRecord->dopplerR2);
DBG_INFO("Tr2-timeL:%u.\n", rttRecord->Tr2.frac_part);
DBG_INFO("Tr2-timeH:%u.\n", rttRecord->Tr2.int_part);
DBG_INFO("TroundA-timeL:%08X.\n", rttRecord->TroundA.frac_part);
DBG_INFO("TroundA-timeH:%08X.\n", rttRecord->TroundA.int_part);
RTT_StageUpdate(rttFsmPtr, RTT_REQ2); RTT_StageUpdate(rttFsmPtr, RTT_REQ2);
} }
else{ else{
DBG_INFO("Skip RTT slot, [Stage: %u].\n", rttFsmPtr->rttStatge.next); DBG_INFO("RTT FSM remain current state [%d].\n", rttFsmPtr->rttStatge);
} }
break; break;
case RTT_REQ2 : case RTT_REQ2 :
if(nextTor == RTT_SEND){ if(torType == L2_TOR_TX){
DBG_INFO("Prepare data for RTT REQ-2\n"); DBG_INFO("Prepare data for RTT REQ-2\n");
rttRecord->Tt3.timeH = L2_GetNextFrameNum(instPtr)*L2_SLOTS_PER_FRAME + L2_GetNextSlotNum(instPtr); // Update Tt3
rttRecord->Tt3.timeL = 0 ; RTT_GetFrameStart(instPtr, &(rttRecord->Tt3), torType);
DBG_INFO("Tt3-timeL:%u.\n", rttRecord->Tt3.timeL);
DBG_INFO("Tt3-timeH:%u.\n", rttRecord->Tt3.timeH);
if ( rttFsmPtr->isR2Valid ) { if ( rttFsmPtr->isR2Valid ) {
// Calculate reply A // Calculate reply A
rttRecord->TreplyA.timeL = COUNTE_FRAC_MAX - rttRecord->Tr2.timeL; RTT_CalculateTimeGap(&(rttRecord->Tt3), &(rttRecord->Tr2), &(rttRecord->TreplyA));
rttRecord->TreplyA.timeH = rttRecord->Tt3.timeH - rttRecord->Tr2.timeH - 1;
DBG_INFO("TreplyA-timeL:%u.\n", rttRecord->TreplyA.timeL);
DBG_INFO("TreplyA-timeH:%u.\n", rttRecord->TreplyA.timeH);
} }
else { else {
rttRecord->TreplyA.timeL = RTT_TIME_NONE; rttRecord->TreplyA = RTT_TIME_NONE;
rttRecord->TreplyA.timeH = RTT_TIME_NONE; DBG_WARNING("RTT RSP-1 have not been received [Stage: %d].\n", rttFsmPtr->rttStatge);
DBG_WARNING("RTT RSP-1 have not been received [Stage: %u].\n", rttFsmPtr->rttStatge.next); DBG_WARNING("Set Tt3 and TreplyA to dummy value.\n");
DBG_WARNING("Set TreplyA to dummy value.\n");
DBG_INFO("TreplyA-timeL:%08X.\n", rttRecord->TreplyA.timeL);
DBG_INFO("TreplyA-timeH:%08X.\n", rttRecord->TreplyA.timeH);
} }
DBG_INFO("Tt3-timeL:%08X.\n", rttRecord->Tt3.frac_part);
DBG_INFO("Tt3-timeH:%08X.\n", rttRecord->Tt3.int_part);
DBG_INFO("TreplyA-timeL:%08X.\n", rttRecord->TreplyA.frac_part);
DBG_INFO("TreplyA-timeH:%08X.\n", rttRecord->TreplyA.int_part);
// Prepare data for sending.
rttFsmPtr->timeStamp = rttRecord->TroundA;
rttFsmPtr->doppler = rttRecord->dopplerR2;
RTT_StageUpdate(rttFsmPtr, RTT_RSP2); RTT_StageUpdate(rttFsmPtr, RTT_RSP2);
} }
else{ else{
DBG_INFO("Skip RTT slot, [Stage: %u].\n", rttFsmPtr->rttStatge.next); DBG_INFO("RTT FSM remain current state [%d].\n", rttFsmPtr->rttStatge);
} }
break; break;
case RTT_RSP2 : case RTT_RSP2 :
if(lastTor == RTT_RCV){ if(torType == L2_TOR_RX){
if(L2_isDataAvail(instPtr) == true){ if(L2_isDataAvail(instPtr) == true){
DBG_INFO("RTT RSP-2 have been received.\n"); DBG_INFO("RTT RSP-2 have been received.\n");
rttFsmPtr->isR4Valid = true; rttFsmPtr->isR4Valid = true;
// Calculate doppler // Update doppler
double doppler = Phy_GetCoarseFreqEst(phyRef); double doppler = Phy_GetCoarseFreqEst(phyRef);
rttRecord->dopplerR2 = (int32_t)doppler; rttRecord->dopplerR2 = (int32_t)doppler;
DBG_INFO("rttRecord->dopplerR2: %d.\n", rttRecord->dopplerR2); // Update Tr4
RTT_GetFrameStart(instPtr, &(rttRecord->Tr4), torType);
// Calculate last frame receive time. // Calculate TroundA
int32_t counter10ns = RTT_GetCounter10nsDeviation(phyRef); RTT_CalculateTimeGap(&(rttRecord->Tt3), &(rttRecord->Tr4), &(rttRecord->TroundA));
if ( counter10ns < 0) {
counter10ns += COUNTE_FRAC_MAX;
rttRecord->Tr4.timeH -- ;
}
rttRecord->Tr2.timeH = L2_GetLastFrameNum(instPtr)*L2_SLOTS_PER_FRAME + L2_GetLastSlotNum(instPtr);
// Get a cnt with clk of 192MHz
rttRecord->Tr4.timeL = (uint32_t)counter10ns;
DBG_INFO("Tr4-timeL:%u.\n", rttRecord->Tr4.timeL);
DBG_INFO("Tr4-timeH:%u.\n", rttRecord->Tr4.timeH);
} }
else{ else{
rttFsmPtr->isR4Valid = false; rttFsmPtr->isR4Valid = false;
rttRecord->Tr4.timeL = RTT_TIME_NONE; rttRecord->dopplerR4 = RTT_DOPPLER_NONE;
rttRecord->Tr4.timeH = RTT_TIME_NONE; rttRecord->Tr4 = RTT_TIME_NONE;
DBG_WARNING("RTT RSP-2 have not been received [Stage: %u].\n", rttFsmPtr->rttStatge.next); rttRecord->TroundA = RTT_TIME_NONE;
DBG_WARNING("Set Tr4 to dummy value.\n"); DBG_WARNING("RTT RSP-2 have not been received [Stage: %d].\n", rttFsmPtr->rttStatge);
DBG_INFO("Tr4-timeL:%08X.\n", rttRecord->Tr4.timeL); DBG_WARNING("Set Tr4 dopplerR4 and TroundA to dummy value.\n");
DBG_INFO("Tr4-timeH:%08X.\n", rttRecord->Tr4.timeH);
} }
DBG_INFO("dopplerR4:%08X.\n", rttRecord->dopplerR4);
DBG_INFO("Tr4-timeL:%08X.\n", rttRecord->Tr4.frac_part);
DBG_INFO("Tr4-timeH:%08X.\n", rttRecord->Tr4.int_part);
DBG_INFO("TroundA-timeL:%08X.\n", rttRecord->TroundA.frac_part);
DBG_INFO("TroundA-timeH:%08X.\n", rttRecord->TroundA.int_part);
RTT_StageUpdate(rttFsmPtr, RTT_NONE_STAGE); RTT_StageUpdate(rttFsmPtr, RTT_NONE_STAGE);
DBG_INFO("RTT procedure end...\n"); DBG_INFO("RTT procedure end...\n");
} }
else{
DBG_INFO("RTT FSM remain current state [%d].\n", rttFsmPtr->rttStatge);
}
break; break;
default : default :
DBG_ERROR("Unexpected RTT FSM stage [%d].\n", rttFsmPtr->rttStatge);
break; break;
} }
} }
// For Member Node: Rx->Tx->Rx->Tx // For Member Node: Rx->Tx->Rx->Tx
else{ else{
switch (rttFsmPtr->rttStatge.next) { switch(rttFsmPtr->rttStatge) {
case RTT_NONE_STAGE :
DBG_INFO("RTT procedure start...\n");
// TODO
// RTT_ResetRecord(rttRecord);
RTT_StageUpdate(rttFsmPtr, RTT_REQ1);
break;
case RTT_REQ1 : case RTT_REQ1 :
if(lastTor == RTT_RCV){ if(torType == L2_TOR_RX){
if(L2_isDataAvail(instPtr) == true){ if(L2_isDataAvail(instPtr) == true){
DBG_INFO("RTT REQ-1 have been received.\n"); DBG_INFO("RTT REQ-1 have been received.\n");
DBG_INFO("Preparing data for RTT RSP-1.\n"); DBG_INFO("Preparing data for RTT RSP-1.\n");
rttFsmPtr->isR2Valid = true; rttFsmPtr->isR2Valid = true;
// Calculate doppler // Update doppler
double doppler = Phy_GetCoarseFreqEst(phyRef); rttRecord->dopplerR2 = (int32_t)Phy_GetCoarseFreqEst(phyRef);
rttRecord->dopplerR2 = (int32_t)doppler; // Update Tr1
DBG_INFO("rttRecord->dopplerR2: %d.\n", rttRecord->dopplerR2); RTT_GetFrameStart(instPtr, &(rttRecord->Tr1), torType);
// Calculate last frame receive time.
int32_t counter10ns = RTT_GetCounter10nsDeviation(phyRef);
if ( counter10ns < 0) {
counter10ns += COUNTE_FRAC_MAX;
rttRecord->Tr1.timeH -- ;
}
rttRecord->Tr1.timeH = L2_GetLastFrameNum(instPtr)*L2_SLOTS_PER_FRAME + L2_GetLastSlotNum(instPtr);
rttRecord->Tr1.timeL = (uint32_t)counter10ns;
DBG_INFO("Tr1-timeL:%u.\n", rttRecord->Tr1.timeL);
DBG_INFO("Tr1-timeH:%u.\n", rttRecord->Tr1.timeH);
} }
else{ else{
rttFsmPtr->isR1Valid = false; rttFsmPtr->isR1Valid = false;
rttRecord->dopplerR1 = RTT_DOPPLER_NONE; rttRecord->dopplerR1 = RTT_DOPPLER_NONE;
rttRecord->Tr1.timeL = RTT_TIME_NONE; rttRecord->Tr1 = RTT_TIME_NONE;
rttRecord->Tr1.timeH = RTT_TIME_NONE; DBG_WARNING("RTT REQ-1 have not been received [Stage: %d].\n", rttFsmPtr->rttStatge);
DBG_WARNING("RTT REQ-1 have not been received [Stage: %u].\n", rttFsmPtr->rttStatge.next);
DBG_WARNING("Set dopplerR1 and Tr1 to dummy value.\n"); DBG_WARNING("Set dopplerR1 and Tr1 to dummy value.\n");
DBG_INFO("dopplerR1:%08X.\n", rttRecord->dopplerR1);
DBG_INFO("Tr1-timeL:%08X.\n", rttRecord->Tr1.timeL);
DBG_INFO("Tr1-timeH:%08X.\n", rttRecord->Tr1.timeH);
} }
DBG_INFO("dopplerR1:%08X.\n", rttRecord->dopplerR1);
DBG_INFO("Tr1-timeL:%08X.\n", rttRecord->Tr1.frac_part);
DBG_INFO("Tr1-timeH:%08X.\n", rttRecord->Tr1.int_part);
RTT_StageUpdate(rttFsmPtr, RTT_RSP1); RTT_StageUpdate(rttFsmPtr, RTT_RSP1);
} }
else{
DBG_INFO("RTT FSM remain current state [%d].\n", rttFsmPtr->rttStatge);
}
break; break;
case RTT_RSP1 : case RTT_RSP1 :
if(nextTor == RTT_SEND){ if(torType == L2_TOR_TX){
DBG_INFO("Prepare data for RTT RSP-1\n"); DBG_INFO("Prepare data for RTT RSP-1\n");
rttRecord->Tt2.timeH = L2_GetNextFrameNum(instPtr)*L2_SLOTS_PER_FRAME + L2_GetNextSlotNum(instPtr); // Update Tt2
rttRecord->Tt2.timeL = 0 ; RTT_GetFrameStart(instPtr, &(rttRecord->Tt2), torType);
DBG_INFO("Tt2-timeL:%u.\n", rttRecord->Tt2.timeL);
DBG_INFO("Tt2-timeH:%u.\n", rttRecord->Tt2.timeH);
if ( rttFsmPtr->isR1Valid ) { if ( rttFsmPtr->isR1Valid ) {
// Calculate reply B // Calculate reply B
rttRecord->TreplyB.timeL = COUNTE_FRAC_MAX - rttRecord->Tr1.timeL; RTT_CalculateTimeGap(&(rttRecord->Tr1), &(rttRecord->Tt2), &(rttRecord->TreplyB));
rttRecord->TreplyB.timeH = rttRecord->Tt2.timeH - rttRecord->Tr1.timeH - 1;
DBG_INFO("TreplyB-timeL:%u.\n", rttRecord->TreplyB.timeL);
DBG_INFO("TreplyB-timeH:%u.\n", rttRecord->TreplyB.timeH);
} }
else { else {
rttRecord->TreplyB.timeL = RTT_TIME_NONE; rttRecord->TreplyB = RTT_TIME_NONE;
rttRecord->TreplyB.timeH = RTT_TIME_NONE; DBG_WARNING("RTT REQ-1 have not been received [Stage: %d].\n", rttFsmPtr->rttStatge);
DBG_WARNING("RTT REQ-1 have not been received [Stage: %u].\n", rttFsmPtr->rttStatge.next);
DBG_WARNING("Set ReplayB to dummy value.\n"); DBG_WARNING("Set ReplayB to dummy value.\n");
DBG_INFO("TreplyB-timeL:%08X.\n", rttRecord->TreplyB.timeL);
DBG_INFO("TreplyB-timeH:%08X.\n", rttRecord->TreplyB.timeH);
} }
DBG_INFO("Tt2-timeL:%u.\n", rttRecord->Tt2.frac_part);
DBG_INFO("Tt2-timeH:%u.\n", rttRecord->Tt2.int_part);
DBG_INFO("TreplyB-timeL:%08X.\n", rttRecord->TreplyB.frac_part);
DBG_INFO("TreplyB-timeH:%08X.\n", rttRecord->TreplyB.int_part);
// Prepare data for sending.
rttFsmPtr->timeStamp = rttRecord->TreplyB;
rttFsmPtr->doppler = rttRecord->dopplerR1;
RTT_StageUpdate(rttFsmPtr, RTT_REQ2); RTT_StageUpdate(rttFsmPtr, RTT_REQ2);
} }
else{
DBG_INFO("RTT FSM remain current state [%d].\n", rttFsmPtr->rttStatge);
}
break; break;
case RTT_REQ2 : case RTT_REQ2 :
if(lastTor == RTT_RCV){ if(torType == L2_TOR_RX){
if(L2_isDataAvail(instPtr) == true){ if(L2_isDataAvail(instPtr) == true){
DBG_INFO("RTT REQ-2 have been received.\n"); DBG_INFO("RTT REQ-2 have been received.\n");
DBG_INFO("Preparing data for RTT RSP-2.\n"); DBG_INFO("Preparing data for RTT RSP-2.\n");
rttFsmPtr->isR3Valid = true; rttFsmPtr->isR3Valid = true;
// Update doppler
// Calculate doppler rttRecord->dopplerR2 = (int32_t)Phy_GetCoarseFreqEst(phyRef);
double doppler = Phy_GetCoarseFreqEst(phyRef); // Update Tr3
rttRecord->dopplerR2 = (int32_t)doppler; RTT_GetFrameStart(instPtr, &(rttRecord->Tr3), torType);
DBG_INFO("rttRecord->dopplerR2: %d.\n", rttRecord->dopplerR2);
// Calculate last frame receive time.
int32_t counter10ns = RTT_GetCounter10nsDeviation(phyRef);
if ( counter10ns < 0) {
counter10ns += COUNTE_FRAC_MAX;
rttRecord->Tr3.timeH -- ;
}
rttRecord->Tr3.timeL = (uint32_t)counter10ns;
DBG_INFO("Tr3-timeL:%u.\n", rttRecord->Tr3.timeL);
DBG_INFO("Tr3-timeH:%u.\n", rttRecord->Tr3.timeH);
// Calculate TroundB // Calculate TroundB
rttRecord->TroundB.timeL = rttRecord->Tr3.timeL; RTT_CalculateTimeGap(&(rttRecord->Tt2), &(rttRecord->Tr3), &(rttRecord->TroundB));
rttRecord->TroundB.timeH = rttRecord->Tr3.timeH - rttRecord->Tt2.timeH;
DBG_INFO("TroundB-timeL:%u.\n", rttRecord->TroundB.timeL);
DBG_INFO("TroundB-timeH:%u.\n", rttRecord->TroundB.timeH);
} }
else{ else{
rttFsmPtr->isR3Valid = false; rttFsmPtr->isR3Valid = false;
rttRecord->dopplerR3 = RTT_DOPPLER_NONE; rttRecord->dopplerR3 = RTT_DOPPLER_NONE;
rttRecord->TroundB.timeL = RTT_TIME_NONE; rttRecord->TroundB = RTT_TIME_NONE;
rttRecord->TroundB.timeH = RTT_TIME_NONE; DBG_WARNING("RTT REQ-2 have not been received [Stage: %d].\n", rttFsmPtr->rttStatge);
DBG_WARNING("RTT REQ-2 have not been received [Stage: %u].\n", rttFsmPtr->rttStatge.next);
DBG_WARNING("Set dopplerR3 and ReplayB to dummy value.\n"); DBG_WARNING("Set dopplerR3 and ReplayB to dummy value.\n");
DBG_INFO("dopplerR3:%08X.\n", rttRecord->dopplerR3);
DBG_INFO("TroundB-timeL:%08X.\n", rttRecord->TroundB.timeL);
DBG_INFO("TroundB-timeH:%08X.\n", rttRecord->TroundB.timeH);
} }
DBG_INFO("dopplerR3:%08X.\n", rttRecord->dopplerR3);
DBG_INFO("Tr3-timeL:%u.\n", rttRecord->Tr3.frac_part);
DBG_INFO("Tr3-timeH:%u.\n", rttRecord->Tr3.int_part);
DBG_INFO("TroundB-timeL:%08X.\n", rttRecord->TroundB.frac_part);
DBG_INFO("TroundB-timeH:%08X.\n", rttRecord->TroundB.int_part);
RTT_StageUpdate(rttFsmPtr, RTT_RSP2); RTT_StageUpdate(rttFsmPtr, RTT_RSP2);
} }
else{
DBG_INFO("RTT FSM remain current state [%d].\n", rttFsmPtr->rttStatge);
}
break; break;
case RTT_RSP2 : case RTT_RSP2 :
if(nextTor == RTT_SEND){ if(torType == L2_TOR_TX){
DBG_INFO("Prepare data for RTT RSP-2\n"); DBG_INFO("Prepare data for RTT RSP-2\n");
rttRecord->Tt4.timeH = L2_GetNextFrameNum(instPtr)*L2_SLOTS_PER_FRAME + L2_GetNextSlotNum(instPtr); // Update Tt4
rttRecord->Tt4.timeL = 0; RTT_GetFrameStart(instPtr, &(rttRecord->Tt4), torType);
DBG_INFO("Tt4-timeL:%u.\n", rttRecord->Tt4.timeL);
DBG_INFO("Tt4-timeH:%u.\n", rttRecord->Tt4.timeH);
if ( rttFsmPtr->isR3Valid ) { if ( rttFsmPtr->isR3Valid ) {
// Calculate reply B // Calculate reply B
rttRecord->TreplyB.timeL = COUNTE_FRAC_MAX - rttRecord->Tr3.timeL; RTT_CalculateTimeGap(&(rttRecord->Tr3), &(rttRecord->Tt4), &(rttRecord->TreplyB));
rttRecord->TreplyB.timeH = rttRecord->Tt4.timeH - rttRecord->Tr3.timeH - 1; // TODO: implement a time adjustment.
DBG_INFO("TreplyB-timeL:%u.\n", rttRecord->TreplyB.timeL);
DBG_INFO("TreplyB-timeH:%u.\n", rttRecord->TreplyB.timeH);
// TODO implement a time adjustment.
DBG_INFO("RTT procedure end...\n");
} }
else { else {
rttRecord->TreplyB.timeL = RTT_TIME_NONE; rttRecord->TreplyB = RTT_TIME_NONE;
rttRecord->TreplyB.timeH = RTT_TIME_NONE; DBG_WARNING("RTT REQ-2 have not been received [Stage: %d].\n", rttFsmPtr->rttStatge);
DBG_WARNING("RTT REQ-2 have not been received [Stage: %u].\n", rttFsmPtr->rttStatge.next);
DBG_WARNING("Set ReplayB to dummy value.\n"); DBG_WARNING("Set ReplayB to dummy value.\n");
DBG_INFO("TreplyB-timeL:%08X.\n", rttRecord->TreplyB.timeL);
DBG_INFO("TreplyB-timeH:%08X.\n", rttRecord->TreplyB.timeH);
} }
DBG_INFO("Tt4-timeL:%u.\n", rttRecord->Tt4.frac_part);
DBG_INFO("Tt4-timeH:%u.\n", rttRecord->Tt4.int_part);
DBG_INFO("TreplyB-timeL:%08X.\n", rttRecord->TreplyB.frac_part);
DBG_INFO("TreplyB-timeH:%08X.\n", rttRecord->TreplyB.int_part);
// Prepare data for sending.
rttFsmPtr->timeStamp = rttRecord->TreplyB;
rttFsmPtr->doppler = rttRecord->dopplerR3;
RTT_StageUpdate(rttFsmPtr, RTT_NONE_STAGE); RTT_StageUpdate(rttFsmPtr, RTT_NONE_STAGE);
DBG_INFO("RTT procedure end...\n");
}
else{
DBG_INFO("RTT FSM remain current state [%d].\n", rttFsmPtr->rttStatge);
} }
break; break;
default : default:
DBG_ERROR("Unexpected RTT FSM stage [%d].\n", rttFsmPtr->rttStatge);
break; break;
} }
} }
return Status; return SUCCESS;
} }