319 lines
11 KiB
C
319 lines
11 KiB
C
/*****************************************************************************/
|
|
/**
|
|
* Description: Implementations of df linklayer scan net.
|
|
*
|
|
* Date: 2021-11-10 19:04
|
|
* LastEditTime: 2022-07-16 17:04
|
|
* Revisions: Rev 0.1
|
|
* History:
|
|
* [2022-07-16]: New branch for DiJian.
|
|
* [2021-11-10]: Initial version.
|
|
*
|
|
* Copyright(c) 2021 Beijing Institute of Technology.
|
|
* Lab of Communication and Networking
|
|
*
|
|
******************************************************************************/
|
|
|
|
/***************************** Include Files *********************************/
|
|
#include <string.h>
|
|
#include "error.h"
|
|
#include "common.h"
|
|
#include "msg_def.h"
|
|
#include "link_layer.h"
|
|
#include "link_layer_types.h"
|
|
#include "../phy_layer/phy_layer.h"
|
|
#include "node_list.h"
|
|
|
|
/************************** Constant Definitions *****************************/
|
|
|
|
/***************** Macros (Inline Functions) Definitions *********************/
|
|
|
|
/**************************** Type Definitions *******************************/
|
|
|
|
/************************** Variable Definitions *****************************/
|
|
|
|
/************************** Function Prototypes ******************************/
|
|
static int32_t ScanMsg_Packing(L2_INST *instPtr, L2_FRAME_TYPE type);
|
|
/*****************************************************************************/
|
|
/**
|
|
* @brief Packing Scan message to a bit array.
|
|
*
|
|
* @param bitarr pointer to bit array buffer.
|
|
* msgType message type.
|
|
*
|
|
* @return EPARAM
|
|
* SUCCESS
|
|
*
|
|
******************************************************************************/
|
|
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(&(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(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(instPtr->txQueue, buff) == false){
|
|
DBG_ERROR("Cannot push packet to queue..\n");
|
|
return EQUEUE;
|
|
}
|
|
break;
|
|
}
|
|
default :{
|
|
DBG_ERROR("Unexpected frame type: %d.\n", type);
|
|
return EPARAM;
|
|
break;
|
|
}
|
|
}
|
|
return SUCCESS;
|
|
}
|
|
|
|
/*****************************************************************************/
|
|
/**
|
|
* @brief Initialize scan FSM.
|
|
*
|
|
* @param instPtr
|
|
*
|
|
* @return
|
|
*
|
|
******************************************************************************/
|
|
int32_t L2_Scan_Init(L2_INST *instPtr) {
|
|
ASSERT(instPtr != NULL);
|
|
instPtr->scanFsm.sacanCnt = 0;
|
|
instPtr->scanFsm.beamNo = 0;
|
|
return SUCCESS;
|
|
}
|
|
|
|
/*****************************************************************************/
|
|
/**
|
|
* @brief Scan net packet handler.
|
|
*
|
|
* @param msg A pointer to frame data buffer.
|
|
*
|
|
* @return
|
|
*
|
|
******************************************************************************/
|
|
int32_t L2_Scan_PktHandler(L2_INST *instPtr, uint8_t *msg) {
|
|
ASSERT(instPtr != NULL);
|
|
ASSERT(msg != NULL);
|
|
int32_t Status;
|
|
switch (instPtr->frame.last) {
|
|
case L2_SCAN_FRAME: {
|
|
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(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 (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(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;
|
|
}
|
|
memcpy(&tmpVal, msg+L2_MSG_INFO_POSX, sizeof(float));
|
|
coordTemp.x = tmpVal;
|
|
memcpy(&tmpVal, msg+L2_MSG_INFO_POSY, sizeof(float));
|
|
coordTemp.y = tmpVal;
|
|
memcpy(&tmpVal, msg+L2_MSG_INFO_POSZ, sizeof(float));
|
|
coordTemp.z = tmpVal;
|
|
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);
|
|
}
|
|
memcpy(&tmpVal, msg+L2_MSG_INFO_SPEEDX, sizeof(float));
|
|
coordTemp.x = tmpVal;
|
|
memcpy(&tmpVal, msg+L2_MSG_INFO_SPEEDY, sizeof(float));
|
|
coordTemp.y = tmpVal;
|
|
memcpy(&tmpVal, msg+L2_MSG_INFO_SPEEDZ, sizeof(float));
|
|
coordTemp.z = tmpVal;
|
|
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(instPtr, &devi);
|
|
if (Status != SUCCESS) {
|
|
DBG_ERROR("Failed to compute slot deviation.\n");
|
|
UpdateNodeTimeDeviation(instPtr->nodeInfoList, L2_MEMBER_NODE_ID, 0);
|
|
}
|
|
else {
|
|
if(devi > INT32_MAX || devi < INT32_MIN) {
|
|
DBG_ERROR("Illegal deviation value: %lld.\n", devi);
|
|
deviTrunc = 0;
|
|
}
|
|
else if(devi > L2_MSG_SVC_TMSYNC_MAX || devi < L2_MSG_SVC_TMSYNC_MIN) {
|
|
DBG_WARNING("Deviation value: %lld is out of adjustable range.\n", devi);
|
|
deviTrunc = (int32_t)devi;
|
|
}
|
|
else {
|
|
deviTrunc = (int32_t)devi;
|
|
}
|
|
UpdateNodeTimeDeviation(instPtr->nodeInfoList, L2_MEMBER_NODE_ID, (uint32_t)deviTrunc);
|
|
}
|
|
break;
|
|
}
|
|
default:
|
|
DBG_ERROR("Unsupported frame type: %ld.\n", instPtr->frame.last);
|
|
Status = EUNEXP;
|
|
break;
|
|
}
|
|
|
|
// Net setup finished.
|
|
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(instPtr, &l2Event);
|
|
|
|
// Assert Routine Table Update...
|
|
newRtneConfig.rtneTable = normalTable;
|
|
newRtneConfig.rtnePrd = ARRAY_SIZE(normalTable)*L2_SLOTS_PER_FRAME;
|
|
newRtneConfig.rtneStep = 0;
|
|
l2Event.event = L2_ROUTINE_UPDATE;
|
|
l2Event.msg = &newRtneConfig;
|
|
AssertL2Event(instPtr, &l2Event);
|
|
}
|
|
return Status;
|
|
}
|
|
|
|
/*****************************************************************************/
|
|
/**
|
|
* @brief Execute a member node scan net procedure.
|
|
*
|
|
* @param instPtr A pointer to scan net fsm.
|
|
*
|
|
* @return used slot (5ms slot).
|
|
*
|
|
******************************************************************************/
|
|
int32_t L2_Scan_Proc(L2_INST *instPtr) {
|
|
int32_t Status;
|
|
/* 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 (instPtr->nextState == NORMAL_STATE) {
|
|
DBG_INFO("NETSET DONE, halt 20ms before normal routine.\n");
|
|
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");
|
|
}
|
|
return NUM_SLOTS_SKIP;
|
|
}
|
|
|
|
/* Head node implement a periodical scan pattern
|
|
* | Tx 20ms | Rx 100ms | Tx 20ms | Rx 100ms ... ...
|
|
* Send pilot signal in Tx slots with narrow beam width.
|
|
* Wait for response in Rx slots with the same beam width and beam number.
|
|
* Increase beam number after a Tx-Rx procedure pair, reset beam number when
|
|
* it reaches the boundary.
|
|
*/
|
|
if (instPtr->config.myType == L2_HEAD_NODE) {
|
|
// Apply a scan procedure
|
|
if (instPtr->scanFsm.sacanCnt % 2 == 0){
|
|
// Antenna configure
|
|
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(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
|
|
instPtr->scanFsm.beamNo = (instPtr->scanFsm.beamNo + 1) % L1_NUM_ANT_NRRW_PATTERN;
|
|
}
|
|
}
|
|
/* Member node listening the head node pilot signal with wide beam width.
|
|
* Once is received a pilot signal, physical layer will perform a response
|
|
* procedure automatically, witch should contains the basic node information
|
|
* (position, speed, etc.).
|
|
* So that, a extra datagram with the up-to-date node information is attached
|
|
* to the listening configuration.
|
|
*/
|
|
else {
|
|
uint8_t *sendData;
|
|
// Pack node information.
|
|
Status = ScanMsg_Packing(instPtr, L2_INFO_FRAME);
|
|
if (Status != SUCCESS) {
|
|
DBG_INFO("Failed to pack response.\n");
|
|
}
|
|
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(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).
|
|
instPtr->scanFsm.sacanCnt ++;
|
|
return NUM_SLOTS_SKIP;
|
|
}
|
|
|
|
/*****************************************************************************/
|
|
|
|
/**
|
|
* @brief Execute a scan net Routine procedure.
|
|
*
|
|
* @param schedEntry Routine configuration
|
|
*
|
|
* @return used slot (5ms slot).
|
|
*
|
|
******************************************************************************/
|
|
int32_t L2_Scan_Routine(L2_INST *instPtr, L2_RTNE_TABLE_ENTRY *schedEntry) {
|
|
DBG_INFO("-------- Scan Routine --------\n");
|
|
ASSERT(false); // Not support.
|
|
return 0;
|
|
}
|