split rtt to individual file.
This commit is contained in:
Gatsby Van
@@ -0,0 +1,472 @@
|
|||||||
|
/*****************************************************************************/
|
||||||
|
/**
|
||||||
|
* Description: RTT algorithm.
|
||||||
|
*
|
||||||
|
* Author: FanHao
|
||||||
|
* Date: 2023-12-10 20:43
|
||||||
|
* LastEditors: FanHao
|
||||||
|
* LastEditTime: 2023-12-10 20:43
|
||||||
|
* Revisions: Rev 0.0
|
||||||
|
* History:
|
||||||
|
* [2023-12-10]: Initial version, split form time_sync.c
|
||||||
|
*
|
||||||
|
* Copyright(c) 2021 Beijing Institute of Technology.
|
||||||
|
* Lab of Communication and Networking
|
||||||
|
*
|
||||||
|
******************************************************************************/
|
||||||
|
|
||||||
|
/***************************** Include Files *********************************/
|
||||||
|
#include <string.h>
|
||||||
|
#include <stdlib.h>
|
||||||
|
#include "linklayer814.h"
|
||||||
|
#include "rtt.h"
|
||||||
|
#include "../time_ctrl/time_ctrl.h"
|
||||||
|
#include "../phy_ctrl/phy_ctrl.h"
|
||||||
|
#include "../error.h"
|
||||||
|
#include "../common.h"
|
||||||
|
|
||||||
|
/************************** 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 *********************/
|
||||||
|
/**************************** Type Definitions *******************************/
|
||||||
|
/************************** Variable Definitions *****************************/
|
||||||
|
/************************** Function Prototypes ******************************/
|
||||||
|
|
||||||
|
static void RTT_StageUpdate(L2_RTT_FSM *rttFsmPtr, L2_RTT_STGE newStage);
|
||||||
|
static void RTT_GetFrameStart(L2_INST *instPtr, TIME_WORD *tmStamp, L2_TOR_TYPE torType);
|
||||||
|
|
||||||
|
/*****************************************************************************/
|
||||||
|
/**
|
||||||
|
* @brief Update corrent time-sync for RTT stage
|
||||||
|
*
|
||||||
|
* @param scanFsmPtr A pointer to scan net fsm.
|
||||||
|
* @param newSatge New state.
|
||||||
|
*
|
||||||
|
* @return NONE.
|
||||||
|
*
|
||||||
|
******************************************************************************/
|
||||||
|
static void RTT_StageUpdate(L2_RTT_FSM *rttFsmPtr, L2_RTT_STGE newStage)
|
||||||
|
{
|
||||||
|
ASSERT(rttFsmPtr != NULL);
|
||||||
|
ASSERT(newStage >= RTT_NONE_STAGE && newStage <= RTT_RSP2);
|
||||||
|
DBG_INFO("RTT Stage Update: %d -> %d.\n", rttFsmPtr->rttStage, newStage);
|
||||||
|
rttFsmPtr->rttStage = newStage;
|
||||||
|
}
|
||||||
|
|
||||||
|
/*****************************************************************************/
|
||||||
|
/**
|
||||||
|
* @brief Reset RTT FSM
|
||||||
|
*
|
||||||
|
* @param rttFsmPtr a pointer to rtt FSM instance.
|
||||||
|
*
|
||||||
|
* @return NONE
|
||||||
|
*
|
||||||
|
******************************************************************************/
|
||||||
|
void RTT_Reset(L2_RTT_FSM *rttFsmPtr)
|
||||||
|
{
|
||||||
|
ASSERT(rttFsmPtr != NULL);
|
||||||
|
memset(&(rttFsmPtr->rttRecord), 0x00, sizeof(RTT_RECORD));
|
||||||
|
rttFsmPtr->rttStage = RTT_REQ1;
|
||||||
|
rttFsmPtr->isR1Valid = false;
|
||||||
|
rttFsmPtr->isR2Valid = false;
|
||||||
|
rttFsmPtr->isR3Valid = 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)
|
||||||
|
{
|
||||||
|
free(rttFsmPtr);
|
||||||
|
}
|
||||||
|
|
||||||
|
/*****************************************************************************/
|
||||||
|
/**
|
||||||
|
* @brief Get the frame start time. If torType == L2_TOR_TX, get the next
|
||||||
|
* slot start time, if torType == L2_TOR_RX get last frame start
|
||||||
|
* time.
|
||||||
|
*
|
||||||
|
* @param instPtr a pointer to linklayer layer instance
|
||||||
|
* @param tmStamp a pointer to store frame start time
|
||||||
|
* @param torType tx/rx mode
|
||||||
|
*
|
||||||
|
* @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_GetLastReceivedFrameSyncTimeStamp(phyRef, &timeStamp);
|
||||||
|
timeDiff = (uint64_t)timeStamp.frac_part;
|
||||||
|
timeDiff += ((uint64_t)timeStamp.int_part) * TIME_FRAC_MAX;
|
||||||
|
Phy_GetLastReceivedSlotStartTimeStamp(phyRef, tmStamp);
|
||||||
|
timeDiff -= ((uint64_t)tmStamp->int_part) * TIME_FRAC_MAX;
|
||||||
|
timeDiff -= (uint64_t)tmStamp->frac_part;
|
||||||
|
timeDiff = (int64_t)Phy_GetTimeComp(L2_L2frame2L1frame(L2_GetLastFrameType(instPtr))) - timeDiff;
|
||||||
|
if (timeDiff < 0)
|
||||||
|
{
|
||||||
|
timeDiff += TIME_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 Initialize RTT FSM, allocate memory for FSM instance.
|
||||||
|
*
|
||||||
|
* @param rttFsmPtr
|
||||||
|
*
|
||||||
|
* @return SUCCESS Initialization successful.
|
||||||
|
* EMAC_MEMORY_ALLOC_ERROR Memory allocate error.
|
||||||
|
*
|
||||||
|
******************************************************************************/
|
||||||
|
int32_t RTT_Init(L2_RTT_FSM **rttFsmPtr)
|
||||||
|
{
|
||||||
|
(*rttFsmPtr) = (L2_RTT_FSM *)malloc(sizeof(L2_RTT_FSM));
|
||||||
|
if (*rttFsmPtr == NULL)
|
||||||
|
{
|
||||||
|
free(*rttFsmPtr);
|
||||||
|
return EALLOC;
|
||||||
|
}
|
||||||
|
RTT_Reset(*rttFsmPtr);
|
||||||
|
return SUCCESS;
|
||||||
|
}
|
||||||
|
|
||||||
|
/*****************************************************************************/
|
||||||
|
/**
|
||||||
|
* @brief Update RTT FSM accordingly.
|
||||||
|
*
|
||||||
|
* @param instPtr a pointer to linklayer layer instance
|
||||||
|
* @param torType tx/rx mode type
|
||||||
|
*
|
||||||
|
* @return SUCCESS
|
||||||
|
* error code
|
||||||
|
*
|
||||||
|
******************************************************************************/
|
||||||
|
int32_t RTT_FsmUpdate(L2_INST *instPtr, L2_TOR_TYPE torType)
|
||||||
|
{
|
||||||
|
ASSERT(instPtr != NULL);
|
||||||
|
L2_RTT_FSM *rttFsmPtr = L2_GetRttFsm(instPtr);
|
||||||
|
RTT_RECORD *rttRecord = &(rttFsmPtr->rttRecord);
|
||||||
|
PHY_INST *phyRef = instPtr->config.phyInstPtr;
|
||||||
|
|
||||||
|
DBG_INFO("-------- RTT FSM Update --------\n");
|
||||||
|
// Stage trans
|
||||||
|
// For Head Node: Tx->Rx->Tx->Rx
|
||||||
|
if (L2_GetNodeType(instPtr) == L2_NODE_HEAD)
|
||||||
|
{
|
||||||
|
switch (rttFsmPtr->rttStage)
|
||||||
|
{
|
||||||
|
case RTT_REQ1:
|
||||||
|
if (torType == L2_TOR_TX)
|
||||||
|
{
|
||||||
|
DBG_INFO("Prepare data for RTT REQ-1\n");
|
||||||
|
// Update Tt1
|
||||||
|
RTT_GetFrameStart(instPtr, &(rttRecord->Tt1), torType);
|
||||||
|
DBG_INFO("Tt1-timeL:%u.\n", rttRecord->Tt1.int_part);
|
||||||
|
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);
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
DBG_INFO("RTT FSM remain current state [%d].\n", rttFsmPtr->rttStage);
|
||||||
|
}
|
||||||
|
break;
|
||||||
|
case RTT_RSP1:
|
||||||
|
if (torType == L2_TOR_RX)
|
||||||
|
{
|
||||||
|
if (L2_isDataAvail(instPtr) == true)
|
||||||
|
{
|
||||||
|
DBG_INFO("RTT RSP-1 have been received.\n");
|
||||||
|
DBG_INFO("Preparing data for RTT REQ-2.\n");
|
||||||
|
rttFsmPtr->isR2Valid = true;
|
||||||
|
// Update doppler
|
||||||
|
rttRecord->dopplerR2 = (int32_t)Phy_GetCoarseFreqEst(phyRef);
|
||||||
|
// Update Tr2
|
||||||
|
RTT_GetFrameStart(instPtr, &(rttRecord->Tr2), torType);
|
||||||
|
// Calculate TroundA
|
||||||
|
TimeCtrl_CalculateTimeGap(&(rttRecord->Tt1), &(rttRecord->Tr2), &(rttRecord->TroundA));
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
DBG_WARNING("RTT RSP-1 had not been received [Stage: %d].\n", rttFsmPtr->rttStage);
|
||||||
|
DBG_WARNING("Set Tr2 dopplerR2 and TroundA to dummy value.\n");
|
||||||
|
rttFsmPtr->isR2Valid = false;
|
||||||
|
rttRecord->dopplerR2 = RTT_DOPPLER_NONE;
|
||||||
|
rttRecord->Tr2 = RTT_TIME_NONE;
|
||||||
|
rttRecord->TroundA = RTT_TIME_NONE;
|
||||||
|
}
|
||||||
|
DBG_INFO("dopplerR2:%d.\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:%u.\n", rttRecord->TroundA.frac_part);
|
||||||
|
DBG_INFO("TroundA-timeH:%u.\n", rttRecord->TroundA.int_part);
|
||||||
|
RTT_StageUpdate(rttFsmPtr, RTT_REQ2);
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
DBG_INFO("RTT FSM remain current state [%d].\n", rttFsmPtr->rttStage);
|
||||||
|
}
|
||||||
|
break;
|
||||||
|
case RTT_REQ2:
|
||||||
|
if (torType == L2_TOR_TX)
|
||||||
|
{
|
||||||
|
DBG_INFO("Prepare data for RTT REQ-2\n");
|
||||||
|
// Update Tt3
|
||||||
|
RTT_GetFrameStart(instPtr, &(rttRecord->Tt3), torType);
|
||||||
|
|
||||||
|
if (rttFsmPtr->isR2Valid)
|
||||||
|
{
|
||||||
|
// Calculate reply A
|
||||||
|
TimeCtrl_CalculateTimeGap(&(rttRecord->Tt3), &(rttRecord->Tr2), &(rttRecord->TreplyA));
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
DBG_WARNING("RTT RSP-1 have not been received [Stage: %d].\n", rttFsmPtr->rttStage);
|
||||||
|
DBG_WARNING("Set TreplyA to dummy value.\n");
|
||||||
|
rttRecord->TreplyA = RTT_TIME_NONE;
|
||||||
|
}
|
||||||
|
DBG_INFO("Tt3-timeL:%u.\n", rttRecord->Tt3.frac_part);
|
||||||
|
DBG_INFO("Tt3-timeH:%u.\n", rttRecord->Tt3.int_part);
|
||||||
|
DBG_INFO("TreplyA-timeL:%u.\n", rttRecord->TreplyA.frac_part);
|
||||||
|
DBG_INFO("TreplyA-timeH:%u.\n", rttRecord->TreplyA.int_part);
|
||||||
|
|
||||||
|
// Prepare data for sending.
|
||||||
|
rttFsmPtr->timeStamp = rttRecord->TroundA;
|
||||||
|
rttFsmPtr->doppler = rttRecord->dopplerR2;
|
||||||
|
RTT_StageUpdate(rttFsmPtr, RTT_RSP2);
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
DBG_INFO("RTT FSM remain current state [%d].\n", rttFsmPtr->rttStage);
|
||||||
|
}
|
||||||
|
break;
|
||||||
|
case RTT_RSP2:
|
||||||
|
if (torType == L2_TOR_RX)
|
||||||
|
{
|
||||||
|
if (L2_isDataAvail(instPtr) == true)
|
||||||
|
{
|
||||||
|
DBG_INFO("RTT RSP-2 have been received.\n");
|
||||||
|
rttFsmPtr->isR4Valid = true;
|
||||||
|
// Update doppler
|
||||||
|
double doppler = Phy_GetCoarseFreqEst(phyRef);
|
||||||
|
rttRecord->dopplerR2 = (int32_t)doppler;
|
||||||
|
// Update Tr4
|
||||||
|
RTT_GetFrameStart(instPtr, &(rttRecord->Tr4), torType);
|
||||||
|
// Calculate TroundA
|
||||||
|
TimeCtrl_CalculateTimeGap(&(rttRecord->Tt3), &(rttRecord->Tr4), &(rttRecord->TroundA));
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
DBG_WARNING("RTT RSP-1 have not been received [Stage: %d].\n", rttFsmPtr->rttStage);
|
||||||
|
DBG_WARNING("Set Tr4 dopplerR4 and TroundA to dummy value.\n");
|
||||||
|
rttFsmPtr->isR4Valid = false;
|
||||||
|
rttRecord->dopplerR4 = RTT_DOPPLER_NONE;
|
||||||
|
rttRecord->Tr4 = RTT_TIME_NONE;
|
||||||
|
rttRecord->TroundA = RTT_TIME_NONE;
|
||||||
|
}
|
||||||
|
DBG_INFO("dopplerR4:%d.\n", rttRecord->dopplerR4);
|
||||||
|
DBG_INFO("Tr4-timeL:%u.\n", rttRecord->Tr4.frac_part);
|
||||||
|
DBG_INFO("Tr4-timeH:%u.\n", rttRecord->Tr4.int_part);
|
||||||
|
DBG_INFO("TroundA-timeL:%u.\n", rttRecord->TroundA.frac_part);
|
||||||
|
DBG_INFO("TroundA-timeH:%u.\n", rttRecord->TroundA.int_part);
|
||||||
|
RTT_StageUpdate(rttFsmPtr, RTT_REQ1);
|
||||||
|
DBG_INFO("RTT procedure end...\n");
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
DBG_INFO("RTT FSM remain current state [%d].\n", rttFsmPtr->rttStage);
|
||||||
|
}
|
||||||
|
break;
|
||||||
|
default:
|
||||||
|
DBG_ERROR("Unexpected RTT FSM stage [%d].\n", rttFsmPtr->rttStage);
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
// For Member Node: Rx->Tx->Rx->Tx
|
||||||
|
else
|
||||||
|
{
|
||||||
|
switch (rttFsmPtr->rttStage)
|
||||||
|
{
|
||||||
|
case RTT_REQ1:
|
||||||
|
if (torType == L2_TOR_RX)
|
||||||
|
{
|
||||||
|
if (L2_isDataAvail(instPtr) == true)
|
||||||
|
{
|
||||||
|
DBG_INFO("RTT REQ-1 have been received.\n");
|
||||||
|
DBG_INFO("Preparing data for RTT RSP-1.\n");
|
||||||
|
rttFsmPtr->isR2Valid = true;
|
||||||
|
// Update doppler
|
||||||
|
rttRecord->dopplerR2 = (int32_t)Phy_GetCoarseFreqEst(phyRef);
|
||||||
|
// Update Tr1
|
||||||
|
RTT_GetFrameStart(instPtr, &(rttRecord->Tr1), torType);
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
DBG_WARNING("RTT REQ-1 have not been received [Stage: %d].\n", rttFsmPtr->rttStage);
|
||||||
|
DBG_WARNING("Set dopplerR1 and Tr1 to dummy value.\n");
|
||||||
|
rttFsmPtr->isR1Valid = false;
|
||||||
|
rttRecord->dopplerR1 = RTT_DOPPLER_NONE;
|
||||||
|
rttRecord->Tr1 = RTT_TIME_NONE;
|
||||||
|
}
|
||||||
|
DBG_INFO("dopplerR1:%d.\n", rttRecord->dopplerR1);
|
||||||
|
DBG_INFO("Tr1-timeL:%u.\n", rttRecord->Tr1.frac_part);
|
||||||
|
DBG_INFO("Tr1-timeH:%u.\n", rttRecord->Tr1.int_part);
|
||||||
|
RTT_StageUpdate(rttFsmPtr, RTT_RSP1);
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
DBG_INFO("RTT FSM remain current state [%d].\n", rttFsmPtr->rttStage);
|
||||||
|
}
|
||||||
|
break;
|
||||||
|
case RTT_RSP1:
|
||||||
|
if (torType == L2_TOR_TX)
|
||||||
|
{
|
||||||
|
DBG_INFO("Prepare data for RTT RSP-1\n");
|
||||||
|
// Update Tt2
|
||||||
|
RTT_GetFrameStart(instPtr, &(rttRecord->Tt2), torType);
|
||||||
|
if (rttFsmPtr->isR1Valid)
|
||||||
|
{
|
||||||
|
// Calculate reply B
|
||||||
|
TimeCtrl_CalculateTimeGap(&(rttRecord->Tr1), &(rttRecord->Tt2), &(rttRecord->TreplyB));
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
rttRecord->TreplyB = RTT_TIME_NONE;
|
||||||
|
DBG_WARNING("RTT REQ-1 have not been received [Stage: %d].\n", rttFsmPtr->rttStage);
|
||||||
|
DBG_WARNING("Set ReplayB to dummy value.\n");
|
||||||
|
}
|
||||||
|
DBG_INFO("Tt2-timeL:%u.\n", rttRecord->Tt2.frac_part);
|
||||||
|
DBG_INFO("Tt2-timeH:%u.\n", rttRecord->Tt2.int_part);
|
||||||
|
DBG_INFO("TreplyB-timeL:%u.\n", rttRecord->TreplyB.frac_part);
|
||||||
|
DBG_INFO("TreplyB-timeH:%u.\n", rttRecord->TreplyB.int_part);
|
||||||
|
|
||||||
|
// Prepare data for sending.
|
||||||
|
rttFsmPtr->timeStamp = rttRecord->TreplyB;
|
||||||
|
rttFsmPtr->doppler = rttRecord->dopplerR1;
|
||||||
|
RTT_StageUpdate(rttFsmPtr, RTT_REQ2);
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
DBG_INFO("RTT FSM remain current state [%d].\n", rttFsmPtr->rttStage);
|
||||||
|
}
|
||||||
|
break;
|
||||||
|
case RTT_REQ2:
|
||||||
|
if (torType == L2_TOR_RX)
|
||||||
|
{
|
||||||
|
if (L2_isDataAvail(instPtr) == true)
|
||||||
|
{
|
||||||
|
DBG_INFO("RTT REQ-2 have been received.\n");
|
||||||
|
DBG_INFO("Preparing data for RTT RSP-2.\n");
|
||||||
|
rttFsmPtr->isR3Valid = true;
|
||||||
|
// Update doppler
|
||||||
|
rttRecord->dopplerR2 = (int32_t)Phy_GetCoarseFreqEst(phyRef);
|
||||||
|
// Update Tr3
|
||||||
|
RTT_GetFrameStart(instPtr, &(rttRecord->Tr3), torType);
|
||||||
|
// Calculate TroundB
|
||||||
|
TimeCtrl_CalculateTimeGap(&(rttRecord->Tt2), &(rttRecord->Tr3), &(rttRecord->TroundB));
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
DBG_WARNING("RTT REQ-2 have not been received [Stage: %d].\n", rttFsmPtr->rttStage);
|
||||||
|
DBG_WARNING("Set dopplerR3 and ReplayB to dummy value.\n");
|
||||||
|
rttFsmPtr->isR3Valid = false;
|
||||||
|
rttRecord->dopplerR3 = RTT_DOPPLER_NONE;
|
||||||
|
rttRecord->TroundB = RTT_TIME_NONE;
|
||||||
|
}
|
||||||
|
DBG_INFO("dopplerR3:%d.\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:%u.\n", rttRecord->TroundB.frac_part);
|
||||||
|
DBG_INFO("TroundB-timeH:%u.\n", rttRecord->TroundB.int_part);
|
||||||
|
RTT_StageUpdate(rttFsmPtr, RTT_RSP2);
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
DBG_INFO("RTT FSM remain current state [%d].\n", rttFsmPtr->rttStage);
|
||||||
|
}
|
||||||
|
break;
|
||||||
|
case RTT_RSP2:
|
||||||
|
if (torType == L2_TOR_TX)
|
||||||
|
{
|
||||||
|
DBG_INFO("Prepare data for RTT RSP-2\n");
|
||||||
|
// Update Tt4
|
||||||
|
RTT_GetFrameStart(instPtr, &(rttRecord->Tt4), torType);
|
||||||
|
if (rttFsmPtr->isR3Valid)
|
||||||
|
{
|
||||||
|
// Calculate reply B
|
||||||
|
TimeCtrl_CalculateTimeGap(&(rttRecord->Tr3), &(rttRecord->Tt4), &(rttRecord->TreplyB));
|
||||||
|
// TODO: implement a time adjustment.
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
DBG_WARNING("RTT REQ-2 have not been received [Stage: %d].\n", rttFsmPtr->rttStage);
|
||||||
|
DBG_WARNING("Set ReplayB to dummy value.\n");
|
||||||
|
rttRecord->TreplyB = RTT_TIME_NONE;
|
||||||
|
}
|
||||||
|
DBG_INFO("Tt4-timeL:%u.\n", rttRecord->Tt4.frac_part);
|
||||||
|
DBG_INFO("Tt4-timeH:%u.\n", rttRecord->Tt4.int_part);
|
||||||
|
DBG_INFO("TreplyB-timeL:%u.\n", rttRecord->TreplyB.frac_part);
|
||||||
|
DBG_INFO("TreplyB-timeH:%u.\n", rttRecord->TreplyB.int_part);
|
||||||
|
|
||||||
|
// Prepare data for sending.
|
||||||
|
rttFsmPtr->timeStamp = rttRecord->TreplyB;
|
||||||
|
rttFsmPtr->doppler = rttRecord->dopplerR3;
|
||||||
|
RTT_StageUpdate(rttFsmPtr, RTT_REQ1);
|
||||||
|
DBG_INFO("RTT procedure end...\n");
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
DBG_INFO("RTT FSM remain current state [%d].\n", rttFsmPtr->rttStage);
|
||||||
|
}
|
||||||
|
break;
|
||||||
|
default:
|
||||||
|
DBG_ERROR("Unexpected RTT FSM stage [%d].\n", rttFsmPtr->rttStage);
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
return SUCCESS;
|
||||||
|
}
|
||||||
@@ -22,25 +22,16 @@
|
|||||||
#include "linklayer814.h"
|
#include "linklayer814.h"
|
||||||
#include "rtt.h"
|
#include "rtt.h"
|
||||||
#include "../time_ctrl/time_ctrl.h"
|
#include "../time_ctrl/time_ctrl.h"
|
||||||
|
#include "../phy_ctrl/phy_ctrl.h"
|
||||||
#include "../error.h"
|
#include "../error.h"
|
||||||
#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_STGE newStage);
|
|
||||||
static void RTT_GetFrameStart(L2_INST *instPtr, TIME_WORD *tmStamp, L2_TOR_TYPE torType);
|
|
||||||
|
|
||||||
/*****************************************************************************/
|
/*****************************************************************************/
|
||||||
/**
|
/**
|
||||||
* @brief a slot alignment, base on physical layer information.
|
* @brief a slot alignment, base on physical layer information.
|
||||||
@@ -86,432 +77,4 @@ int32_t L2_TimeAlign(L2_INST *instPtr, uint32_t frameNum, uint32_t slotNum, bool
|
|||||||
slotTimerAdjmet.frac_part = (uint32_t)(timeDiff % TIME_FRAC_MAX);
|
slotTimerAdjmet.frac_part = (uint32_t)(timeDiff % TIME_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 Update corrent time-sync for RTT stage
|
|
||||||
*
|
|
||||||
* @param scanFsmPtr A pointer to scan net fsm.
|
|
||||||
* @param newSatge New state.
|
|
||||||
*
|
|
||||||
* @return NONE.
|
|
||||||
*
|
|
||||||
******************************************************************************/
|
|
||||||
static void RTT_StageUpdate(L2_RTT_FSM *rttFsmPtr, L2_RTT_STGE newStage)
|
|
||||||
{
|
|
||||||
ASSERT(rttFsmPtr != NULL);
|
|
||||||
ASSERT(newStage >= RTT_NONE_STAGE && newStage <= RTT_RSP2);
|
|
||||||
DBG_INFO("RTT Stage Update: %d -> %d.\n", rttFsmPtr->rttStage, newStage);
|
|
||||||
rttFsmPtr->rttStage = newStage;
|
|
||||||
}
|
|
||||||
|
|
||||||
/*****************************************************************************/
|
|
||||||
/**
|
|
||||||
* @brief Reset RTT FSM
|
|
||||||
*
|
|
||||||
* @param rttFsmPtr a pointer to rtt FSM instance.
|
|
||||||
*
|
|
||||||
* @return NONE
|
|
||||||
*
|
|
||||||
******************************************************************************/
|
|
||||||
void RTT_Reset(L2_RTT_FSM *rttFsmPtr)
|
|
||||||
{
|
|
||||||
ASSERT(rttFsmPtr != NULL);
|
|
||||||
memset(&(rttFsmPtr->rttRecord), 0x00, sizeof(RTT_RECORD));
|
|
||||||
rttFsmPtr->rttStage = RTT_REQ1;
|
|
||||||
rttFsmPtr->isR1Valid = false;
|
|
||||||
rttFsmPtr->isR2Valid = false;
|
|
||||||
rttFsmPtr->isR3Valid = 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)
|
|
||||||
{
|
|
||||||
free(rttFsmPtr);
|
|
||||||
}
|
|
||||||
|
|
||||||
/*****************************************************************************/
|
|
||||||
/**
|
|
||||||
* @brief Get the frame start time. If torType == L2_TOR_TX, get the next
|
|
||||||
* slot start time, if torType == L2_TOR_RX get last frame start
|
|
||||||
* time.
|
|
||||||
*
|
|
||||||
* @param instPtr a pointer to linklayer layer instance
|
|
||||||
* @param tmStamp a pointer to store frame start time
|
|
||||||
* @param torType tx/rx mode
|
|
||||||
*
|
|
||||||
* @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_GetLastReceivedFrameSyncTimeStamp(phyRef, &timeStamp);
|
|
||||||
timeDiff = (uint64_t)timeStamp.frac_part;
|
|
||||||
timeDiff += ((uint64_t)timeStamp.int_part) * TIME_FRAC_MAX;
|
|
||||||
Phy_GetLastReceivedSlotStartTimeStamp(phyRef, tmStamp);
|
|
||||||
timeDiff -= ((uint64_t)tmStamp->int_part) * TIME_FRAC_MAX;
|
|
||||||
timeDiff -= (uint64_t)tmStamp->frac_part;
|
|
||||||
timeDiff = (int64_t)Phy_GetTimeComp(L2_L2frame2L1frame(L2_GetLastFrameType(instPtr))) - timeDiff;
|
|
||||||
if (timeDiff < 0)
|
|
||||||
{
|
|
||||||
timeDiff += TIME_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 Initialize RTT FSM, allocate memory for FSM instance.
|
|
||||||
*
|
|
||||||
* @param rttFsmPtr
|
|
||||||
*
|
|
||||||
* @return SUCCESS Initialization successful.
|
|
||||||
* EMAC_MEMORY_ALLOC_ERROR Memory allocate error.
|
|
||||||
*
|
|
||||||
******************************************************************************/
|
|
||||||
int32_t RTT_Init(L2_RTT_FSM **rttFsmPtr)
|
|
||||||
{
|
|
||||||
(*rttFsmPtr) = (L2_RTT_FSM *)malloc(sizeof(L2_RTT_FSM));
|
|
||||||
if (*rttFsmPtr == NULL)
|
|
||||||
{
|
|
||||||
free(*rttFsmPtr);
|
|
||||||
return EALLOC;
|
|
||||||
}
|
|
||||||
RTT_Reset(*rttFsmPtr);
|
|
||||||
return SUCCESS;
|
|
||||||
}
|
|
||||||
|
|
||||||
/*****************************************************************************/
|
|
||||||
/**
|
|
||||||
* @brief Update RTT FSM accordingly.
|
|
||||||
*
|
|
||||||
* @param instPtr a pointer to linklayer layer instance
|
|
||||||
* @param torType tx/rx mode type
|
|
||||||
*
|
|
||||||
* @return SUCCESS
|
|
||||||
* error code
|
|
||||||
*
|
|
||||||
******************************************************************************/
|
|
||||||
int32_t RTT_FsmUpdate(L2_INST *instPtr, L2_TOR_TYPE torType)
|
|
||||||
{
|
|
||||||
ASSERT(instPtr != NULL);
|
|
||||||
L2_RTT_FSM *rttFsmPtr = L2_GetRttFsm(instPtr);
|
|
||||||
RTT_RECORD *rttRecord = &(rttFsmPtr->rttRecord);
|
|
||||||
PHY_INST *phyRef = instPtr->config.phyInstPtr;
|
|
||||||
|
|
||||||
DBG_INFO("-------- RTT FSM Update --------\n");
|
|
||||||
// Stage trans
|
|
||||||
// For Head Node: Tx->Rx->Tx->Rx
|
|
||||||
if (L2_GetNodeType(instPtr) == L2_NODE_HEAD)
|
|
||||||
{
|
|
||||||
switch (rttFsmPtr->rttStage)
|
|
||||||
{
|
|
||||||
case RTT_REQ1:
|
|
||||||
if (torType == L2_TOR_TX)
|
|
||||||
{
|
|
||||||
DBG_INFO("Prepare data for RTT REQ-1\n");
|
|
||||||
// Update Tt1
|
|
||||||
RTT_GetFrameStart(instPtr, &(rttRecord->Tt1), torType);
|
|
||||||
DBG_INFO("Tt1-timeL:%u.\n", rttRecord->Tt1.int_part);
|
|
||||||
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);
|
|
||||||
}
|
|
||||||
else
|
|
||||||
{
|
|
||||||
DBG_INFO("RTT FSM remain current state [%d].\n", rttFsmPtr->rttStage);
|
|
||||||
}
|
|
||||||
break;
|
|
||||||
case RTT_RSP1:
|
|
||||||
if (torType == L2_TOR_RX)
|
|
||||||
{
|
|
||||||
if (L2_isDataAvail(instPtr) == true)
|
|
||||||
{
|
|
||||||
DBG_INFO("RTT RSP-1 have been received.\n");
|
|
||||||
DBG_INFO("Preparing data for RTT REQ-2.\n");
|
|
||||||
rttFsmPtr->isR2Valid = true;
|
|
||||||
// Update doppler
|
|
||||||
rttRecord->dopplerR2 = (int32_t)Phy_GetCoarseFreqEst(phyRef);
|
|
||||||
// Update Tr2
|
|
||||||
RTT_GetFrameStart(instPtr, &(rttRecord->Tr2), torType);
|
|
||||||
// Calculate TroundA
|
|
||||||
TimeCtrl_CalculateTimeGap(&(rttRecord->Tt1), &(rttRecord->Tr2), &(rttRecord->TroundA));
|
|
||||||
}
|
|
||||||
else
|
|
||||||
{
|
|
||||||
DBG_WARNING("RTT RSP-1 had not been received [Stage: %d].\n", rttFsmPtr->rttStage);
|
|
||||||
DBG_WARNING("Set Tr2 dopplerR2 and TroundA to dummy value.\n");
|
|
||||||
rttFsmPtr->isR2Valid = false;
|
|
||||||
rttRecord->dopplerR2 = RTT_DOPPLER_NONE;
|
|
||||||
rttRecord->Tr2 = RTT_TIME_NONE;
|
|
||||||
rttRecord->TroundA = RTT_TIME_NONE;
|
|
||||||
}
|
|
||||||
DBG_INFO("dopplerR2:%d.\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:%u.\n", rttRecord->TroundA.frac_part);
|
|
||||||
DBG_INFO("TroundA-timeH:%u.\n", rttRecord->TroundA.int_part);
|
|
||||||
RTT_StageUpdate(rttFsmPtr, RTT_REQ2);
|
|
||||||
}
|
|
||||||
else
|
|
||||||
{
|
|
||||||
DBG_INFO("RTT FSM remain current state [%d].\n", rttFsmPtr->rttStage);
|
|
||||||
}
|
|
||||||
break;
|
|
||||||
case RTT_REQ2:
|
|
||||||
if (torType == L2_TOR_TX)
|
|
||||||
{
|
|
||||||
DBG_INFO("Prepare data for RTT REQ-2\n");
|
|
||||||
// Update Tt3
|
|
||||||
RTT_GetFrameStart(instPtr, &(rttRecord->Tt3), torType);
|
|
||||||
|
|
||||||
if (rttFsmPtr->isR2Valid)
|
|
||||||
{
|
|
||||||
// Calculate reply A
|
|
||||||
TimeCtrl_CalculateTimeGap(&(rttRecord->Tt3), &(rttRecord->Tr2), &(rttRecord->TreplyA));
|
|
||||||
}
|
|
||||||
else
|
|
||||||
{
|
|
||||||
DBG_WARNING("RTT RSP-1 have not been received [Stage: %d].\n", rttFsmPtr->rttStage);
|
|
||||||
DBG_WARNING("Set TreplyA to dummy value.\n");
|
|
||||||
rttRecord->TreplyA = RTT_TIME_NONE;
|
|
||||||
}
|
|
||||||
DBG_INFO("Tt3-timeL:%u.\n", rttRecord->Tt3.frac_part);
|
|
||||||
DBG_INFO("Tt3-timeH:%u.\n", rttRecord->Tt3.int_part);
|
|
||||||
DBG_INFO("TreplyA-timeL:%u.\n", rttRecord->TreplyA.frac_part);
|
|
||||||
DBG_INFO("TreplyA-timeH:%u.\n", rttRecord->TreplyA.int_part);
|
|
||||||
|
|
||||||
// Prepare data for sending.
|
|
||||||
rttFsmPtr->timeStamp = rttRecord->TroundA;
|
|
||||||
rttFsmPtr->doppler = rttRecord->dopplerR2;
|
|
||||||
RTT_StageUpdate(rttFsmPtr, RTT_RSP2);
|
|
||||||
}
|
|
||||||
else
|
|
||||||
{
|
|
||||||
DBG_INFO("RTT FSM remain current state [%d].\n", rttFsmPtr->rttStage);
|
|
||||||
}
|
|
||||||
break;
|
|
||||||
case RTT_RSP2:
|
|
||||||
if (torType == L2_TOR_RX)
|
|
||||||
{
|
|
||||||
if (L2_isDataAvail(instPtr) == true)
|
|
||||||
{
|
|
||||||
DBG_INFO("RTT RSP-2 have been received.\n");
|
|
||||||
rttFsmPtr->isR4Valid = true;
|
|
||||||
// Update doppler
|
|
||||||
double doppler = Phy_GetCoarseFreqEst(phyRef);
|
|
||||||
rttRecord->dopplerR2 = (int32_t)doppler;
|
|
||||||
// Update Tr4
|
|
||||||
RTT_GetFrameStart(instPtr, &(rttRecord->Tr4), torType);
|
|
||||||
// Calculate TroundA
|
|
||||||
TimeCtrl_CalculateTimeGap(&(rttRecord->Tt3), &(rttRecord->Tr4), &(rttRecord->TroundA));
|
|
||||||
}
|
|
||||||
else
|
|
||||||
{
|
|
||||||
DBG_WARNING("RTT RSP-1 have not been received [Stage: %d].\n", rttFsmPtr->rttStage);
|
|
||||||
DBG_WARNING("Set Tr4 dopplerR4 and TroundA to dummy value.\n");
|
|
||||||
rttFsmPtr->isR4Valid = false;
|
|
||||||
rttRecord->dopplerR4 = RTT_DOPPLER_NONE;
|
|
||||||
rttRecord->Tr4 = RTT_TIME_NONE;
|
|
||||||
rttRecord->TroundA = RTT_TIME_NONE;
|
|
||||||
}
|
|
||||||
DBG_INFO("dopplerR4:%d.\n", rttRecord->dopplerR4);
|
|
||||||
DBG_INFO("Tr4-timeL:%u.\n", rttRecord->Tr4.frac_part);
|
|
||||||
DBG_INFO("Tr4-timeH:%u.\n", rttRecord->Tr4.int_part);
|
|
||||||
DBG_INFO("TroundA-timeL:%u.\n", rttRecord->TroundA.frac_part);
|
|
||||||
DBG_INFO("TroundA-timeH:%u.\n", rttRecord->TroundA.int_part);
|
|
||||||
RTT_StageUpdate(rttFsmPtr, RTT_REQ1);
|
|
||||||
DBG_INFO("RTT procedure end...\n");
|
|
||||||
}
|
|
||||||
else
|
|
||||||
{
|
|
||||||
DBG_INFO("RTT FSM remain current state [%d].\n", rttFsmPtr->rttStage);
|
|
||||||
}
|
|
||||||
break;
|
|
||||||
default:
|
|
||||||
DBG_ERROR("Unexpected RTT FSM stage [%d].\n", rttFsmPtr->rttStage);
|
|
||||||
break;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
// For Member Node: Rx->Tx->Rx->Tx
|
|
||||||
else
|
|
||||||
{
|
|
||||||
switch (rttFsmPtr->rttStage)
|
|
||||||
{
|
|
||||||
case RTT_REQ1:
|
|
||||||
if (torType == L2_TOR_RX)
|
|
||||||
{
|
|
||||||
if (L2_isDataAvail(instPtr) == true)
|
|
||||||
{
|
|
||||||
DBG_INFO("RTT REQ-1 have been received.\n");
|
|
||||||
DBG_INFO("Preparing data for RTT RSP-1.\n");
|
|
||||||
rttFsmPtr->isR2Valid = true;
|
|
||||||
// Update doppler
|
|
||||||
rttRecord->dopplerR2 = (int32_t)Phy_GetCoarseFreqEst(phyRef);
|
|
||||||
// Update Tr1
|
|
||||||
RTT_GetFrameStart(instPtr, &(rttRecord->Tr1), torType);
|
|
||||||
}
|
|
||||||
else
|
|
||||||
{
|
|
||||||
DBG_WARNING("RTT REQ-1 have not been received [Stage: %d].\n", rttFsmPtr->rttStage);
|
|
||||||
DBG_WARNING("Set dopplerR1 and Tr1 to dummy value.\n");
|
|
||||||
rttFsmPtr->isR1Valid = false;
|
|
||||||
rttRecord->dopplerR1 = RTT_DOPPLER_NONE;
|
|
||||||
rttRecord->Tr1 = RTT_TIME_NONE;
|
|
||||||
}
|
|
||||||
DBG_INFO("dopplerR1:%d.\n", rttRecord->dopplerR1);
|
|
||||||
DBG_INFO("Tr1-timeL:%u.\n", rttRecord->Tr1.frac_part);
|
|
||||||
DBG_INFO("Tr1-timeH:%u.\n", rttRecord->Tr1.int_part);
|
|
||||||
RTT_StageUpdate(rttFsmPtr, RTT_RSP1);
|
|
||||||
}
|
|
||||||
else
|
|
||||||
{
|
|
||||||
DBG_INFO("RTT FSM remain current state [%d].\n", rttFsmPtr->rttStage);
|
|
||||||
}
|
|
||||||
break;
|
|
||||||
case RTT_RSP1:
|
|
||||||
if (torType == L2_TOR_TX)
|
|
||||||
{
|
|
||||||
DBG_INFO("Prepare data for RTT RSP-1\n");
|
|
||||||
// Update Tt2
|
|
||||||
RTT_GetFrameStart(instPtr, &(rttRecord->Tt2), torType);
|
|
||||||
if (rttFsmPtr->isR1Valid)
|
|
||||||
{
|
|
||||||
// Calculate reply B
|
|
||||||
TimeCtrl_CalculateTimeGap(&(rttRecord->Tr1), &(rttRecord->Tt2), &(rttRecord->TreplyB));
|
|
||||||
}
|
|
||||||
else
|
|
||||||
{
|
|
||||||
rttRecord->TreplyB = RTT_TIME_NONE;
|
|
||||||
DBG_WARNING("RTT REQ-1 have not been received [Stage: %d].\n", rttFsmPtr->rttStage);
|
|
||||||
DBG_WARNING("Set ReplayB to dummy value.\n");
|
|
||||||
}
|
|
||||||
DBG_INFO("Tt2-timeL:%u.\n", rttRecord->Tt2.frac_part);
|
|
||||||
DBG_INFO("Tt2-timeH:%u.\n", rttRecord->Tt2.int_part);
|
|
||||||
DBG_INFO("TreplyB-timeL:%u.\n", rttRecord->TreplyB.frac_part);
|
|
||||||
DBG_INFO("TreplyB-timeH:%u.\n", rttRecord->TreplyB.int_part);
|
|
||||||
|
|
||||||
// Prepare data for sending.
|
|
||||||
rttFsmPtr->timeStamp = rttRecord->TreplyB;
|
|
||||||
rttFsmPtr->doppler = rttRecord->dopplerR1;
|
|
||||||
RTT_StageUpdate(rttFsmPtr, RTT_REQ2);
|
|
||||||
}
|
|
||||||
else
|
|
||||||
{
|
|
||||||
DBG_INFO("RTT FSM remain current state [%d].\n", rttFsmPtr->rttStage);
|
|
||||||
}
|
|
||||||
break;
|
|
||||||
case RTT_REQ2:
|
|
||||||
if (torType == L2_TOR_RX)
|
|
||||||
{
|
|
||||||
if (L2_isDataAvail(instPtr) == true)
|
|
||||||
{
|
|
||||||
DBG_INFO("RTT REQ-2 have been received.\n");
|
|
||||||
DBG_INFO("Preparing data for RTT RSP-2.\n");
|
|
||||||
rttFsmPtr->isR3Valid = true;
|
|
||||||
// Update doppler
|
|
||||||
rttRecord->dopplerR2 = (int32_t)Phy_GetCoarseFreqEst(phyRef);
|
|
||||||
// Update Tr3
|
|
||||||
RTT_GetFrameStart(instPtr, &(rttRecord->Tr3), torType);
|
|
||||||
// Calculate TroundB
|
|
||||||
TimeCtrl_CalculateTimeGap(&(rttRecord->Tt2), &(rttRecord->Tr3), &(rttRecord->TroundB));
|
|
||||||
}
|
|
||||||
else
|
|
||||||
{
|
|
||||||
DBG_WARNING("RTT REQ-2 have not been received [Stage: %d].\n", rttFsmPtr->rttStage);
|
|
||||||
DBG_WARNING("Set dopplerR3 and ReplayB to dummy value.\n");
|
|
||||||
rttFsmPtr->isR3Valid = false;
|
|
||||||
rttRecord->dopplerR3 = RTT_DOPPLER_NONE;
|
|
||||||
rttRecord->TroundB = RTT_TIME_NONE;
|
|
||||||
}
|
|
||||||
DBG_INFO("dopplerR3:%d.\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:%u.\n", rttRecord->TroundB.frac_part);
|
|
||||||
DBG_INFO("TroundB-timeH:%u.\n", rttRecord->TroundB.int_part);
|
|
||||||
RTT_StageUpdate(rttFsmPtr, RTT_RSP2);
|
|
||||||
}
|
|
||||||
else
|
|
||||||
{
|
|
||||||
DBG_INFO("RTT FSM remain current state [%d].\n", rttFsmPtr->rttStage);
|
|
||||||
}
|
|
||||||
break;
|
|
||||||
case RTT_RSP2:
|
|
||||||
if (torType == L2_TOR_TX)
|
|
||||||
{
|
|
||||||
DBG_INFO("Prepare data for RTT RSP-2\n");
|
|
||||||
// Update Tt4
|
|
||||||
RTT_GetFrameStart(instPtr, &(rttRecord->Tt4), torType);
|
|
||||||
if (rttFsmPtr->isR3Valid)
|
|
||||||
{
|
|
||||||
// Calculate reply B
|
|
||||||
TimeCtrl_CalculateTimeGap(&(rttRecord->Tr3), &(rttRecord->Tt4), &(rttRecord->TreplyB));
|
|
||||||
// TODO: implement a time adjustment.
|
|
||||||
}
|
|
||||||
else
|
|
||||||
{
|
|
||||||
DBG_WARNING("RTT REQ-2 have not been received [Stage: %d].\n", rttFsmPtr->rttStage);
|
|
||||||
DBG_WARNING("Set ReplayB to dummy value.\n");
|
|
||||||
rttRecord->TreplyB = RTT_TIME_NONE;
|
|
||||||
}
|
|
||||||
DBG_INFO("Tt4-timeL:%u.\n", rttRecord->Tt4.frac_part);
|
|
||||||
DBG_INFO("Tt4-timeH:%u.\n", rttRecord->Tt4.int_part);
|
|
||||||
DBG_INFO("TreplyB-timeL:%u.\n", rttRecord->TreplyB.frac_part);
|
|
||||||
DBG_INFO("TreplyB-timeH:%u.\n", rttRecord->TreplyB.int_part);
|
|
||||||
|
|
||||||
// Prepare data for sending.
|
|
||||||
rttFsmPtr->timeStamp = rttRecord->TreplyB;
|
|
||||||
rttFsmPtr->doppler = rttRecord->dopplerR3;
|
|
||||||
RTT_StageUpdate(rttFsmPtr, RTT_REQ1);
|
|
||||||
DBG_INFO("RTT procedure end...\n");
|
|
||||||
}
|
|
||||||
else
|
|
||||||
{
|
|
||||||
DBG_INFO("RTT FSM remain current state [%d].\n", rttFsmPtr->rttStage);
|
|
||||||
}
|
|
||||||
break;
|
|
||||||
default:
|
|
||||||
DBG_ERROR("Unexpected RTT FSM stage [%d].\n", rttFsmPtr->rttStage);
|
|
||||||
break;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
return SUCCESS;
|
|
||||||
}
|
|
||||||
Reference in New Issue
Block a user