Add new frequency estimation.

datalink814_ge/src/linklayer/rtt.c

@@ -224,7 +224,7 @@ int32_t RTT_FsmUpdate(L2_INST *instPtr, L2_TOR_TYPE torType)
 					DBG_INFO("Preparing data for RTT REQ-2.\n");
 					rttFsmPtr->isR2Valid = true;
 					// Update doppler
-					rttRecord->dopplerR2 = (int32_t)Phy_GetCoarseFreqEst(phyRef);
+					rttRecord->dopplerR2 = (int32_t)Phy_GetFreqEst(phyRef);
 					// Update Tr2
 					RTT_GetFrameStart(instPtr, &(rttRecord->Tr2), torType);
 					// Calculate TroundA
@@ -292,7 +292,7 @@ int32_t RTT_FsmUpdate(L2_INST *instPtr, L2_TOR_TYPE torType)
 					DBG_INFO("RTT RSP-2 have been received.\n");
 					rttFsmPtr->isR4Valid = true;
 					// Update doppler
-					double doppler = Phy_GetCoarseFreqEst(phyRef);
+					double doppler = Phy_GetFreqEst(phyRef);
 					rttRecord->dopplerR2 = (int32_t)doppler;
 					// Update Tr4
 					RTT_GetFrameStart(instPtr, &(rttRecord->Tr4), torType);
@@ -340,7 +340,7 @@ int32_t RTT_FsmUpdate(L2_INST *instPtr, L2_TOR_TYPE torType)
 					DBG_INFO("Preparing data for RTT RSP-1.\n");
 					rttFsmPtr->isR2Valid = true;
 					// Update doppler
-					rttRecord->dopplerR2 = (int32_t)Phy_GetCoarseFreqEst(phyRef);
+					rttRecord->dopplerR2 = (int32_t)Phy_GetFreqEst(phyRef);
 					// Update Tr1
 					RTT_GetFrameStart(instPtr, &(rttRecord->Tr1), torType);
 				}
@@ -403,7 +403,7 @@ int32_t RTT_FsmUpdate(L2_INST *instPtr, L2_TOR_TYPE torType)
 					DBG_INFO("Preparing data for RTT RSP-2.\n");
 					rttFsmPtr->isR3Valid = true;
 					// Update doppler
-					rttRecord->dopplerR2 = (int32_t)Phy_GetCoarseFreqEst(phyRef);
+					rttRecord->dopplerR2 = (int32_t)Phy_GetFreqEst(phyRef);
 					// Update Tr3
 					RTT_GetFrameStart(instPtr, &(rttRecord->Tr3), torType);
 					// Calculate TroundB

datalink814_ge/src/phy_ctrl/phy_ctrl.c

@@ -156,11 +156,27 @@ inline L1_FRAME_TYPE Phy_GetLastRecvFrameType(PHY_INST *instPtr)
 	{
 		return L1_NONE_FRAME;
 	}
-	int32_t frame_type = (PhyRead(instPtr, PHY_MODE_CTRL) & PHY_MODE_CTRL_FRAME_MASK) >> PHY_MODE_CTRL_FRAME_SHIFT;
+	int32_t frame_type = (PhyRead(instPtr, COASE_FREQEST) & COASE_TYPE_MASK);
-	if (frame_type < L1_1M2BPS_5MS_FRAME)
+	switch (frame_type) {
-		return frame_type;
+		case COASE_TYPE_2KBPS_20MS:
-	else
+		return L1_2KBPS_20MS_FRAME;
+		case COASE_TYPE_2KBPS_100MS:
+		return L1_2KBPS_100MS_FRAME;
+		case COASE_TYPE_10KBPS_10MS:
+		return L1_10KBPS_10MS_FRAME;
+		case COASE_TYPE_10KBPS_20MS:
+		return L1_10KBPS_20MS_FRAME;
+		case COASE_TYPE_40KBPS_20MS:
+		return L1_40KBPS_20MS_FRAME;
+		case COASE_TYPE_40KBPS_10MS:
+		return L1_40KBPS_10MS_FRAME;
+		case COASE_TYPE_200KBPS_5MS:
+		return L1_200KBPS_5MS_FRAME;
+		case COASE_TYPE_1M2BPS_5MS:
+		return L1_1M2BPS_5MS_FRAME;
+		default:
 		return L1_NONE_FRAME;
+	}
 }
 
 /*****************************************************************************/
@@ -175,7 +191,7 @@ inline L1_FRAME_TYPE Phy_GetLastRecvFrameType(PHY_INST *instPtr)
 inline L1_FRAME_TYPE Phy_GetLastSendFrameType(PHY_INST *instPtr)
 {
 	ASSERT(instPtr != NULL);
-	if (instPtr->TOR.last != PHY_TOR_TX)
+	if (instPtr->tor.last != PHY_TOR_TX)
 	{
 		return L1_NONE_FRAME;
 	}
@@ -539,9 +555,6 @@ int32_t Phy_SetupNextSlot(PHY_INST *instPtr, PHY_TOR_TYPE txrxMode,
 	instPtr->antConfigOffset = (L1_ANTCONFIG_MAX_LEN >> 1) - instPtr->antConfigOffset;
 	uint32_t antConfigAddr = instPtr->antConfigAddr + instPtr->antConfigOffset;
 
-	instPtr->TOR.next = txrxMode;
-	instPtr->frameType.next = frameType;
-
 	switch (txrxMode)
 	{
 	case PHY_TOR_SUSPEND:
@@ -723,6 +736,59 @@ uint32_t Phy_GetTimeComp(L1_FRAME_TYPE frameType)
 	}
 }
 
+/*****************************************************************************/
+/**
+ * @brief   Get frequency estimation of last valid frame.
+ *
+ * @param   instPtr  a pointer to physical instance
+ *
+ * @return  frequency estimation
+ *
+ ******************************************************************************/
+double Phy_GetFreqEst(PHY_INST *instPtr)
+{
+	ASSERT(instPtr != NULL);
+	uint32_t coarseReg, fineReg, estAbs;
+	uint8_t acqFreqIdx;
+	int8_t syncFreqIdx;
+	L1_FRAME_TYPE frameType;
+	double coarse, fine;
+
+	
+	frameType = Phy_GetLastRecvFrameType(instPtr);
+	if (frameType > L1_1M2BPS_5MS_FRAME || frameType <= L1_NONE_FRAME)
+	{
+		return INFINITY; // FIXME
+	}
+
+	coarseReg = PhyRead(instPtr, COASE_FREQEST);
+	acqFreqIdx = (coarseReg & COASE_FREQEST_ACQ_MASK) >> COASE_FREQEST_ACQ_SHIFT;
+	syncFreqIdx = (coarseReg & COASE_FREQEST_SYNC_MASK) >> COASE_FREQEST_SYNC_SHIFT;
+	fineReg = PhyRead(instPtr, FINE_FREQEST);
+	estAbs = fineReg & FINE_FREQEST_VAL_MASK;
+	if (frameType >= L1_200KBPS_5MS_FRAME)
+	{
+		// 1.5M Rc
+		coarse = (double)(80 - acqFreqIdx) * (31.25e3 / 4);
+		coarse += (double)syncFreqIdx * 1.5e6 / 64 / 128;
+
+		fine = 12e6/2048*((double)estAbs)/8192.0;
+	}
+	else
+	{
+		// 12M Rc
+		coarse = (double)(8 - acqFreqIdx) * (250e3 / 4);
+		coarse += (double)syncFreqIdx * 12e6 / 64 / 128;
+
+		fine = 48e6/2048*((double)estAbs)/8192.0;
+	}
+
+	fine = ((fineReg & FINE_FREQEST_SIGN_MASK) == FINE_FREQEST_SIGN_NEG) ? 
+			-fine : fine;
+	
+	return (coarse + fine);
+}
+
 /*****************************************************************************/
 /**
  * @brief   Get coarse frequency estimation of last valid frame.
@@ -742,7 +808,7 @@ double Phy_GetCoarseFreqEst(PHY_INST *instPtr)
 	double freqEst;
 
 	frameType = Phy_GetLastRecvFrameType(instPtr);
-	if (frameType > L1_1M2BPS_5MS_FRAME || frameType == L1_NONE_FRAME)
+	if (frameType > L1_1M2BPS_5MS_FRAME || frameType <= L1_NONE_FRAME)
 	{
 		return INFINITY; // FIXME
 	}
@@ -776,8 +842,30 @@ double Phy_GetCoarseFreqEst(PHY_INST *instPtr)
  ******************************************************************************/
 double Phy_GetFineFreqEst(PHY_INST *instPtr)
 {
-	// TODO:
+	uint32_t regValue, estAbs;
-	return 0.0;
+	double freqEst;
+	L1_FRAME_TYPE frameType;
+	frameType = Phy_GetLastRecvFrameType(instPtr);
+	if (frameType > L1_1M2BPS_5MS_FRAME || frameType <= L1_NONE_FRAME)
+	{
+		return INFINITY; // FIXME
+	}
+
+	regValue = PhyRead(instPtr, FINE_FREQEST);
+	estAbs = regValue & FINE_FREQEST_VAL_MASK;
+	if (frameType >= L1_200KBPS_5MS_FRAME)
+	{
+		// 1.5M Rc
+		freqEst = 12e6/2048*((double)estAbs)/8192.0;
+	}
+	else
+	{
+		// 12M Rc
+		freqEst = 48e6/2048*((double)estAbs)/8192.0;
+	}
+	freqEst = ((regValue & FINE_FREQEST_SIGN_MASK) == FINE_FREQEST_SIGN_NEG) ? 
+			-freqEst : freqEst;
+	return freqEst;
 }
 
 /*****************************************************************************/

datalink814_ge/src/phy_ctrl/phy_ctrl.h

@@ -275,7 +275,9 @@ int32_t Phy_SetBeamNo(PHY_INST *instPtr, int32_t beamNo);
 int32_t Phy_SetBeamAngle(PHY_INST *phyInstPtr, double offAx, double rndAx);
 int32_t Phy_SetTimeWord(PHY_INST *instPtr, const TIME_WORD *tmWord, PHY_TIME_WORD_TYPE tmType);
 
+double Phy_GetFreqEst(PHY_INST *instPtr);
 double Phy_GetCoarseFreqEst(PHY_INST *instPtr);
+double Phy_GetFineFreqEst(PHY_INST *instPtr);
 void Phy_GetFrameSyncTimeStamp(PHY_INST *instPtr, TIME_WORD *frameSyncT);
 void Phy_GetLastReceivedFrameSyncTimeStamp(PHY_INST *instPtr, TIME_WORD *frameSyncT);
 void Phy_GetSlotStartTimeStamp(PHY_INST *instPtr, TIME_WORD *slotStart);

datalink814_ge/src/phy_ctrl/phy_reg.h

@@ -228,15 +228,31 @@
 #define COASE_FREQEST_ACQ_MASK 0x000000FFU
 #define COASE_FREQEST_SYNC_SHIFT 8
 #define COASE_FREQEST_SYNC_WIDTH 8
-#define COASE_FREQEST_SYNC_MASK 0x000FF00U
+#define COASE_FREQEST_SYNC_MASK 0x0000FF00U
+#define COASE_TYPE_SHIFT 16
+#define COASE_TYPE_WIDTH 8
+#define COASE_TYPE_MASK 0x00FF0000U
+#define COASE_TYPE_2KBPS_20MS  0x00010000U
+#define COASE_TYPE_2KBPS_100MS 0x00020000U
+#define COASE_TYPE_10KBPS_10MS 0x00030000U
+#define COASE_TYPE_10KBPS_20MS 0x00040000U
+#define COASE_TYPE_40KBPS_20MS 0x00050000U
+#define COASE_TYPE_40KBPS_10MS 0x00060000U
+#define COASE_TYPE_200KBPS_5MS 0x00070000U
+#define COASE_TYPE_1M2BPS_5MS  0x00080000U
 
 /**
  * Register: FINE_FREQEST
  */
 #define FINE_FREQEST 0x00000040U
-#define FINE_FREQEST_SHIFT 0
+#define FINE_FREQEST_VAL_SHIFT 0
-#define FINE_FREQEST_WIDTH 32
+#define FINE_FREQEST_VAL_WIDTH 24
-#define FINE_FREQEST_MASK 0xFFFFFFFFU
+#define FINE_FREQEST_VAL_MASK 0x00FFFFFFU
+#define FINE_FREQEST_SIGN_SHIFT 24
+#define FINE_FREQEST_SIGN_WIDTH 8
+#define FINE_FREQEST_SIGN_MASK 0xFF000000U
+#define FINE_FREQEST_SIGN_POS 0x00000000U
+#define FINE_FREQEST_SIGN_NEG 0x01000000U
 
 /**
  * Register: BEAM_SELECT

datalink814_sp/src/inter_comm/inter_comm.c

@@ -713,7 +713,7 @@ int32_t InterComm_PhyDataUpload(INTER_COMM_INST* instPtr){
 	memcpy(txMsgBuff+DSEGRD_TIME_OFFSET + DSEGRD_TIME_LEN/2, &fsTimeH, DSEGRD_TIME_LEN/2);
 
 	// Doppler Estimate
-	double doppler = Phy_GetCoarseFreqEst(instPtr->phyInstPtr);
+	double doppler = Phy_GetFreqEst(instPtr->phyInstPtr);
 	// LSB = 0.001Hz
 	int64_t dopplerQnt = doppler*1000;
 	memcpy(txMsgBuff+DSEGRD_DPL_OFFSET, &dopplerQnt, DSEGRD_DPL_LEN);