first commit, new repo branches from datalink_repo
This commit is contained in:
Gatsby Van
@@ -0,0 +1,141 @@
|
||||
/*****************************************************************************/
|
||||
/**
|
||||
* Description: Functions for framing operations.
|
||||
*
|
||||
* Date: 2022-07-16 14:17
|
||||
* LastEditors: Fan Hao
|
||||
* LastEditTime: 2022-07-16 17:11
|
||||
* Revisions: Rev 0.0
|
||||
* History:
|
||||
* [2022-07-16]: Description of modification.
|
||||
*
|
||||
* Copyright(c) 2022 Beijing Institute of Technology.
|
||||
* Lab of Communication and Networking
|
||||
*
|
||||
******************************************************************************/
|
||||
|
||||
/***************************** Include Files *********************************/
|
||||
#include <stdint.h>
|
||||
#include "error.h"
|
||||
#include "common.h"
|
||||
#include "msg_def.h"
|
||||
#include "link_layer.h"
|
||||
#include "../config/app_config.h"
|
||||
|
||||
/************************** Constant Definitions *****************************/
|
||||
|
||||
/*****************************************************************************/
|
||||
/**
|
||||
* @brief Generate crc of specified data.
|
||||
*
|
||||
* @param dataBuff pointer to data buffer.
|
||||
* @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
|
||||
*
|
||||
******************************************************************************/
|
||||
static int32_t L2_CrcGen(uint8_t *dataBuff, uint32_t dataLen, L2_CRC_TYPE crcType, uint32_t *crc)
|
||||
{
|
||||
ASSERT(dataBuff != NULL);
|
||||
int32_t Status;
|
||||
switch (crcType)
|
||||
{
|
||||
case L2_CRC16_1021:
|
||||
Status = crc16_gen(dataBuff, dataLen, 0x1021, (uint16_t *)crc);
|
||||
break;
|
||||
case L2_CRC16_8005:
|
||||
Status = crc16_gen(dataBuff, dataLen, 0x8005, (uint16_t *)crc);
|
||||
break;
|
||||
case L2_CRC8_31:
|
||||
Status = crc8_gen(dataBuff, dataLen, 0x31, (uint8_t *)crc);
|
||||
break;
|
||||
default:
|
||||
Status = EPARAM;
|
||||
break;
|
||||
}
|
||||
return Status;
|
||||
}
|
||||
|
||||
/*****************************************************************************/
|
||||
/**
|
||||
* @brief Append a crc of specified data, to a specified position.
|
||||
*
|
||||
* @param dataBuff pointer to data buffer.
|
||||
* @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
|
||||
*
|
||||
******************************************************************************/
|
||||
int32_t L2_CrcAppend(uint8_t *dataBuff, uint32_t dataLen, uint32_t crcPos, L2_CRC_TYPE crcType)
|
||||
{
|
||||
ASSERT(dataBuff != NULL);
|
||||
int32_t Status;
|
||||
uint32_t crcVal;
|
||||
uint8_t *crcPtr;
|
||||
crcPtr = dataBuff + crcPos;
|
||||
Status = L2_CrcGen(dataBuff, dataLen, crcType, &crcVal);
|
||||
if (Status != SUCCESS)
|
||||
{
|
||||
return Status;
|
||||
}
|
||||
switch (crcType)
|
||||
{
|
||||
case L2_CRC16_1021:
|
||||
*(crcPtr + 1) = (uint8_t)(crcVal >> 8);
|
||||
*crcPtr = (uint8_t)(crcVal);
|
||||
break;
|
||||
case L2_CRC16_8005:
|
||||
*(crcPtr + 1) = (uint8_t)(crcVal >> 8);
|
||||
*crcPtr = (uint8_t)(crcVal);
|
||||
break;
|
||||
case L2_CRC8_31:
|
||||
*crcPtr = (uint8_t)crcVal;
|
||||
break;
|
||||
default:
|
||||
Status = EPARAM;
|
||||
break;
|
||||
}
|
||||
return Status;
|
||||
}
|
||||
|
||||
/*****************************************************************************/
|
||||
/**
|
||||
* @brief Check data validation by crc.
|
||||
*
|
||||
* @param dataBuff pointer to data buffer.
|
||||
* @param dataLen length of specified data.
|
||||
* @param crcType type of crc
|
||||
*
|
||||
* @return FAILURE crc type not support.
|
||||
* SUCCESS
|
||||
*
|
||||
******************************************************************************/
|
||||
bool L2_CrcCheck(uint8_t *dataBuff, uint32_t dataLen, L2_CRC_TYPE crcType)
|
||||
{
|
||||
ASSERT(dataBuff != NULL);
|
||||
int32_t Status;
|
||||
uint32_t crcVal;
|
||||
crcVal = 0;
|
||||
Status = L2_CrcGen(dataBuff, dataLen, crcType, &crcVal);
|
||||
if (Status == SUCCESS)
|
||||
{
|
||||
if (crcVal == 0)
|
||||
{
|
||||
return true;
|
||||
}
|
||||
else
|
||||
{
|
||||
return false;
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
return false;
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,211 @@
|
||||
//
|
||||
// Copyright (C) 2020 OpenSim Ltd.
|
||||
//
|
||||
// SPDX-License-Identifier: LGPL-3.0-or-later
|
||||
//
|
||||
|
||||
#include <math.h>
|
||||
#include "coord.h"
|
||||
|
||||
const COORD COORD_NIL = {NAN, NAN, NAN};
|
||||
const COORD COORD_ZERO = {0.0, 0.0, 0.0};
|
||||
const COORD COORD_ONE = {1.0, 1.0, 1.0};
|
||||
const COORD COORD_X_AXIS = {1.0, 0.0, 0.0};
|
||||
const COORD COORD_Y_AXIS = {0.0, 1.0, 0.0};
|
||||
const COORD COORD_Z_AXIS = {0.0, 0.0, 1.0};
|
||||
|
||||
/** @brief Subtracts two coordinate vectors. */
|
||||
COORD Coord_Sub(COORD a, COORD b)
|
||||
{
|
||||
COORD ret;
|
||||
ret.x = a.x - b.x;
|
||||
ret.y = a.y - b.y;
|
||||
ret.z = a.z - b.z;
|
||||
return ret;
|
||||
}
|
||||
|
||||
/** @brief Adds two coordinate vectors. */
|
||||
COORD Coord_Add(COORD a, COORD b)
|
||||
{
|
||||
COORD ret;
|
||||
ret.x = a.x + b.x;
|
||||
ret.y = a.y + b.y;
|
||||
ret.z = a.z + b.z;
|
||||
return ret;
|
||||
}
|
||||
|
||||
/** @brief Multiplies a coordinate vector by a real number. */
|
||||
void Coord_ScalarMult(COORD *a, double t)
|
||||
{
|
||||
a->x *= t;
|
||||
a->y *= t;
|
||||
a->z *= t;
|
||||
}
|
||||
|
||||
/** @brief Multiplies a coordinate vector by a real number. */
|
||||
COORD Coord_GetScalarMult(COORD a, double t)
|
||||
{
|
||||
COORD ret;
|
||||
ret.x = a.x * t;
|
||||
ret.y = a.y * t;
|
||||
ret.z = a.z * t;
|
||||
return ret;
|
||||
}
|
||||
|
||||
/** @brief Divides a coordinate vector by a real number. */
|
||||
void Coord_ScalarDiv(COORD *a, double t)
|
||||
{
|
||||
a->x /= t;
|
||||
a->y /= t;
|
||||
a->z /= t;
|
||||
}
|
||||
|
||||
/** @brief Divides a coordinate vector by a real number. */
|
||||
COORD Coord_GetScalarDiv(COORD a, double t)
|
||||
{
|
||||
COORD ret;
|
||||
ret.x = a.x / t;
|
||||
ret.y = a.y / t;
|
||||
ret.z = a.z / t;
|
||||
return ret;
|
||||
}
|
||||
|
||||
/** @brief Dot product. */
|
||||
inline double Coord_Dot(COORD a, COORD b)
|
||||
{
|
||||
return (a.x * b.x + a.y * b.y + a.z * b.z);
|
||||
}
|
||||
|
||||
/** @brief Cross product. */
|
||||
COORD Coord_Cross(COORD a, COORD b)
|
||||
{
|
||||
COORD ret;
|
||||
ret.x = a.y * b.z - a.z * b.y;
|
||||
ret.y = a.z * b.x - a.x * b.z;
|
||||
ret.z = a.x * b.y - a.y * b.x;
|
||||
return ret;
|
||||
}
|
||||
|
||||
/** @brief Multiplies two coordinate vector in elementwise. */
|
||||
COORD Coord_ElementwiseMult(COORD a, COORD b)
|
||||
{
|
||||
COORD ret;
|
||||
ret.x = a.x * b.x;
|
||||
ret.y = a.y * b.y;
|
||||
ret.z = a.z * b.z;
|
||||
return ret;
|
||||
}
|
||||
|
||||
/** @brief Divides two coordinate vector in elementwise. */
|
||||
COORD Coord_ElementwiseDiv(COORD a, COORD b)
|
||||
{
|
||||
COORD ret;
|
||||
ret.x = a.x / b.x;
|
||||
ret.y = a.y / b.y;
|
||||
ret.z = a.z / b.z;
|
||||
return ret;
|
||||
}
|
||||
|
||||
/** @brief Calculate a coordinate vector length^2 (omits calling square root). */
|
||||
double Coord_SquareLength(COORD a)
|
||||
{
|
||||
return (a.x * a.x + a.y * a.y + a.z * a.z);
|
||||
}
|
||||
|
||||
/** @brief Calculate a coordinate vector length. */
|
||||
double Coord_Length(COORD a)
|
||||
{
|
||||
return sqrt(Coord_SquareLength(a));
|
||||
}
|
||||
|
||||
/** @brief Calculate two coordinates' distance. */
|
||||
double Coord_Distance(COORD a, COORD b)
|
||||
{
|
||||
return Coord_Length(Coord_Sub(a, b));
|
||||
}
|
||||
|
||||
/** @brief Normalize coordinate vector. */
|
||||
void Coord_Normalize(COORD *a)
|
||||
{
|
||||
Coord_ScalarDiv(a, Coord_Length(*a));
|
||||
}
|
||||
|
||||
/** @brief Normalize coordinate vector. */
|
||||
COORD Coord_GetNormalized(COORD a)
|
||||
{
|
||||
Coord_Normalize(&a);
|
||||
return a;
|
||||
}
|
||||
|
||||
/** @brief Returns the angle between the two vectors. */
|
||||
double Coord_Angle(COORD a, COORD b)
|
||||
{
|
||||
Coord_Normalize(&a);
|
||||
Coord_Normalize(&b);
|
||||
return acos(Coord_Dot(a, b));
|
||||
}
|
||||
|
||||
// Additional Coordinates convert
|
||||
// TODO: Confirm correctness
|
||||
/** @brief Convert coordinate from Launch coordinate system to ECEF. */
|
||||
COORD Coord_Launch2Ecef(COORD o, COORD p, double A, double B, double lambda)
|
||||
{
|
||||
COORD ret;
|
||||
double sinA, cosA, sinB, cosB, sinL, cosL;
|
||||
double S11, S12, S13, S21, S22, S23, S31, S32, S33;
|
||||
sinA = sin(A);
|
||||
cosA = cos(A);
|
||||
sinB = sin(B);
|
||||
cosB = cos(B);
|
||||
sinL = sin(lambda);
|
||||
cosL = cos(lambda);
|
||||
|
||||
S11 = -sinA * sinL - cosA * sinB * cosL;
|
||||
S12 = cosB * cosL;
|
||||
S13 = -cosA * sinL + sinA * sinB * cosL;
|
||||
|
||||
S21 = sinA * cosL - cosA * sinB * sinL;
|
||||
S22 = cosB * sinL;
|
||||
S23 = cosA * cosL + sinA * sinB * sinL;
|
||||
|
||||
S31 = cosA * cosB;
|
||||
S32 = sinB;
|
||||
S33 = -sinA * cosB;
|
||||
|
||||
ret.x = p.x * (S11 + S21 + S31) + o.x;
|
||||
ret.y = p.y * (S12 + S22 + S32) + o.y;
|
||||
ret.z = p.z * (S13 + S23 + S33) + o.z;
|
||||
|
||||
return ret;
|
||||
}
|
||||
|
||||
/** @brief Convert coordinate from launch coordinate system to Inertia coordinate system. */
|
||||
COORD Coord_Launch2Inertia(COORD o, COORD p, double A, double B, double we, double t)
|
||||
{
|
||||
COORD ret;
|
||||
double OmgX, OmgY, OmgZ, sinwet, coswet;
|
||||
double M11, M12, M13, M21, M22, M23, M31, M32, M33;
|
||||
OmgX = cos(A) * cos(B);
|
||||
OmgY = sin(B);
|
||||
OmgZ = -sin(A) * cos(B);
|
||||
sinwet = sin(we * t);
|
||||
coswet = cos(we * t);
|
||||
|
||||
M11 = 1 - (1 - OmgX * OmgX) * (1 - coswet);
|
||||
M12 = OmgX * OmgY * (1 - coswet) + OmgZ * sinwet;
|
||||
M13 = OmgX * OmgZ * (1 - coswet) - OmgY * sinwet;
|
||||
|
||||
M21 = OmgY * OmgX * (1 - coswet) - OmgZ * sinwet;
|
||||
M22 = 1 - (1 - OmgY * OmgY) * (1 - coswet);
|
||||
M23 = OmgY * OmgZ * (1 - coswet) + OmgX * sinwet;
|
||||
|
||||
M31 = OmgZ * OmgX * (1 - coswet) + OmgY * sinwet;
|
||||
M32 = OmgZ * OmgY * (1 - coswet) - OmgX * sinwet;
|
||||
M33 = 1 - (1 - OmgZ * OmgZ) * (1 - coswet);
|
||||
|
||||
ret.x = p.x * (M11 + M21 + M31) + o.x;
|
||||
ret.y = p.y * (M12 + M22 + M32) + o.y;
|
||||
ret.z = p.z * (M13 + M23 + M33) + o.z;
|
||||
|
||||
return ret;
|
||||
}
|
||||
@@ -0,0 +1,80 @@
|
||||
//
|
||||
// Copyright (C) 2020 OpenSim Ltd.
|
||||
//
|
||||
// SPDX-License-Identifier: LGPL-3.0-or-later
|
||||
//
|
||||
|
||||
#ifndef _COORD_H
|
||||
#define _COORD_H
|
||||
|
||||
/**
|
||||
* @brief Structure for storing 3D coordinates.
|
||||
*
|
||||
* Some comparison and basic arithmetic operators are implemented.
|
||||
*
|
||||
* @ingroup utils
|
||||
*/
|
||||
|
||||
typedef struct
|
||||
{
|
||||
double x;
|
||||
double y;
|
||||
double z;
|
||||
} COORD;
|
||||
|
||||
extern const COORD COORD_NIL;
|
||||
extern const COORD COORD_ZERO;
|
||||
extern const COORD COORD_ONE;
|
||||
extern const COORD COORD_X_AXIS;
|
||||
extern const COORD COORD_Y_AXIS;
|
||||
extern const COORD COORD_Z_AXIS;
|
||||
|
||||
/** @brief Subtracts two coordinate vectors. */
|
||||
COORD Coord_Sub(COORD a, COORD b);
|
||||
|
||||
/** @brief Adds two coordinate vectors. */
|
||||
COORD Coord_Add(COORD a, COORD b);
|
||||
|
||||
/** @brief Multiplies a coordinate vector by a real number. */
|
||||
void Coord_ScalarMult(COORD *a, double t);
|
||||
|
||||
/** @brief Multiplies a coordinate vector by a real number. */
|
||||
COORD Coord_GetScalarMult(COORD a, double t);
|
||||
|
||||
/** @brief Divides a coordinate vector by a real number. */
|
||||
void Coord_ScalarDiv(COORD *a, double t);
|
||||
|
||||
/** @brief Divides a coordinate vector by a real number. */
|
||||
COORD Coord_GetScalarDiv(COORD a, double t);
|
||||
|
||||
/** @brief Dot product. */
|
||||
double Coord_Dot(COORD a, COORD b);
|
||||
|
||||
/** @brief Cross product. */
|
||||
COORD Coord_Cross(COORD a, COORD b);
|
||||
|
||||
/** @brief Multiplies two coordinate vector in elementwise. */
|
||||
COORD Coord_ElementwiseMult(COORD a, COORD b);
|
||||
|
||||
/** @brief Divides two coordinate vector in elementwise. */
|
||||
COORD Coord_ElementwiseDiv(COORD a, COORD b);
|
||||
|
||||
/** @brief Calculate a coordinate vector length^2 (omits calling square root). */
|
||||
double Coord_SquareLength(COORD a);
|
||||
|
||||
/** @brief Calculate a coordinate vector length. */
|
||||
double Coord_Length(COORD a);
|
||||
|
||||
/** @brief Calculate two coordinates' distance. */
|
||||
double Coord_Distance(COORD a, COORD b);
|
||||
|
||||
/** @brief Normalize coordinate vector. */
|
||||
void Coord_Normalize(COORD *a);
|
||||
|
||||
/** @brief Normalize coordinate vector. */
|
||||
COORD Coord_GetNormalized(COORD a);
|
||||
|
||||
/** @brief Returns the angle between the two vectors. */
|
||||
double Coord_Angle(COORD a, COORD b);
|
||||
|
||||
#endif
|
||||
@@ -0,0 +1,61 @@
|
||||
//
|
||||
// Copyright (C) 2020 OpenSim Ltd.
|
||||
//
|
||||
// SPDX-License-Identifier: LGPL-3.0-or-later
|
||||
//
|
||||
|
||||
#include <math.h>
|
||||
#include "eulerangles.h"
|
||||
|
||||
const EULERANGLES EULERANGLES_NIL = {NAN, NAN, NAN};
|
||||
const EULERANGLES EULERANGLES_ZERO = {0.0, 0.0, 0.0};
|
||||
|
||||
/** @brief Add two EulerAngles. */
|
||||
EULERANGLES EulerAngles_Add(EULERANGLES e1, EULERANGLES e2)
|
||||
{
|
||||
EULERANGLES ret;
|
||||
ret.alpha = e1.alpha + e2.alpha;
|
||||
ret.beta = e1.beta + e2.beta;
|
||||
ret.gamma = e1.gamma + e2.gamma;
|
||||
return ret;
|
||||
}
|
||||
|
||||
/** @brief Subtracts two EulerAngles. */
|
||||
EULERANGLES EulerAngles_Sub(EULERANGLES e1, EULERANGLES e2)
|
||||
{
|
||||
EULERANGLES ret;
|
||||
ret.alpha = e1.alpha - e2.alpha;
|
||||
ret.beta = e1.beta - e2.beta;
|
||||
ret.gamma = e1.gamma - e2.gamma;
|
||||
return ret;
|
||||
}
|
||||
|
||||
/** @brief Multiplies EulerAngles by a real number. */
|
||||
void EulerAngles_ScalarMult(EULERANGLES *e, double f)
|
||||
{
|
||||
e->alpha *= f;
|
||||
e->beta *= f;
|
||||
e->gamma *= f;
|
||||
}
|
||||
|
||||
/** @brief Multiplies EulerAngles by a real number. */
|
||||
EULERANGLES EulerAngles_GetScalarMult(EULERANGLES e, double f)
|
||||
{
|
||||
EulerAngles_ScalarMult(&e, f);
|
||||
return e;
|
||||
}
|
||||
|
||||
/** @brief Normalize EulerAngles. */
|
||||
void EulerAngles_Normalize(EULERANGLES *e)
|
||||
{
|
||||
e->alpha = fmod(e->alpha, 2 * EUL_PI);
|
||||
e->beta = fmod(e->beta, 2 * EUL_PI);
|
||||
e->gamma = fmod(e->gamma, 2 * EUL_PI);
|
||||
}
|
||||
|
||||
/** @brief Normalize EulerAngles. */
|
||||
EULERANGLES EulerAngles_GetNormalize(EULERANGLES e)
|
||||
{
|
||||
EulerAngles_Normalize(&e);
|
||||
return e;
|
||||
}
|
||||
@@ -0,0 +1,55 @@
|
||||
#ifndef __EULERANGLES_H
|
||||
#define __EULERANGLES_H
|
||||
|
||||
#define EUL_PI 3.14159265358979323846
|
||||
|
||||
#define DEG2RAD(d) ((EUL_PI / 180.0) * d)
|
||||
#define RAD2DEG(r) ((180.0 / EUL_PI) * r)
|
||||
|
||||
/**
|
||||
* Orientations are represented by 3D double precision Tait-Bryan (Euler) tuples
|
||||
* called EulerAngles. The angles are in Z, Y', X" order that is often called
|
||||
* intrinsic rotations, they are measured in radians. The default (unrotated)
|
||||
* orientation is along the X axis. Conceptually, the Z axis rotation is heading,
|
||||
* the Y' axis rotation is descending (negative elevation), the X" axis rotation is bank.
|
||||
* For example, positive rotation along the Z axis rotates X into Y (turns left), positive
|
||||
* rotation along the Y axis rotates Z into X (leans forward), positive rotation
|
||||
* along the X axis rotates Y into Z (leans right).
|
||||
*/
|
||||
typedef struct
|
||||
{
|
||||
double alpha; // unit rad
|
||||
double beta; // unit rad
|
||||
double gamma; // unit rad
|
||||
} EULERANGLES;
|
||||
|
||||
typedef enum
|
||||
{
|
||||
EULERANGLES_NONE = 0,
|
||||
EULERANGLES_ZYX = 1,
|
||||
EULERANGLES_ZYZ = 2,
|
||||
EULERANGLES_XYZ = 3,
|
||||
} EULERANGLES_TYPE;
|
||||
|
||||
extern const EULERANGLES EULERANGLES_ZERO;
|
||||
extern const EULERANGLES EULERANGLES_NIL;
|
||||
|
||||
/** @brief Add two EulerAngles. */
|
||||
EULERANGLES EulerAngles_Add(EULERANGLES e1, EULERANGLES e2);
|
||||
|
||||
/** @brief Subtracts two EulerAngles. */
|
||||
EULERANGLES EulerAngles_Sub(EULERANGLES e1, EULERANGLES e2);
|
||||
|
||||
/** @brief Multiplies EulerAngles by a real number. */
|
||||
void EulerAngles_ScalarMult(EULERANGLES *e, double f);
|
||||
|
||||
/** @brief Multiplies EulerAngles by a real number. */
|
||||
EULERANGLES EulerAngles_GetScalarMult(EULERANGLES e, double f);
|
||||
|
||||
/** @brief Normalize EulerAngles. */
|
||||
void EulerAngles_Normalize(EULERANGLES *e);
|
||||
|
||||
/** @brief Normalize EulerAngles. */
|
||||
EULERANGLES EulerAngles_GetNormalize(EULERANGLES e);
|
||||
|
||||
#endif
|
||||
@@ -0,0 +1,452 @@
|
||||
//
|
||||
// Copyright (C) 2014 OpenSim Ltd.
|
||||
//
|
||||
// SPDX-License-Identifier: LGPL-3.0-or-later
|
||||
//
|
||||
|
||||
#include <math.h>
|
||||
#include <stdint.h>
|
||||
#include <stdbool.h>
|
||||
#include "coord.h"
|
||||
#include "eulerangles.h"
|
||||
#include "quaternion.h"
|
||||
|
||||
typedef union
|
||||
{
|
||||
float value;
|
||||
uint32_t raw;
|
||||
} SOFT_FLOAT;
|
||||
|
||||
typedef union
|
||||
{
|
||||
double value;
|
||||
uint64_t raw;
|
||||
} SOFT_DOUBLE;
|
||||
|
||||
const SOFT_FLOAT EPS_FLOAT = {.raw = 0x34000000};
|
||||
const SOFT_DOUBLE EPS_DOUBLE = {.raw = 0x3CB0000000000000};
|
||||
|
||||
const QUATERNION QUATERNION_IDENTITY = {1.0, 0.0, 0.0, 0.0};
|
||||
const QUATERNION QUATERNION_NIL = {NAN, NAN, NAN, NAN};
|
||||
/** @brief Subtracts two quaternions. */
|
||||
QUATERNION Quaternion_Sub(QUATERNION q1, QUATERNION q2)
|
||||
{
|
||||
QUATERNION ret;
|
||||
ret.s = q1.s - q2.s;
|
||||
ret.i = q1.i - q2.i;
|
||||
ret.j = q1.j - q2.j;
|
||||
ret.k = q1.k - q2.k;
|
||||
return ret;
|
||||
}
|
||||
|
||||
/** @brief Adds two quaternions. */
|
||||
QUATERNION Quaternion_Add(QUATERNION q1, QUATERNION q2)
|
||||
{
|
||||
QUATERNION ret;
|
||||
ret.s = q1.s + q2.s;
|
||||
ret.i = q1.i + q2.i;
|
||||
ret.j = q1.j + q2.j;
|
||||
ret.k = q1.k + q2.k;
|
||||
return ret;
|
||||
}
|
||||
|
||||
/** @brief Multiplies a quaternion by a real number. */
|
||||
void Quaternion_ScalarMult(QUATERNION *q, double t)
|
||||
{
|
||||
q->s *= t;
|
||||
q->i *= t;
|
||||
q->j *= t;
|
||||
q->k *= t;
|
||||
}
|
||||
|
||||
/** @brief Multiplies a quaternion by a real number. */
|
||||
QUATERNION Quaternion_GetScalarMult(QUATERNION a, double t)
|
||||
{
|
||||
Quaternion_ScalarMult(&a, t);
|
||||
return a;
|
||||
}
|
||||
|
||||
/** @brief Divides a quaternion by a real number. */
|
||||
void Quaternion_ScalarDiv(QUATERNION *q, double t)
|
||||
{
|
||||
q->s /= t;
|
||||
q->i /= t;
|
||||
q->j /= t;
|
||||
q->k /= t;
|
||||
}
|
||||
|
||||
/** @brief Divides a quaternion by a real number. */
|
||||
QUATERNION Quaternion_GetScalarDiv(QUATERNION q, double t)
|
||||
{
|
||||
Quaternion_ScalarDiv(&q, t);
|
||||
return q;
|
||||
}
|
||||
|
||||
/** @brief Multiplies two quaternions. */
|
||||
QUATERNION Quaternion_Mult(QUATERNION q1, QUATERNION q2)
|
||||
{
|
||||
QUATERNION ret;
|
||||
ret.s = q1.s * q2.s - (q1.i * q2.i + q1.j * q2.j + q1.k * q2.k);
|
||||
ret.i = q1.j * q2.k - q1.k * q2.j + q1.s * q2.i + q1.i * q2.s;
|
||||
ret.j = q1.k * q2.i - q1.i * q2.k + q1.s * q2.j + q1.j * q2.s;
|
||||
ret.k = q1.i * q2.j - q1.j * q2.i + q1.s * q2.k + q1.k * q2.s;
|
||||
return ret;
|
||||
}
|
||||
|
||||
/** @brief Divides two quaternions. */
|
||||
QUATERNION Quaternion_Div(QUATERNION q1, QUATERNION q2)
|
||||
{
|
||||
Quaternion_Inverse(&q2);
|
||||
return Quaternion_Mult(q1, q2);
|
||||
}
|
||||
|
||||
/** @brief Dot product. */
|
||||
inline double Quaternion_Dot(QUATERNION q1, QUATERNION q2)
|
||||
{
|
||||
return (q1.s * q2.s + q1.i * q2.i + q1.j * q2.j + q1.k * q2.k);
|
||||
}
|
||||
|
||||
/** @brief Calculate a quaternion length^2 (omits calling square root). */
|
||||
inline double Quaternion_SquareLength(QUATERNION q)
|
||||
{
|
||||
return (q.s * q.s + q.i * q.i + q.j * q.j + q.k * q.k);
|
||||
}
|
||||
|
||||
/** @brief Calculate a quaternion length. */
|
||||
inline double Quaternion_Length(QUATERNION q)
|
||||
{
|
||||
return sqrt(Quaternion_SquareLength(q));
|
||||
}
|
||||
|
||||
/** @brief Normalize quaternion. */
|
||||
void Quaternion_Normalize(QUATERNION *q)
|
||||
{
|
||||
Quaternion_ScalarDiv(q, Quaternion_Length(*q));
|
||||
}
|
||||
|
||||
/** @brief Normalize quaternion. */
|
||||
QUATERNION Quaternion_GetNormalized(QUATERNION q)
|
||||
{
|
||||
Quaternion_Normalize(&q);
|
||||
return q;
|
||||
}
|
||||
|
||||
/** @brief Conjugated quaternion. */
|
||||
void Quaternion_Conjugated(QUATERNION *q)
|
||||
{
|
||||
q->i = -q->i;
|
||||
q->j = -q->j;
|
||||
q->k = -q->k;
|
||||
}
|
||||
|
||||
/** @brief Conjugated quaternion. */
|
||||
QUATERNION Quaternion_GetConjugated(QUATERNION q)
|
||||
{
|
||||
Quaternion_Conjugated(&q);
|
||||
return q;
|
||||
}
|
||||
|
||||
/** @brief Inverse quaternion. */
|
||||
void Quaternion_Inverse(QUATERNION *q)
|
||||
{
|
||||
Quaternion_Conjugated(q);
|
||||
Quaternion_ScalarDiv(q, Quaternion_SquareLength(*q));
|
||||
}
|
||||
|
||||
/** @brief Inverse quaternion. */
|
||||
QUATERNION Quaternion_GeInverse(QUATERNION q)
|
||||
{
|
||||
Quaternion_Inverse(&q);
|
||||
return q;
|
||||
}
|
||||
|
||||
inline double Quaternion_GetRotationAngle(QUATERNION q)
|
||||
{
|
||||
return (acos(q.s) * 2);
|
||||
}
|
||||
|
||||
/** @brief Get axis of quaternion. */
|
||||
COORD Quaternion_GetRotationAxis(QUATERNION q)
|
||||
{
|
||||
double sin_theta_inv;
|
||||
COORD ret;
|
||||
sin_theta_inv = 1.0 / sin(acos(q.s));
|
||||
ret.x = q.i * sin_theta_inv;
|
||||
ret.y = q.j * sin_theta_inv;
|
||||
ret.z = q.k * sin_theta_inv;
|
||||
return ret;
|
||||
}
|
||||
|
||||
/** @brief Get the axis and rotate angle of a unit Quaternion */
|
||||
void Quaternion_GetRotationAxisAndAngle(QUATERNION q, COORD *axis, double *angle)
|
||||
{
|
||||
double sin_theta_inv;
|
||||
*angle = acos(q.s);
|
||||
|
||||
// pre-compute to save time
|
||||
sin_theta_inv = 1.0 / sin(*angle);
|
||||
|
||||
// now the vector
|
||||
axis->x = q.i * sin_theta_inv;
|
||||
axis->y = q.j * sin_theta_inv;
|
||||
axis->z = q.k * sin_theta_inv;
|
||||
*angle = (*angle) * 2;
|
||||
}
|
||||
|
||||
/** @brief Decompose the rotation on to 2 parts. */
|
||||
void Quaternion_GetSwingAndTwist(QUATERNION q, COORD axis, QUATERNION *swing, QUATERNION *twist)
|
||||
{
|
||||
double p;
|
||||
COORD v;
|
||||
v.x = q.i;
|
||||
v.y = q.j;
|
||||
v.z = q.k;
|
||||
// return projection v1 on to v2 (parallel component)
|
||||
p = Coord_Dot(v, axis);
|
||||
v = Coord_GetScalarMult(axis, p);
|
||||
twist->s = q.s;
|
||||
twist->i = v.x;
|
||||
twist->j = v.y;
|
||||
twist->k = v.z;
|
||||
Quaternion_Normalize(twist);
|
||||
*swing = Quaternion_Mult(q, Quaternion_GetConjugated(*twist));
|
||||
}
|
||||
|
||||
/** @brief rotates v by this quaternion (quaternion must be unit) */
|
||||
COORD Quaternion_Rotate(QUATERNION q, COORD v)
|
||||
{
|
||||
QUATERNION temp;
|
||||
COORD ret;
|
||||
temp.s = 0;
|
||||
temp.i = v.x;
|
||||
temp.j = v.y;
|
||||
temp.k = v.z;
|
||||
temp = Quaternion_Mult(q, temp);
|
||||
Quaternion_Conjugated(&q);
|
||||
temp = Quaternion_Mult(temp, q);
|
||||
ret.x = temp.i;
|
||||
ret.y = temp.j;
|
||||
ret.z = temp.k;
|
||||
return ret;
|
||||
}
|
||||
|
||||
/** @brief returns the euler angles from a rotation Quaternion */
|
||||
EULERANGLES Quaternion_ToEulerAngles(QUATERNION q, EULERANGLES_TYPE type)
|
||||
{
|
||||
EULERANGLES ret;
|
||||
switch (type)
|
||||
{
|
||||
case EULERANGLES_ZYX:
|
||||
{
|
||||
/* Covert form matlab 2023a */
|
||||
// Cap all inputs to asin to 1, since values >1 produce complex
|
||||
// results
|
||||
// Since the quaternion is of unit length, this should never happen,
|
||||
// but some code generation configuration seem to hit this edge case
|
||||
// under some circumstances.
|
||||
|
||||
double asinValue;
|
||||
bool mask1, mask2;
|
||||
|
||||
asinValue = -2.0 * (q.i * q.k - q.s * q.j);
|
||||
mask1 = asinValue >= 1.0 - 10 * EPS_DOUBLE.value;
|
||||
mask2 = asinValue <= -1.0 + 10 * EPS_DOUBLE.value;
|
||||
if (mask1)
|
||||
asinValue = 1;
|
||||
if (mask2)
|
||||
asinValue = -1;
|
||||
|
||||
if (mask1 || mask2)
|
||||
{
|
||||
ret.alpha = (asinValue > 0) ? (-2.0 * atan2(q.i, q.s)) : (2.0 * atan2(q.i, q.s));
|
||||
ret.gamma = 0;
|
||||
}
|
||||
else
|
||||
{
|
||||
ret.alpha = atan2(2.0 * (q.i * q.j + q.s * q.k), q.s * q.s + q.i * q.i - q.j * q.j - q.k * q.k);
|
||||
ret.gamma = atan2(2.0 * (q.j * q.k + q.s * q.i), q.s * q.s - q.i * q.i - q.j * q.j + q.k * q.k);
|
||||
}
|
||||
ret.beta = asin(asinValue);
|
||||
break;
|
||||
}
|
||||
|
||||
case EULERANGLES_ZYZ:
|
||||
{
|
||||
// TODO;
|
||||
ret = EULERANGLES_NIL;
|
||||
break;
|
||||
}
|
||||
|
||||
case EULERANGLES_XYZ:
|
||||
{
|
||||
/* Covert form matlab 2023a */
|
||||
// Prevent singularities as done in ZYX case
|
||||
// Alternative to rotm2eul(quat2rotm(q), 'XYZ') with fewer
|
||||
// operations
|
||||
// sin(y) = R13 = 2 * (qx*qz + qy*qw)
|
||||
// tan(x) = sin(x) / cos(x) = -R23 / R33
|
||||
// = -2 * (qy*qz - qx*qw) / (1 - 2*(qx^2 + qy^2))
|
||||
// = -2 * (qy*qz - qx*qw) / (qw^2 - qx^2 - qy^2 + qz^2)
|
||||
// tan(z) = sin(z) / cos(z) = -R12 / R11
|
||||
// = -2 * (qx*qy - qz*qw) / (1 - 2*(qy^2 + qz^2))
|
||||
// = -2 * (qy*qz - qx*qw) / (qw^2 + qx^2 - qy^2 - qz^2)
|
||||
|
||||
double asinValue;
|
||||
bool mask1, mask2;
|
||||
|
||||
asinValue = 2.0 * (q.i * q.k + q.s * q.j);
|
||||
mask1 = asinValue >= 1.0 - 10 * EPS_DOUBLE.value;
|
||||
mask2 = asinValue <= -1.0 + 10 * EPS_DOUBLE.value;
|
||||
if (mask1)
|
||||
asinValue = 1;
|
||||
if (mask2)
|
||||
asinValue = -1;
|
||||
if (mask1 || mask2)
|
||||
{
|
||||
ret.alpha = 0;
|
||||
ret.gamma = (asinValue > 0) ? (2.0 * atan2(q.i, q.s)) : (-2.0 * atan2(q.i, q.s));
|
||||
}
|
||||
else
|
||||
{
|
||||
ret.alpha = atan2(-2.0 * (q.j * q.k - q.s * q.i), q.s * q.s - q.i * q.i - q.j * q.j + q.k * q.k);
|
||||
ret.gamma = atan2(-2.0 * (q.i * q.j - q.s * q.k), q.s * q.s + q.i * q.i - q.j * q.j - q.k * q.k);
|
||||
}
|
||||
ret.beta = asin(asinValue);
|
||||
break;
|
||||
}
|
||||
|
||||
default:
|
||||
ret = EULERANGLES_NIL;
|
||||
}
|
||||
return ret;
|
||||
}
|
||||
|
||||
/** @brief returns the rotation Quaternion from a euler angles */
|
||||
QUATERNION Quaternion_FromeEulerAngles(EULERANGLES e, EULERANGLES_TYPE type)
|
||||
{
|
||||
QUATERNION ret;
|
||||
double cosx, sinx, cosy, siny, cosz, sinz;
|
||||
cosx = cos(e.alpha * 0.5);
|
||||
sinx = sin(e.alpha * 0.5);
|
||||
cosy = cos(e.beta * 0.5);
|
||||
siny = sin(e.beta * 0.5);
|
||||
cosz = cos(e.gamma * 0.5);
|
||||
sinz = sin(e.gamma * 0.5);
|
||||
switch (type)
|
||||
{
|
||||
case EULERANGLES_ZYX:
|
||||
ret.s = cosx * cosy * cosz + sinx * siny * sinz;
|
||||
ret.i = cosx * cosy * sinz - sinx * siny * cosz;
|
||||
ret.j = cosx * siny * cosz + sinx * cosy * sinz;
|
||||
ret.k = sinx * cosy * cosz - cosx * siny * sinz;
|
||||
break;
|
||||
case EULERANGLES_ZYZ:
|
||||
ret.s = cosx * cosy * cosz - sinx * cosy * sinz;
|
||||
ret.i = cosx * siny * sinz - sinx * siny * cosz;
|
||||
ret.j = cosx * siny * cosz + sinx * siny * sinz;
|
||||
ret.k = sinx * cosy * cosz + cosx * cosy * sinz;
|
||||
break;
|
||||
case EULERANGLES_XYZ:
|
||||
ret.s = cosx * cosy * cosz - sinx * siny * sinz;
|
||||
ret.i = sinx * cosy * cosz + cosx * siny * sinz;
|
||||
ret.j = -sinx * cosy * sinz + cosx * siny * cosz;
|
||||
ret.k = cosx * cosy * sinz + sinx * siny * cosz;
|
||||
break;
|
||||
default:
|
||||
ret = QUATERNION_NIL;
|
||||
break;
|
||||
}
|
||||
return ret;
|
||||
}
|
||||
|
||||
/** @brief adapted from https://svn.code.sf.net/p/irrlicht/code/trunk/include/quaternion.h */
|
||||
QUATERNION Quaternion_RotationFromTo(COORD from, COORD to)
|
||||
{
|
||||
// Based on Stan Melax's article in Game Programming Gems
|
||||
COORD axis;
|
||||
QUATERNION ret;
|
||||
double c, s;
|
||||
Coord_Normalize(&from);
|
||||
Coord_Normalize(&to);
|
||||
|
||||
c = Coord_Dot(from, to);
|
||||
|
||||
if (c >= 1.0)
|
||||
{ // If dot == 1, vectors are the same
|
||||
return QUATERNION_IDENTITY; // identity
|
||||
}
|
||||
else if (c <= -1.0)
|
||||
{ // exactly opposite
|
||||
axis = Coord_Cross(COORD_X_AXIS, from);
|
||||
|
||||
if (Coord_Length(axis) == 0)
|
||||
axis = Coord_Cross(COORD_Y_AXIS, from);
|
||||
|
||||
// same as fromAxisAngle(axis, PI).normalize();
|
||||
ret.s = 0;
|
||||
ret.i = axis.x;
|
||||
ret.j = axis.y;
|
||||
ret.k = axis.z;
|
||||
Quaternion_Normalize(&ret);
|
||||
return ret;
|
||||
}
|
||||
|
||||
s = sqrt((1 + c) * 2);
|
||||
axis = Coord_Cross(from, to);
|
||||
Coord_ScalarDiv(&axis, s);
|
||||
ret.s = s * 0.5;
|
||||
ret.i = axis.x;
|
||||
ret.j = axis.y;
|
||||
ret.k = axis.z;
|
||||
Quaternion_Normalize(&ret);
|
||||
return ret;
|
||||
}
|
||||
|
||||
/** @brief linear quaternion interpolation */
|
||||
QUATERNION Quaternion_Lerp(QUATERNION q1, QUATERNION q2, double t)
|
||||
{
|
||||
QUATERNION ret;
|
||||
Quaternion_ScalarMult(&q1, 1 - t);
|
||||
Quaternion_ScalarMult(&q1, t);
|
||||
ret = Quaternion_Add(q1, q2);
|
||||
Quaternion_Normalize(&ret);
|
||||
return ret;
|
||||
}
|
||||
|
||||
/** @brief spherical linear interpolation */
|
||||
QUATERNION Quaternion_Slerp(QUATERNION q1, QUATERNION q2, double t)
|
||||
{
|
||||
double dot;
|
||||
QUATERNION q3, ret;
|
||||
dot = Quaternion_Dot(q1, q2);
|
||||
/* dot = cos(theta)
|
||||
if (dot < 0), q1 and q2 are more than 90 degrees apart,
|
||||
so we can invert one to reduce spinning */
|
||||
if (dot < 0)
|
||||
{
|
||||
dot = -dot;
|
||||
q3.s = -q2.s;
|
||||
q3.i = -q2.i;
|
||||
q3.j = -q2.j;
|
||||
q3.k = -q2.k;
|
||||
}
|
||||
else
|
||||
{
|
||||
q3 = q2;
|
||||
}
|
||||
|
||||
if (dot < 0.95)
|
||||
{
|
||||
double angle = acos(dot);
|
||||
Quaternion_ScalarMult(&q1, sin(angle * (1 - t)));
|
||||
Quaternion_ScalarMult(&q3, sin(angle * t));
|
||||
ret = Quaternion_Add(q1, q3);
|
||||
Quaternion_ScalarDiv(&ret, sin(angle));
|
||||
return ret;
|
||||
}
|
||||
// if the angle is small, use linear interpolation
|
||||
else
|
||||
{
|
||||
return Quaternion_Lerp(q1, q3, t);
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,109 @@
|
||||
|
||||
#ifndef __QUATERNION_H
|
||||
#define __QUATERNION_H
|
||||
|
||||
#include "coord.h"
|
||||
#include "eulerangles.h"
|
||||
|
||||
// borrowed and adapted from https://github.com/MegaManSE/willperone/blob/master/Math/quaternion.h
|
||||
typedef struct
|
||||
{
|
||||
double s;
|
||||
double i;
|
||||
double j;
|
||||
double k;
|
||||
} QUATERNION;
|
||||
|
||||
extern const QUATERNION QUATERNION_IDENTITY;
|
||||
extern const QUATERNION QUATERNION_NIL;
|
||||
|
||||
/** @brief Subtracts two quaternions. */
|
||||
QUATERNION Quaternion_Sub(QUATERNION q1, QUATERNION q2);
|
||||
|
||||
/** @brief Adds two quaternions. */
|
||||
QUATERNION Quaternion_Add(QUATERNION q1, QUATERNION q2);
|
||||
|
||||
/** @brief Multiplies a quaternion by a real number. */
|
||||
void Quaternion_ScalarMult(QUATERNION *q, double t);
|
||||
|
||||
/** @brief Multiplies a quaternion by a real number. */
|
||||
QUATERNION Quaternion_GetScalarMult(QUATERNION a, double t);
|
||||
|
||||
/** @brief Divides a quaternion by a real number. */
|
||||
void Quaternion_ScalarDiv(QUATERNION *q, double t);
|
||||
|
||||
/** @brief Divides a quaternion by a real number. */
|
||||
QUATERNION Quaternion_GetScalarDiv(QUATERNION a, double t);
|
||||
|
||||
/** @brief Multiplies two quaternions. */
|
||||
QUATERNION Quaternion_Mult(QUATERNION q1, QUATERNION q2);
|
||||
|
||||
/** @brief Divides two quaternions. */
|
||||
QUATERNION Quaternion_Div(QUATERNION q1, QUATERNION q2);
|
||||
|
||||
/** @brief Dot product. */
|
||||
double Quaternion_Dot(QUATERNION q1, QUATERNION q2);
|
||||
|
||||
/** @brief Calculate a quaternion length^2 (omits calling square root). */
|
||||
double Quaternion_SquareLength(QUATERNION q);
|
||||
|
||||
/** @brief Calculate a quaternion length. */
|
||||
double Quaternion_Length(QUATERNION q);
|
||||
|
||||
/** @brief Normalize quaternion. */
|
||||
void Quaternion_Normalize(QUATERNION *q);
|
||||
|
||||
/** @brief Normalize quaternion. */
|
||||
QUATERNION Quaternion_GetNormalized(QUATERNION q);
|
||||
|
||||
/** @brief Conjugated quaternion. */
|
||||
void Quaternion_Conjugated(QUATERNION *q);
|
||||
|
||||
/** @brief Conjugated quaternion. */
|
||||
QUATERNION Quaternion_GetConjugated(QUATERNION q);
|
||||
|
||||
/** @brief Inverse quaternion. */
|
||||
void Quaternion_Inverse(QUATERNION *q);
|
||||
|
||||
/** @brief Inverse quaternion. */
|
||||
QUATERNION Quaternion_GeInverse(QUATERNION q);
|
||||
|
||||
/** @brief Get rotate angle of quaternion. */
|
||||
double Quaternion_GetRotationAngle(QUATERNION q);
|
||||
|
||||
/** @brief Get axis of quaternion. */
|
||||
COORD Quaternion_GetRotationAxis(QUATERNION q);
|
||||
|
||||
/** @brief Get the axis and rotate angle of a unit Quaternion */
|
||||
void Quaternion_GetRotationAxisAndAngle(QUATERNION q, COORD *axis, double *angle);
|
||||
|
||||
/** @brief Decompose the rotation on to 2 parts.
|
||||
1. Twist - rotation around the "direction" vector
|
||||
2. Swing - rotation around axis that is perpendicular to "direction" vector
|
||||
The rotation can be composed back by rotation = swing * twist
|
||||
|
||||
has singularity in case of swing_rotation close to 180 degrees rotation.
|
||||
if the input quaternion is of non-unit length, the outputs are non-unit as well
|
||||
otherwise, outputs are both unit
|
||||
*/
|
||||
void Quaternion_GetSwingAndTwist(QUATERNION q, COORD axis, QUATERNION *swing, QUATERNION *twist);
|
||||
|
||||
/** @brief rotates v by this quaternion (quaternion must be unit) */
|
||||
COORD Quaternion_Rotate(QUATERNION q, COORD v);
|
||||
|
||||
/** @brief returns the euler angles from a rotation Quaternion */
|
||||
EULERANGLES Quaternion_ToEulerAngles(QUATERNION q, EULERANGLES_TYPE type);
|
||||
|
||||
/** @brief returns the rotation Quaternion from a euler angles */
|
||||
QUATERNION Quaternion_FromeEulerAngles(EULERANGLES e, EULERANGLES_TYPE type);
|
||||
|
||||
/** @brief adapted from https://svn.code.sf.net/p/irrlicht/code/trunk/include/quaternion.h */
|
||||
QUATERNION Quaternion_RotationFromTo(COORD from, COORD to);
|
||||
|
||||
/** @brief linear quaternion interpolation */
|
||||
QUATERNION Quaternion_Lerp(QUATERNION q1, QUATERNION q2, double t);
|
||||
|
||||
/** @brief spherical linear interpolation */
|
||||
QUATERNION Quaternion_Slerp(QUATERNION q1, QUATERNION q2, double t);
|
||||
|
||||
#endif
|
||||
File diff suppressed because it is too large
Load Diff
@@ -0,0 +1,145 @@
|
||||
/*****************************************************************************/
|
||||
/**
|
||||
* Description: Header file for df linklayer functions
|
||||
*
|
||||
* Date: 2021-11-13 13:33
|
||||
* LastEditTime: 2022-07-16 10:12
|
||||
* Revisions: Rev 0.1
|
||||
* History:
|
||||
* [2022-07-16]: New branch for DiJian.
|
||||
* [2021-11-23]: Initial version.
|
||||
*
|
||||
* Copyright(c) 2021 Beijing Institute of Technology.
|
||||
* Lab of Communication and Networking
|
||||
*
|
||||
******************************************************************************/
|
||||
|
||||
#ifndef __LINK_LAYER_H_
|
||||
#define __LINK_LAYER_H_
|
||||
|
||||
/***************************** Include Files *********************************/
|
||||
#include <stdint.h>
|
||||
#include "circ_queue.h"
|
||||
#include "link_layer_types.h"
|
||||
#include "../phy_layer/phy_layer.h"
|
||||
#include "../../system_monitor.h"
|
||||
#include "../config/app_config.h"
|
||||
#include "node_list.h"
|
||||
/************************** Constant Definitions *****************************/
|
||||
#define L2_HEAD_NODE_ID 1
|
||||
#define L2_MEMBER_NODE_ID 2
|
||||
/***************** Macros (Inline Functions) Definitions *********************/
|
||||
// #define AssertL2Event(inst, event) Linklayer_EventHandler(inst, event)
|
||||
/**************************** Type Definitions *******************************/
|
||||
|
||||
typedef struct
|
||||
{
|
||||
L2_RTNE_TABLE_ENTRY **rtneTable;
|
||||
uint32_t rtnePrd;
|
||||
uint32_t rtneStep;
|
||||
L2_RTNE_TABLE_ENTRY *last;
|
||||
L2_RTNE_TABLE_ENTRY *curr;
|
||||
L2_RTNE_TABLE_ENTRY *next;
|
||||
} L2_RTNE_CONFIG;
|
||||
|
||||
typedef struct
|
||||
{
|
||||
L2_NODE_TYPE myType;
|
||||
// bool isInterHead;
|
||||
// uint8_t clustId;
|
||||
uint8_t myId;
|
||||
uint8_t headId;
|
||||
// uint8_t bkpHeadId;
|
||||
uint8_t netScale;
|
||||
PHY_INST *phyInstRef; // pyhsical layer reference
|
||||
} L2_CONFIG;
|
||||
|
||||
typedef struct
|
||||
{
|
||||
L2_CONFIG config;
|
||||
bool dataAvail; // if there is a valid received frame data in rx buffer
|
||||
bool cxn2Net; // if the node is connected to the network
|
||||
L2_RCD frameNo; // stores the last current and next frame number.
|
||||
L2_RCD slotNo; // stores the last current and next slot number.
|
||||
L2_RCD frame; // stores the last current and next link layer frame type.
|
||||
L2_RCD torState; // stores the last current and next link layer send/receive state.
|
||||
L2_NODE_STATE mainState; // the main state of link layer state
|
||||
L2_NODE_STATE nextState; // next state of link layer state, which will be applied to mainState every second.
|
||||
L2_RTNE_CONFIG rtneConfig; // current routine table configuration and status.
|
||||
L2_SCHD_EVENT *schdEventPool[L2_SCHD_EVENT_MAX];
|
||||
uint32_t schdEventCnt;
|
||||
SCAN_FSM scanFsm;
|
||||
SVC_FSM serviceFsm;
|
||||
NODE_LIST *nodeInfoList;
|
||||
uint8_t *sendPktData;
|
||||
uint8_t *recvPktData;
|
||||
// for system log
|
||||
SYSINFO sysInfo;
|
||||
SLOTLOG txSlotLog;
|
||||
SLOTLOG rxSlotLog;
|
||||
PRTCLLOG prtcLog;
|
||||
CIRC_QUEUE *txQueue;
|
||||
// for debug
|
||||
uint32_t recvCnt;
|
||||
uint32_t recvFineCnt;
|
||||
uint32_t sentCnt;
|
||||
} L2_INST;
|
||||
|
||||
/************************** Variable Definitions *****************************/
|
||||
extern L2_RTNE_TABLE_ENTRY *normalTable[L2_RTNE_PERIOD];
|
||||
/************************** Function Prototypes ******************************/
|
||||
// Main
|
||||
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 *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 *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 *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 *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 *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 *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 *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 *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_ */
|
||||
@@ -0,0 +1,212 @@
|
||||
/*****************************************************************************/
|
||||
/**
|
||||
* Description: A brief description of this file
|
||||
*
|
||||
* Date: 2022-08-28 22:53
|
||||
* LastEditTime: 2022-08-29 00:45
|
||||
* Revisions: Rev 0.0
|
||||
* History:
|
||||
* [2022-08-29]: Initial version.
|
||||
*
|
||||
* Copyright(c) 2022 Beijing Institute of Technology.
|
||||
* Lab of Communication and Networking
|
||||
*
|
||||
******************************************************************************/
|
||||
|
||||
#ifndef __LINK_LAYER_DEF_H_
|
||||
#define __LINK_LAYER_DEF_H_
|
||||
|
||||
/***************************** Include Files *********************************/
|
||||
#include <stdbool.h>
|
||||
#include <stdint.h>
|
||||
#include "geometry/coord.h"
|
||||
#include "geometry/eulerangles.h"
|
||||
#include "geometry/quaternion.h"
|
||||
#include "../config/app_config.h"
|
||||
#include "../phy_layer/phy_layer.h"
|
||||
/************************** Constant Definitions *****************************/
|
||||
// Common node ID definition
|
||||
#define BROADCAST_ID 0xFF
|
||||
|
||||
// Routine configure
|
||||
#define L2_RTNE_PERIOD 60
|
||||
|
||||
// Common slot number definition
|
||||
#define L2_SLOTNO_NONE 0
|
||||
#define L2_FRAMENO_NONE 0
|
||||
|
||||
// Common number of slots definition
|
||||
#define L2_FRAME_NO_MAX 256
|
||||
#define L2_SLOTS_PER_FRAME 200
|
||||
#define L2_SLOTS_PER_SUBFRAME 20
|
||||
|
||||
#define NUM_SLOTS_NONE UINT32_MAX
|
||||
#define NUM_SLOTS_100MS 20
|
||||
#define NUM_SLOTS_20MS 4
|
||||
#define NUM_SLOTS_10MS 2
|
||||
#define NUM_SLOTS_5MS 1
|
||||
#define NUM_SLOTS_SKIP 0
|
||||
|
||||
// Common schedule event definition
|
||||
#define L2_SCHD_EVENT_MAX 16
|
||||
|
||||
/***************** Macros (Inline Functions) Definitions *********************/
|
||||
#define AssertL2Event(inst, envent) Linklayer_EventHandler(inst, envent)
|
||||
/**************************** Type Definitions *******************************/
|
||||
|
||||
typedef struct
|
||||
{
|
||||
int32_t last;
|
||||
int32_t curr;
|
||||
int32_t next;
|
||||
} L2_RCD;
|
||||
|
||||
typedef enum
|
||||
{
|
||||
L2_TOR_SUSPEND = 0,
|
||||
L2_TOR_TX = 1,
|
||||
L2_TOR_RX = 2
|
||||
} L2_TOR_TYPE;
|
||||
|
||||
/** @name Prior information type definition
|
||||
*/
|
||||
typedef enum
|
||||
{
|
||||
L2_NONE_TYPE = 0,
|
||||
L2_HEAD_NODE = 1,
|
||||
L2_MEMBER_NODE = 2,
|
||||
L2_BACKUP_HEAD = 3
|
||||
} L2_NODE_TYPE;
|
||||
/** @name Node state type definition
|
||||
*/
|
||||
|
||||
typedef enum
|
||||
{
|
||||
NONE_STATE = 0,
|
||||
INIT_STATE = 1,
|
||||
NORMAL_STATE = 2
|
||||
} L2_NODE_STATE;
|
||||
|
||||
/** @name Node information type definition
|
||||
* The following types define the detail node info which needed by the link layer.
|
||||
*/
|
||||
typedef struct
|
||||
{
|
||||
uint8_t nodeId;
|
||||
uint8_t nodeType;
|
||||
uint8_t beamNo;
|
||||
COORD pos;
|
||||
COORD speed;
|
||||
QUATERNION attitude;
|
||||
} L2_NODE_INFO;
|
||||
|
||||
/** @name L2 event enum
|
||||
* A enum of L2 event types.
|
||||
*/
|
||||
typedef enum
|
||||
{
|
||||
L2_EV_NONE = 0,
|
||||
L2_EV_SCAN_PROC_DONE = 1,
|
||||
L2_EV_NET_SETUP_DONE = 2,
|
||||
L2_EV_TIME_SYNC = 3,
|
||||
L2_EV_LOST_CXN = 4,
|
||||
L2_EV_LOST_NODE = 5,
|
||||
L2_EV_FRAME_NO_UPDATE = 6,
|
||||
L2_EV_SLOT_NO_UPDATE = 7,
|
||||
L2_EV_MAIN_STATE_UPDATE = 8,
|
||||
L2_ROUTINE_FINISH = 9,
|
||||
L2_ROUTINE_UPDATE = 10
|
||||
} L2_EVENT_TYPE;
|
||||
|
||||
/** @name L2 event
|
||||
* A definition of L2 event instance.
|
||||
*/
|
||||
typedef struct
|
||||
{
|
||||
L2_EVENT_TYPE event;
|
||||
void *msg;
|
||||
} L2_EVENT;
|
||||
|
||||
/** @name L2 event
|
||||
* A definition of L2 event instance.
|
||||
*/
|
||||
typedef struct
|
||||
{
|
||||
uint8_t eventId;
|
||||
bool isSchd;
|
||||
L2_EVENT l2Event;
|
||||
uint32_t schdFrame;
|
||||
uint32_t schdSlot;
|
||||
} L2_SCHD_EVENT;
|
||||
|
||||
/** @name Slot types
|
||||
* A definition of different slots.
|
||||
*/
|
||||
typedef enum
|
||||
{
|
||||
L2_SLOT_NONE = 0,
|
||||
L2_SLOT_IDLE = 1,
|
||||
L2_SLOT_SCAN = 2,
|
||||
L2_SLOT_SERVICE = 3,
|
||||
L2_SLOT_ADVERTISE = 4,
|
||||
} L2_SLOT_TYPE;
|
||||
|
||||
/** @name Frame types
|
||||
* A definition of different frame types.
|
||||
*/
|
||||
typedef enum
|
||||
{
|
||||
L2_NONE_FRAME = 0,
|
||||
L2_SCAN_FRAME = 1,
|
||||
L2_SVC_FRAME = 2,
|
||||
L2_INFO_FRAME = 3
|
||||
} L2_FRAME_TYPE;
|
||||
|
||||
/** @name Frame types
|
||||
* A definition of different frame types.
|
||||
*/
|
||||
typedef enum
|
||||
{
|
||||
L2_CRC_NONE = 0,
|
||||
L2_CRC16_1021 = 1,
|
||||
L2_CRC16_8005 = 2,
|
||||
L2_CRC8_31 = 3
|
||||
} L2_CRC_TYPE;
|
||||
|
||||
/** @name Schedule subtable entry
|
||||
* A definition of Schedule subtable entry.
|
||||
*/
|
||||
typedef struct
|
||||
{
|
||||
L2_SLOT_TYPE slotType : 8;
|
||||
uint8_t slotSeq;
|
||||
uint8_t src;
|
||||
uint8_t dst;
|
||||
} L2_RTNE_TABLE_ENTRY;
|
||||
|
||||
/** @name Scan procedure FSM
|
||||
*/
|
||||
typedef struct
|
||||
{
|
||||
uint8_t sacanCnt;
|
||||
uint8_t beamNo;
|
||||
} SCAN_FSM;
|
||||
|
||||
/** @name Service procedure FSM
|
||||
*/
|
||||
typedef struct
|
||||
{
|
||||
uint32_t txSeqNum;
|
||||
uint32_t rxSeqNum;
|
||||
uint8_t rxLog[256];
|
||||
uint8_t txLog[256];
|
||||
uint32_t pktRecv;
|
||||
uint32_t pktRecved;
|
||||
#ifdef DATALINK_ENABLE_BERTEST
|
||||
uint32_t bitTotal;
|
||||
uint32_t bitError;
|
||||
#endif
|
||||
} SVC_FSM;
|
||||
/************************** Variable Definitions *****************************/
|
||||
|
||||
#endif /* __LINK_LAYER_DEF_H_ */
|
||||
@@ -0,0 +1,68 @@
|
||||
/*****************************************************************************/
|
||||
/**
|
||||
* Description: Header of 814 linklayer messages
|
||||
*
|
||||
* Date: 2022-07-14 00:08
|
||||
* LastEditTime: 2022-07-14 22:52
|
||||
* Revisions: Rev 0.0
|
||||
* History:
|
||||
* [2022-07-14]: Initial version.
|
||||
*
|
||||
* Copyright(c) 2021 Beijing Institute of Technology.
|
||||
* Lab of Communication and Networking
|
||||
*
|
||||
******************************************************************************/
|
||||
#ifndef __L2_MSG_DEF_H_
|
||||
#define __L2_MSG_DEF_H_
|
||||
|
||||
#define PACK_LEN_MAX 512
|
||||
|
||||
#define SVC_PACK_LEN 30 // bytes(240 bits)
|
||||
#define RSP_PACK_LEN 30 // bytes(240 bits)
|
||||
|
||||
// Common structure:
|
||||
// [bytes] [name]
|
||||
// 1 fame number
|
||||
// 1 slot number
|
||||
// 26 payload
|
||||
// 2 crc
|
||||
#define L2_MSG_FRAME_NO 0
|
||||
#define L2_MSG_SLOT_NO 1
|
||||
#define L2_MSG_PAYLOAD 2
|
||||
#define L2_MSG_CRC 28
|
||||
|
||||
// service frame structure:
|
||||
// [bytes] [name]
|
||||
// 1 fame number
|
||||
// 1 slot number
|
||||
// 3 time sync
|
||||
// 23 payload
|
||||
// 2 crc
|
||||
#define L2_MSG_SVC_TMSYNC 2
|
||||
#define L2_MSG_SVC_TMSYNC_LEN 3
|
||||
#define L2_MSG_SVC_TMSYNC_MAX (8388607) // 0x007FFFFF
|
||||
#define L2_MSG_SVC_TMSYNC_MIN (-8388608) // 0x00800000
|
||||
#define L2_MSG_SVC_SEQ_ID 5
|
||||
#define L2_MSG_SVC_PAYLOAD 6
|
||||
#define L2_MSG_SVC_PAYLOAD_LEN 22
|
||||
|
||||
// response frame structure:
|
||||
// [bytes] [name]
|
||||
// 1 fame number
|
||||
// 1 slot number
|
||||
// 4 position x
|
||||
// 4 position y
|
||||
// 4 position z
|
||||
// 4 speed x
|
||||
// 4 speed y
|
||||
// 4 speed z
|
||||
// 2 dummy
|
||||
// 2 crc
|
||||
#define L2_MSG_INFO_POSX 2
|
||||
#define L2_MSG_INFO_POSY 6
|
||||
#define L2_MSG_INFO_POSZ 10
|
||||
#define L2_MSG_INFO_SPEEDX 14
|
||||
#define L2_MSG_INFO_SPEEDY 18
|
||||
#define L2_MSG_INFO_SPEEDZ 22
|
||||
#define L2_MSG_INFO_DUMMY 26
|
||||
#endif //__L2_MSG_DEF_H_
|
||||
@@ -0,0 +1,527 @@
|
||||
/*****************************************************************************/
|
||||
/**
|
||||
* Description: Implementations of df linklayer local record.
|
||||
*
|
||||
* Date: 2022-07-14 00:08
|
||||
* LastEditTime: 2022-07-14 22:52
|
||||
* Revisions: Rev 0.0
|
||||
* History:
|
||||
* [2022-07-14]: Initial version.
|
||||
*
|
||||
* Copyright(c) 2021 Beijing Institute of Technology.
|
||||
* Lab of Communication and Networking
|
||||
*
|
||||
******************************************************************************/
|
||||
|
||||
/***************************** Include Files *********************************/
|
||||
#include "common.h"
|
||||
#include "error.h"
|
||||
#include "link_layer.h"
|
||||
#include "link_layer_types.h"
|
||||
#include "node_list.h"
|
||||
|
||||
/************************** Constant Definitions *****************************/
|
||||
|
||||
/***************** Macros (Inline Functions) Definitions *********************/
|
||||
inline int32_t GetNodeNum(NODE_LIST *list)
|
||||
{
|
||||
ASSERT(list != NULL);
|
||||
return list->nodeNum;
|
||||
}
|
||||
/**************************** Type Definitions *******************************/
|
||||
|
||||
/************************** Variable Definitions *****************************/
|
||||
|
||||
/************************** Function Prototypes ******************************/
|
||||
|
||||
/*****************************************************************************/
|
||||
/**
|
||||
* @brief Initialize node list
|
||||
*
|
||||
* @param list
|
||||
*
|
||||
* @return SUCCESS
|
||||
* ENOMEM - not enough memory.
|
||||
*
|
||||
******************************************************************************/
|
||||
int32_t NodeList_Init(NODE_LIST **list)
|
||||
{
|
||||
NODE_ENTRY *entry;
|
||||
(*list) = malloc(sizeof(NODE_LIST));
|
||||
if (*list == NULL)
|
||||
{
|
||||
return ENOMEM;
|
||||
}
|
||||
entry = malloc(sizeof(NODE_ENTRY));
|
||||
if (entry == NULL)
|
||||
{
|
||||
free(*list);
|
||||
return ENOMEM;
|
||||
}
|
||||
entry->nodeInfo.nodeId = 0;
|
||||
entry->nextNode = entry;
|
||||
entry->preNode = entry;
|
||||
(*list)->head = entry;
|
||||
(*list)->nodeNum = 0;
|
||||
return SUCCESS;
|
||||
}
|
||||
|
||||
void NodeList_Clear(NODE_LIST *list)
|
||||
{
|
||||
ASSERT(list != NULL);
|
||||
NODE_ENTRY *nodeEntry, *deletEntry;
|
||||
nodeEntry = list->head->nextNode;
|
||||
while (nodeEntry != list->head)
|
||||
{
|
||||
deletEntry = nodeEntry;
|
||||
nodeEntry = nodeEntry->nextNode;
|
||||
deletEntry->preNode->nextNode = deletEntry->nextNode;
|
||||
deletEntry->nextNode->preNode = deletEntry->preNode;
|
||||
DBG_INFO("Deleting node [%d].\n", deletEntry->nodeInfo.nodeId);
|
||||
free(deletEntry);
|
||||
list->nodeNum--;
|
||||
}
|
||||
}
|
||||
|
||||
void NodeList_Destroy(NODE_LIST *list)
|
||||
{
|
||||
if (list == NULL)
|
||||
{
|
||||
return;
|
||||
}
|
||||
NodeList_Clear(list);
|
||||
free(list->head);
|
||||
free(list);
|
||||
}
|
||||
|
||||
/*****************************************************************************/
|
||||
/**
|
||||
* @brief Get node entry frome node list base on node id.
|
||||
*
|
||||
* @param list
|
||||
* @param nodeId
|
||||
*
|
||||
* @return Node entry
|
||||
* NULL if node dose not exist.
|
||||
*
|
||||
******************************************************************************/
|
||||
NODE_ENTRY *GetNode(NODE_LIST *list, uint32_t nodeId)
|
||||
{
|
||||
ASSERT(list != NULL);
|
||||
NODE_ENTRY *nodeEntry = list->head->nextNode;
|
||||
while (nodeEntry != list->head)
|
||||
{
|
||||
if (nodeEntry->nodeInfo.nodeId == nodeId)
|
||||
{
|
||||
break;
|
||||
}
|
||||
else
|
||||
{
|
||||
nodeEntry = nodeEntry->nextNode;
|
||||
}
|
||||
}
|
||||
if (nodeEntry == list->head)
|
||||
{
|
||||
return NULL;
|
||||
}
|
||||
else
|
||||
{
|
||||
return nodeEntry;
|
||||
}
|
||||
}
|
||||
|
||||
/*****************************************************************************/
|
||||
/**
|
||||
* @brief Check whether the node is already in the node list.
|
||||
*
|
||||
* @param list
|
||||
* @param nodeId
|
||||
*
|
||||
* @return true node in the list
|
||||
* false node not in the list
|
||||
*
|
||||
******************************************************************************/
|
||||
inline bool IsNodeInList(NODE_LIST *list, uint8_t nodeId)
|
||||
{
|
||||
ASSERT(list != NULL);
|
||||
return (GetNode(list, nodeId) != NULL);
|
||||
}
|
||||
|
||||
/*****************************************************************************/
|
||||
/**
|
||||
* @brief Append a new node entry to the node list
|
||||
*
|
||||
* @param list
|
||||
* @param nodeId
|
||||
* @param nodeType
|
||||
*
|
||||
* @return SUCCESS Node was added successfully.
|
||||
* Other Error occurred.(Node already exist, memory alloc error ect.)
|
||||
*
|
||||
******************************************************************************/
|
||||
int32_t AppendNode(NODE_LIST *list, uint8_t nodeId, L2_NODE_TYPE nodeType)
|
||||
{
|
||||
ASSERT(list != NULL);
|
||||
if (IsNodeInList(list, nodeId))
|
||||
{
|
||||
DBG_WARNING("Node [%d] already exist.\n", nodeId);
|
||||
return FAILURE;
|
||||
}
|
||||
DBG_INFO("Link layer appending node [%d] to node list.\n", nodeId);
|
||||
NODE_ENTRY *nodeEntry = (NODE_ENTRY *)malloc(sizeof(NODE_ENTRY));
|
||||
if (nodeEntry == NULL)
|
||||
{
|
||||
return ENOMEM;
|
||||
}
|
||||
nodeEntry->nodeInfo.nodeId = nodeId;
|
||||
nodeEntry->nodeInfo.nodeType = (uint8_t)nodeType;
|
||||
nodeEntry->nodeInfo.pos.x = 0;
|
||||
nodeEntry->nodeInfo.pos.y = 0;
|
||||
nodeEntry->nodeInfo.pos.z = 0;
|
||||
nodeEntry->nodeInfo.speed.x = 0;
|
||||
nodeEntry->nodeInfo.speed.y = 0;
|
||||
nodeEntry->nodeInfo.speed.z = 0;
|
||||
nodeEntry->nodeInfo.beamNo = 0;
|
||||
nodeEntry->ttlCnt = 0;
|
||||
|
||||
nodeEntry->nextNode = list->head;
|
||||
nodeEntry->preNode = list->head->nextNode;
|
||||
list->head->nextNode = nodeEntry;
|
||||
|
||||
list->nodeNum++;
|
||||
return SUCCESS;
|
||||
}
|
||||
|
||||
/*****************************************************************************/
|
||||
/**
|
||||
* @brief Delete a node in list based on node id.
|
||||
*
|
||||
* @param list
|
||||
* @param nodeId
|
||||
*
|
||||
* @return SUCCESS Node has been successfully deleted.
|
||||
* Other Node not exist.
|
||||
*
|
||||
******************************************************************************/
|
||||
int32_t DeletNode(NODE_LIST *list, uint8_t nodeId)
|
||||
{
|
||||
ASSERT(list != NULL);
|
||||
NODE_ENTRY *nodeEntry = GetNode(list, nodeId);
|
||||
if (nodeEntry == NULL)
|
||||
{
|
||||
DBG_WARNING("Node [%d] in not in node list, skip deleting process.\n", nodeId);
|
||||
return SUCCESS;
|
||||
}
|
||||
nodeEntry->preNode->nextNode = nodeEntry->nextNode;
|
||||
nodeEntry->nextNode->preNode = nodeEntry->preNode;
|
||||
DBG_INFO("Deleting node [%d].\n", nodeId);
|
||||
free(nodeEntry);
|
||||
list->nodeNum--;
|
||||
return SUCCESS;
|
||||
}
|
||||
|
||||
/*****************************************************************************/
|
||||
/**
|
||||
* @brief Get the distance between me and the node.
|
||||
*
|
||||
* @param list
|
||||
* @param nodeId
|
||||
* @param dst A pointer to store distance.
|
||||
*
|
||||
* @return SUCCESS distance has been successfully store in dst
|
||||
* FAILURE Node dose not exit.
|
||||
*
|
||||
******************************************************************************/
|
||||
|
||||
int32_t GetDistance(NODE_LIST *list, COORD myCoord, uint8_t nodeId, double *dst)
|
||||
{
|
||||
ASSERT(list != NULL);
|
||||
NODE_ENTRY *nodeEntry = GetNode(list, nodeId);
|
||||
if (nodeEntry == NULL)
|
||||
{
|
||||
return FAILURE;
|
||||
}
|
||||
else
|
||||
{
|
||||
*dst = Coord_Distance(myCoord, nodeEntry->nodeInfo.pos);
|
||||
return SUCCESS;
|
||||
}
|
||||
}
|
||||
|
||||
/*****************************************************************************/
|
||||
/**
|
||||
* @brief Update node positions information.
|
||||
*
|
||||
* @param list
|
||||
* @param nodeId
|
||||
* @param newPos A structure stores the corresponding node position info.
|
||||
*
|
||||
* @return SUCCESS update successfully.
|
||||
* FAILURE node not exist.
|
||||
*
|
||||
******************************************************************************/
|
||||
int32_t UpdateNodePos(NODE_LIST *list, uint8_t nodeId, const COORD *newPos)
|
||||
{
|
||||
ASSERT(list != NULL);
|
||||
NODE_ENTRY *nodeEntry = GetNode(list, nodeId);
|
||||
if (nodeEntry == NULL)
|
||||
{
|
||||
return FAILURE;
|
||||
}
|
||||
else
|
||||
{
|
||||
memcpy(&(nodeEntry->nodeInfo.pos), newPos, sizeof(COORD));
|
||||
}
|
||||
return SUCCESS;
|
||||
}
|
||||
|
||||
/*****************************************************************************/
|
||||
/**
|
||||
* @brief Get node positions information.
|
||||
*
|
||||
* @param list
|
||||
* @param nodeId
|
||||
* @param newPos A structure stores the corresponding node position info.
|
||||
*
|
||||
* @return SUCCESS update successfully.
|
||||
* FAILURE node not exist.
|
||||
*
|
||||
******************************************************************************/
|
||||
int32_t GetNodePos(NODE_LIST *list, uint8_t nodeId, COORD *pos)
|
||||
{
|
||||
ASSERT(list != NULL);
|
||||
NODE_ENTRY *nodeEntry = GetNode(list, nodeId);
|
||||
if (nodeEntry == NULL)
|
||||
{
|
||||
return FAILURE;
|
||||
}
|
||||
else
|
||||
{
|
||||
memcpy(pos, &(nodeEntry->nodeInfo.pos), sizeof(COORD));
|
||||
}
|
||||
return SUCCESS;
|
||||
}
|
||||
|
||||
/*****************************************************************************/
|
||||
/**
|
||||
* @brief Update node speed information.
|
||||
*
|
||||
* @param list
|
||||
* @param nodeId
|
||||
* @param newSpeed A structure stores the corresponding node speed info.
|
||||
*
|
||||
* @return SUCCESS update successfully.
|
||||
* FAILURE node not exist.
|
||||
*
|
||||
******************************************************************************/
|
||||
int32_t UpdateNodeSpeed(NODE_LIST *list, uint8_t nodeId, const COORD *newSpeed)
|
||||
{
|
||||
ASSERT(list != NULL);
|
||||
NODE_ENTRY *nodeEntry = GetNode(list, nodeId);
|
||||
if (nodeEntry == NULL)
|
||||
{
|
||||
return FAILURE;
|
||||
}
|
||||
else
|
||||
{
|
||||
memcpy(&(nodeEntry->nodeInfo.speed), newSpeed, sizeof(COORD));
|
||||
}
|
||||
return SUCCESS;
|
||||
}
|
||||
|
||||
/*****************************************************************************/
|
||||
/**
|
||||
* @brief Get node speed information.
|
||||
*
|
||||
* @param list
|
||||
* @param nodeId
|
||||
* @param newSpeed A structure stores the corresponding node speed info.
|
||||
*
|
||||
* @return SUCCESS update successfully.
|
||||
* FAILURE node not exist.
|
||||
*
|
||||
******************************************************************************/
|
||||
int32_t GetNodeSpeed(NODE_LIST *list, uint8_t nodeId, COORD *speed)
|
||||
{
|
||||
ASSERT(list != NULL);
|
||||
NODE_ENTRY *nodeEntry = GetNode(list, nodeId);
|
||||
if (nodeEntry == NULL)
|
||||
{
|
||||
return FAILURE;
|
||||
}
|
||||
else
|
||||
{
|
||||
memcpy(speed, &(nodeEntry->nodeInfo.speed), sizeof(COORD));
|
||||
}
|
||||
return SUCCESS;
|
||||
}
|
||||
/*****************************************************************************/
|
||||
/**
|
||||
* @brief Update node ACK status.
|
||||
*
|
||||
* @param list
|
||||
* @param nodeId
|
||||
* @param defl
|
||||
*
|
||||
* @return SUCCESS update successfully.
|
||||
* FAILURE Node not exist.
|
||||
*
|
||||
******************************************************************************/
|
||||
int32_t UpdateNodeTimeDeviation(NODE_LIST *list, uint8_t nodeId, int32_t devi)
|
||||
{
|
||||
ASSERT(list != NULL);
|
||||
NODE_ENTRY *nodeEntry = GetNode(list, nodeId);
|
||||
if (nodeEntry == NULL)
|
||||
{
|
||||
return FAILURE;
|
||||
}
|
||||
else
|
||||
{
|
||||
nodeEntry->timeDevi = devi;
|
||||
}
|
||||
return SUCCESS;
|
||||
}
|
||||
|
||||
/*****************************************************************************/
|
||||
/**
|
||||
* @brief Increase TTL of specified node in the list.
|
||||
*
|
||||
* @param list
|
||||
* @param inc cnt of slot to add
|
||||
* @param nodeId
|
||||
*
|
||||
* @return NONE
|
||||
*
|
||||
******************************************************************************/
|
||||
int32_t IncNodeTTL(NODE_LIST *list, uint8_t nodeId, uint32_t inc)
|
||||
{
|
||||
ASSERT(list != NULL);
|
||||
NODE_ENTRY *nodeEntry = GetNode(list, nodeId);
|
||||
if (nodeEntry == NULL)
|
||||
{
|
||||
return FAILURE;
|
||||
}
|
||||
else
|
||||
{
|
||||
nodeEntry->ttlCnt += inc;
|
||||
}
|
||||
return SUCCESS;
|
||||
}
|
||||
|
||||
/*****************************************************************************/
|
||||
/**
|
||||
* @brief Reset node TTL based on node id.
|
||||
*
|
||||
* @param list
|
||||
* @param nodeId
|
||||
*
|
||||
* @return SUCCESS TTL has been successfully cleared.
|
||||
* FAILURE Node not exist.
|
||||
*
|
||||
******************************************************************************/
|
||||
int32_t RstNodeTTL(NODE_LIST *list, uint8_t nodeId)
|
||||
{
|
||||
ASSERT(list != NULL);
|
||||
NODE_ENTRY *nodeEntry = GetNode(list, nodeId);
|
||||
if (nodeEntry == NULL)
|
||||
{
|
||||
return FAILURE;
|
||||
}
|
||||
else
|
||||
{
|
||||
nodeEntry->ttlCnt = 0;
|
||||
}
|
||||
return SUCCESS;
|
||||
}
|
||||
|
||||
/*****************************************************************************/
|
||||
/**
|
||||
* @brief Get TTL of specified node in the list.
|
||||
*
|
||||
* @param list
|
||||
* @param nodeId
|
||||
* @param ttl
|
||||
*
|
||||
* @return NONE
|
||||
*
|
||||
******************************************************************************/
|
||||
int32_t GetNodeTTL(NODE_LIST *list, uint8_t nodeId, uint32_t *ttl)
|
||||
{
|
||||
ASSERT(list != NULL);
|
||||
NODE_ENTRY *nodeEntry = GetNode(list, nodeId);
|
||||
if (nodeEntry == NULL)
|
||||
{
|
||||
return FAILURE;
|
||||
}
|
||||
else
|
||||
{
|
||||
*ttl = nodeEntry->ttlCnt;
|
||||
}
|
||||
return SUCCESS;
|
||||
}
|
||||
|
||||
/*****************************************************************************/
|
||||
/**
|
||||
* @brief Implements a edit operations to each node.
|
||||
*
|
||||
* @param list
|
||||
* @param param
|
||||
* @param operation A pointer to a function handled "NODE_ENTRY*" as input
|
||||
* which defines the operations need to be apply.
|
||||
*
|
||||
* @return SUCCESS Operations has been applied successfully to all nodes.
|
||||
* Other Error occurred.
|
||||
*
|
||||
******************************************************************************/
|
||||
int32_t IterEditNode(NODE_LIST *list, const void *param, int32_t (*operation)(NODE_ENTRY *, const void *))
|
||||
{
|
||||
ASSERT(list != NULL);
|
||||
int32_t Status = SUCCESS;
|
||||
NODE_ENTRY *nodeEntry = list->head->nextNode;
|
||||
while (nodeEntry != list->head)
|
||||
{
|
||||
Status = operation(nodeEntry, param);
|
||||
if (Status != SUCCESS)
|
||||
{
|
||||
break;
|
||||
}
|
||||
nodeEntry = nodeEntry->nextNode;
|
||||
}
|
||||
return Status;
|
||||
}
|
||||
|
||||
/*****************************************************************************/
|
||||
/**
|
||||
* @brief Filter all node, delete all the node not satisfied the condition.
|
||||
*
|
||||
* @param list
|
||||
* @param condition A pointer to a function handled "NODE_ENTRY*" as input
|
||||
* which defines the condition need to be verify.
|
||||
*
|
||||
* @return SUCCESS
|
||||
*
|
||||
******************************************************************************/
|
||||
int32_t IterDeleteNode(NODE_LIST *list, bool (*condition)(NODE_ENTRY *))
|
||||
{
|
||||
ASSERT(list != NULL);
|
||||
NODE_ENTRY *nodeEntry, *deletEntry;
|
||||
nodeEntry = list->head->nextNode;
|
||||
while (nodeEntry != list->head)
|
||||
{
|
||||
if (condition(nodeEntry) == false)
|
||||
{
|
||||
deletEntry = nodeEntry;
|
||||
nodeEntry = nodeEntry->nextNode;
|
||||
deletEntry->preNode->nextNode = deletEntry->nextNode;
|
||||
deletEntry->nextNode->preNode = deletEntry->preNode;
|
||||
DBG_INFO("Deleting node [%d].\n", deletEntry->nodeInfo.nodeId);
|
||||
free(deletEntry);
|
||||
list->nodeNum--;
|
||||
}
|
||||
else
|
||||
{
|
||||
nodeEntry = nodeEntry->nextNode;
|
||||
}
|
||||
}
|
||||
return SUCCESS;
|
||||
}
|
||||
@@ -0,0 +1,66 @@
|
||||
/*****************************************************************************/
|
||||
/**
|
||||
* Description: Header of 814 linklayer local record.
|
||||
*
|
||||
* Date: 2022-07-14 00:08
|
||||
* LastEditTime: 2022-07-14 22:52
|
||||
* Revisions: Rev 0.0
|
||||
* History:
|
||||
* [2022-07-14]: Initial version.
|
||||
*
|
||||
* Copyright(c) 2021 Beijing Institute of Technology.
|
||||
* Lab of Communication and Networking
|
||||
*
|
||||
******************************************************************************/
|
||||
|
||||
/***************************** Include Files *********************************/
|
||||
#ifndef __L2_LOCAL_RECORD_H_
|
||||
#define __L2_LOCAL_RECORD_H_
|
||||
#include <stdlib.h>
|
||||
#include <stdio.h>
|
||||
#include <math.h>
|
||||
#include <stdbool.h>
|
||||
#include "util.h"
|
||||
#include "link_layer_types.h"
|
||||
|
||||
/************************** Constant Definitions *****************************/
|
||||
/***************** Macros (Inline Functions) Definitions *********************/
|
||||
/**************************** Type Definitions *******************************/
|
||||
typedef struct NODE_LIST_ENTRY
|
||||
{
|
||||
L2_NODE_INFO nodeInfo;
|
||||
uint32_t ttlCnt;
|
||||
int32_t timeDevi;
|
||||
struct NODE_LIST_ENTRY *nextNode;
|
||||
struct NODE_LIST_ENTRY *preNode;
|
||||
} NODE_ENTRY;
|
||||
|
||||
typedef struct
|
||||
{
|
||||
uint32_t nodeNum;
|
||||
NODE_ENTRY *head;
|
||||
} NODE_LIST;
|
||||
|
||||
/************************** Variable Definitions *****************************/
|
||||
|
||||
/************************** Function Prototypes ******************************/
|
||||
int32_t NodeList_Init(NODE_LIST **list);
|
||||
void NodeList_Clear(NODE_LIST *list);
|
||||
void NodeList_Destroy(NODE_LIST *list);
|
||||
NODE_ENTRY *GetNode(NODE_LIST *list, uint32_t nodeId);
|
||||
int32_t AppendNode(NODE_LIST *list, uint8_t nodeId, L2_NODE_TYPE nodeType);
|
||||
int32_t DeletNode(NODE_LIST *list, uint8_t nodeId);
|
||||
int32_t UpdateNodePos(NODE_LIST *list, uint8_t nodeId, const COORD *newPos);
|
||||
int32_t GetNodePos(NODE_LIST *list, uint8_t nodeId, COORD *newPos);
|
||||
int32_t UpdateNodeSpeed(NODE_LIST *list, uint8_t nodeId, const COORD *newSpeed);
|
||||
int32_t GetNodeSpeed(NODE_LIST *list, uint8_t nodeId, COORD *speed);
|
||||
int32_t UpdateNodeTimeDeviation(NODE_LIST *list, uint8_t nodeId, int32_t devi);
|
||||
int32_t GetDistance(NODE_LIST *list, COORD myCoord, uint8_t nodeId, double *dst);
|
||||
int32_t GetNodeNum(NODE_LIST *list);
|
||||
int32_t RstNodeTTL(NODE_LIST *list, uint8_t nodeId);
|
||||
int32_t IncNodeTTL(NODE_LIST *list, uint8_t nodeId, uint32_t inc);
|
||||
int32_t GetNodeTTL(NODE_LIST *list, uint8_t nodeId, uint32_t *ttl);
|
||||
bool IsNodeInList(NODE_LIST *list, uint8_t nodeId);
|
||||
int32_t IterEditNode(NODE_LIST *list, const void *param, int32_t (*operation)(NODE_ENTRY *, const void *));
|
||||
int32_t IterDeleteNode(NODE_LIST *list, bool (*condition)(NODE_ENTRY *));
|
||||
#endif /* __L2_LOCAL_RECORD_H_ */
|
||||
@@ -0,0 +1,928 @@
|
||||
/*****************************************************************************/
|
||||
/**
|
||||
* Description: Time allocations for df datalink,
|
||||
* Date: 2022-07-16 17:05
|
||||
* LastEditTime: 2022-07-16 17:05
|
||||
* Revisions: Rev 0.1
|
||||
* History:
|
||||
* [2021-11-13]: Initial version.
|
||||
*
|
||||
* Copyright(c) 2021 Beijing Institute of Technology.
|
||||
* Lab of Communication and Networking
|
||||
*
|
||||
******************************************************************************/
|
||||
|
||||
#include <inttypes.h>
|
||||
#include "link_layer.h"
|
||||
#include "link_layer_types.h"
|
||||
#include "../config/app_config.h"
|
||||
|
||||
#ifndef DATALINK_ENABLE_BERTEST
|
||||
L2_RTNE_TABLE_ENTRY normalSub1[L2_SLOTS_PER_FRAME] = {
|
||||
// SubFrame 1
|
||||
{L2_SLOT_SERVICE, 0x01, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x02, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x03, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x04, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x05, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x06, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x07, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x08, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x09, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x0A, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x0B, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x0C, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x0D, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x0E, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x0F, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x10, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x11, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x12, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x13, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x14, 0x01, 0x02},
|
||||
|
||||
// SubFrame 2
|
||||
{L2_SLOT_SERVICE, 0x01, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x02, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x03, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x04, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x05, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x06, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x07, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x08, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x09, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x0A, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x0B, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x0C, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x0D, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x0E, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x0F, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x10, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x11, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x12, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x13, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x14, 0x01, 0x02},
|
||||
|
||||
// SubFrame 3
|
||||
{L2_SLOT_SERVICE, 0x01, 0x02, 0x01},
|
||||
{L2_SLOT_SERVICE, 0x02, 0x02, 0x01},
|
||||
{L2_SLOT_SERVICE, 0x03, 0x02, 0x01},
|
||||
{L2_SLOT_SERVICE, 0x04, 0x02, 0x01},
|
||||
{L2_SLOT_SERVICE, 0x05, 0x02, 0x01},
|
||||
{L2_SLOT_SERVICE, 0x06, 0x02, 0x01},
|
||||
{L2_SLOT_SERVICE, 0x07, 0x02, 0x01},
|
||||
{L2_SLOT_SERVICE, 0x08, 0x02, 0x01},
|
||||
{L2_SLOT_SERVICE, 0x09, 0x02, 0x01},
|
||||
{L2_SLOT_SERVICE, 0x0A, 0x02, 0x01},
|
||||
{L2_SLOT_SERVICE, 0x0B, 0x02, 0x01},
|
||||
{L2_SLOT_SERVICE, 0x0C, 0x02, 0x01},
|
||||
{L2_SLOT_SERVICE, 0x0D, 0x02, 0x01},
|
||||
{L2_SLOT_SERVICE, 0x0E, 0x02, 0x01},
|
||||
{L2_SLOT_SERVICE, 0x0F, 0x02, 0x01},
|
||||
{L2_SLOT_SERVICE, 0x10, 0x02, 0x01},
|
||||
{L2_SLOT_SERVICE, 0x11, 0x02, 0x01},
|
||||
{L2_SLOT_SERVICE, 0x12, 0x02, 0x01},
|
||||
{L2_SLOT_SERVICE, 0x13, 0x02, 0x01},
|
||||
{L2_SLOT_SERVICE, 0x14, 0x02, 0x01},
|
||||
|
||||
// SubFrame 4
|
||||
{L2_SLOT_SERVICE, 0x01, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x02, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x03, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x04, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x05, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x06, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x07, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x08, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x09, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x0A, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x0B, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x0C, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x0D, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x0E, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x0F, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x10, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x11, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x12, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x13, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x14, 0x01, 0x02},
|
||||
|
||||
// SubFrame 5
|
||||
{L2_SLOT_SERVICE, 0x01, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x02, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x03, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x04, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x05, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x06, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x07, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x08, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x09, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x0A, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x0B, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x0C, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x0D, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x0E, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x0F, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x10, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x11, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x12, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x13, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x14, 0x01, 0x02},
|
||||
|
||||
// SubFrame 6
|
||||
{L2_SLOT_SERVICE, 0x01, 0x02, 0x01},
|
||||
{L2_SLOT_SERVICE, 0x02, 0x02, 0x01},
|
||||
{L2_SLOT_SERVICE, 0x03, 0x02, 0x01},
|
||||
{L2_SLOT_SERVICE, 0x04, 0x02, 0x01},
|
||||
{L2_SLOT_SERVICE, 0x05, 0x02, 0x01},
|
||||
{L2_SLOT_SERVICE, 0x06, 0x02, 0x01},
|
||||
{L2_SLOT_SERVICE, 0x07, 0x02, 0x01},
|
||||
{L2_SLOT_SERVICE, 0x08, 0x02, 0x01},
|
||||
{L2_SLOT_SERVICE, 0x09, 0x02, 0x01},
|
||||
{L2_SLOT_SERVICE, 0x0A, 0x02, 0x01},
|
||||
{L2_SLOT_SERVICE, 0x0B, 0x02, 0x01},
|
||||
{L2_SLOT_SERVICE, 0x0C, 0x02, 0x01},
|
||||
{L2_SLOT_SERVICE, 0x0D, 0x02, 0x01},
|
||||
{L2_SLOT_SERVICE, 0x0E, 0x02, 0x01},
|
||||
{L2_SLOT_SERVICE, 0x0F, 0x02, 0x01},
|
||||
{L2_SLOT_SERVICE, 0x10, 0x02, 0x01},
|
||||
{L2_SLOT_SERVICE, 0x11, 0x02, 0x01},
|
||||
{L2_SLOT_SERVICE, 0x12, 0x02, 0x01},
|
||||
{L2_SLOT_SERVICE, 0x13, 0x02, 0x01},
|
||||
{L2_SLOT_SERVICE, 0x14, 0x02, 0x01},
|
||||
|
||||
// SubFrame 7
|
||||
{L2_SLOT_SERVICE, 0x01, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x02, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x03, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x04, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x05, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x06, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x07, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x08, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x09, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x0A, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x0B, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x0C, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x0D, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x0E, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x0F, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x10, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x11, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x12, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x13, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x14, 0x01, 0x02},
|
||||
|
||||
// SubFrame 8
|
||||
{L2_SLOT_SERVICE, 0x01, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x02, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x03, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x04, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x05, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x06, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x07, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x08, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x09, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x0A, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x0B, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x0C, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x0D, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x0E, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x0F, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x10, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x11, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x12, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x13, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x14, 0x01, 0x02},
|
||||
|
||||
// SubFrame 9
|
||||
{L2_SLOT_SERVICE, 0x01, 0x02, 0x01},
|
||||
{L2_SLOT_SERVICE, 0x02, 0x02, 0x01},
|
||||
{L2_SLOT_SERVICE, 0x03, 0x02, 0x01},
|
||||
{L2_SLOT_SERVICE, 0x04, 0x02, 0x01},
|
||||
{L2_SLOT_SERVICE, 0x05, 0x02, 0x01},
|
||||
{L2_SLOT_SERVICE, 0x06, 0x02, 0x01},
|
||||
{L2_SLOT_SERVICE, 0x07, 0x02, 0x01},
|
||||
{L2_SLOT_SERVICE, 0x08, 0x02, 0x01},
|
||||
{L2_SLOT_SERVICE, 0x09, 0x02, 0x01},
|
||||
{L2_SLOT_SERVICE, 0x0A, 0x02, 0x01},
|
||||
{L2_SLOT_SERVICE, 0x0B, 0x02, 0x01},
|
||||
{L2_SLOT_SERVICE, 0x0C, 0x02, 0x01},
|
||||
{L2_SLOT_SERVICE, 0x0D, 0x02, 0x01},
|
||||
{L2_SLOT_SERVICE, 0x0E, 0x02, 0x01},
|
||||
{L2_SLOT_SERVICE, 0x0F, 0x02, 0x01},
|
||||
{L2_SLOT_SERVICE, 0x10, 0x02, 0x01},
|
||||
{L2_SLOT_SERVICE, 0x11, 0x02, 0x01},
|
||||
{L2_SLOT_SERVICE, 0x12, 0x02, 0x01},
|
||||
{L2_SLOT_SERVICE, 0x13, 0x02, 0x01},
|
||||
{L2_SLOT_SERVICE, 0x14, 0x02, 0x01},
|
||||
|
||||
// SubFrame 10
|
||||
{L2_SLOT_SERVICE, 0x01, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x02, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x03, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x04, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x05, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x06, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x07, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x08, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x09, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x0A, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x0B, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x0C, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x0D, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x0E, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x0F, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x10, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x11, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x12, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x13, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x14, 0x01, 0x02}};
|
||||
|
||||
L2_RTNE_TABLE_ENTRY normalSub2[L2_SLOTS_PER_FRAME] = {
|
||||
// SubFrame 1
|
||||
{L2_SLOT_SERVICE, 0x01, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x02, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x03, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x04, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x05, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x06, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x07, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x08, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x09, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x0A, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x0B, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x0C, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x0D, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x0E, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x0F, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x10, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x11, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x12, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x13, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x14, 0x01, 0x02},
|
||||
|
||||
// SubFrame 2
|
||||
{L2_SLOT_SERVICE, 0x01, 0x02, 0x01},
|
||||
{L2_SLOT_SERVICE, 0x02, 0x02, 0x01},
|
||||
{L2_SLOT_SERVICE, 0x03, 0x02, 0x01},
|
||||
{L2_SLOT_SERVICE, 0x04, 0x02, 0x01},
|
||||
{L2_SLOT_SERVICE, 0x05, 0x02, 0x01},
|
||||
{L2_SLOT_SERVICE, 0x06, 0x02, 0x01},
|
||||
{L2_SLOT_SERVICE, 0x07, 0x02, 0x01},
|
||||
{L2_SLOT_SERVICE, 0x08, 0x02, 0x01},
|
||||
{L2_SLOT_SERVICE, 0x09, 0x02, 0x01},
|
||||
{L2_SLOT_SERVICE, 0x0A, 0x02, 0x01},
|
||||
{L2_SLOT_SERVICE, 0x0B, 0x02, 0x01},
|
||||
{L2_SLOT_SERVICE, 0x0C, 0x02, 0x01},
|
||||
{L2_SLOT_SERVICE, 0x0D, 0x02, 0x01},
|
||||
{L2_SLOT_SERVICE, 0x0E, 0x02, 0x01},
|
||||
{L2_SLOT_SERVICE, 0x0F, 0x02, 0x01},
|
||||
{L2_SLOT_SERVICE, 0x10, 0x02, 0x01},
|
||||
{L2_SLOT_SERVICE, 0x11, 0x02, 0x01},
|
||||
{L2_SLOT_SERVICE, 0x12, 0x02, 0x01},
|
||||
{L2_SLOT_SERVICE, 0x13, 0x02, 0x01},
|
||||
{L2_SLOT_SERVICE, 0x14, 0x02, 0x01},
|
||||
|
||||
// SubFrame 3
|
||||
{L2_SLOT_SERVICE, 0x01, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x02, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x03, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x04, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x05, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x06, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x07, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x08, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x09, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x0A, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x0B, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x0C, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x0D, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x0E, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x0F, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x10, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x11, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x12, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x13, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x14, 0x01, 0x02},
|
||||
|
||||
// SubFrame 4
|
||||
{L2_SLOT_SERVICE, 0x01, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x02, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x03, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x04, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x05, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x06, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x07, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x08, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x09, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x0A, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x0B, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x0C, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x0D, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x0E, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x0F, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x10, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x11, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x12, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x13, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x14, 0x01, 0x02},
|
||||
|
||||
// SubFrame 5
|
||||
{L2_SLOT_SERVICE, 0x01, 0x02, 0x01},
|
||||
{L2_SLOT_SERVICE, 0x02, 0x02, 0x01},
|
||||
{L2_SLOT_SERVICE, 0x03, 0x02, 0x01},
|
||||
{L2_SLOT_SERVICE, 0x04, 0x02, 0x01},
|
||||
{L2_SLOT_SERVICE, 0x05, 0x02, 0x01},
|
||||
{L2_SLOT_SERVICE, 0x06, 0x02, 0x01},
|
||||
{L2_SLOT_SERVICE, 0x07, 0x02, 0x01},
|
||||
{L2_SLOT_SERVICE, 0x08, 0x02, 0x01},
|
||||
{L2_SLOT_SERVICE, 0x09, 0x02, 0x01},
|
||||
{L2_SLOT_SERVICE, 0x0A, 0x02, 0x01},
|
||||
{L2_SLOT_SERVICE, 0x0B, 0x02, 0x01},
|
||||
{L2_SLOT_SERVICE, 0x0C, 0x02, 0x01},
|
||||
{L2_SLOT_SERVICE, 0x0D, 0x02, 0x01},
|
||||
{L2_SLOT_SERVICE, 0x0E, 0x02, 0x01},
|
||||
{L2_SLOT_SERVICE, 0x0F, 0x02, 0x01},
|
||||
{L2_SLOT_SERVICE, 0x10, 0x02, 0x01},
|
||||
{L2_SLOT_SERVICE, 0x11, 0x02, 0x01},
|
||||
{L2_SLOT_SERVICE, 0x12, 0x02, 0x01},
|
||||
{L2_SLOT_SERVICE, 0x13, 0x02, 0x01},
|
||||
{L2_SLOT_SERVICE, 0x14, 0x02, 0x01},
|
||||
|
||||
// SubFrame 6
|
||||
{L2_SLOT_SERVICE, 0x01, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x02, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x03, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x04, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x05, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x06, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x07, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x08, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x09, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x0A, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x0B, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x0C, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x0D, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x0E, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x0F, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x10, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x11, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x12, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x13, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x14, 0x01, 0x02},
|
||||
|
||||
// SubFrame 7
|
||||
{L2_SLOT_SERVICE, 0x01, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x02, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x03, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x04, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x05, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x06, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x07, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x08, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x09, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x0A, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x0B, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x0C, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x0D, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x0E, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x0F, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x10, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x11, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x12, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x13, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x14, 0x01, 0x02},
|
||||
|
||||
// SubFrame 8
|
||||
{L2_SLOT_SERVICE, 0x01, 0x02, 0x01},
|
||||
{L2_SLOT_SERVICE, 0x02, 0x02, 0x01},
|
||||
{L2_SLOT_SERVICE, 0x03, 0x02, 0x01},
|
||||
{L2_SLOT_SERVICE, 0x04, 0x02, 0x01},
|
||||
{L2_SLOT_SERVICE, 0x05, 0x02, 0x01},
|
||||
{L2_SLOT_SERVICE, 0x06, 0x02, 0x01},
|
||||
{L2_SLOT_SERVICE, 0x07, 0x02, 0x01},
|
||||
{L2_SLOT_SERVICE, 0x08, 0x02, 0x01},
|
||||
{L2_SLOT_SERVICE, 0x09, 0x02, 0x01},
|
||||
{L2_SLOT_SERVICE, 0x0A, 0x02, 0x01},
|
||||
{L2_SLOT_SERVICE, 0x0B, 0x02, 0x01},
|
||||
{L2_SLOT_SERVICE, 0x0C, 0x02, 0x01},
|
||||
{L2_SLOT_SERVICE, 0x0D, 0x02, 0x01},
|
||||
{L2_SLOT_SERVICE, 0x0E, 0x02, 0x01},
|
||||
{L2_SLOT_SERVICE, 0x0F, 0x02, 0x01},
|
||||
{L2_SLOT_SERVICE, 0x10, 0x02, 0x01},
|
||||
{L2_SLOT_SERVICE, 0x11, 0x02, 0x01},
|
||||
{L2_SLOT_SERVICE, 0x12, 0x02, 0x01},
|
||||
{L2_SLOT_SERVICE, 0x13, 0x02, 0x01},
|
||||
{L2_SLOT_SERVICE, 0x14, 0x02, 0x01},
|
||||
|
||||
// SubFrame 9
|
||||
{L2_SLOT_SERVICE, 0x01, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x02, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x03, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x04, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x05, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x06, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x07, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x08, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x09, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x0A, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x0B, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x0C, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x0D, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x0E, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x0F, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x10, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x11, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x12, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x13, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x14, 0x01, 0x02},
|
||||
|
||||
// SubFrame 10
|
||||
{L2_SLOT_SERVICE, 0x01, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x02, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x03, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x04, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x05, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x06, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x07, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x08, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x09, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x0A, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x0B, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x0C, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x0D, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x0E, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x0F, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x10, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x11, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x12, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x13, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x14, 0x01, 0x02}};
|
||||
|
||||
L2_RTNE_TABLE_ENTRY normalSub3[L2_SLOTS_PER_FRAME] = {
|
||||
// SubFrame 1
|
||||
{L2_SLOT_SERVICE, 0x01, 0x02, 0x01},
|
||||
{L2_SLOT_SERVICE, 0x02, 0x02, 0x01},
|
||||
{L2_SLOT_SERVICE, 0x03, 0x02, 0x01},
|
||||
{L2_SLOT_SERVICE, 0x04, 0x02, 0x01},
|
||||
{L2_SLOT_SERVICE, 0x05, 0x02, 0x01},
|
||||
{L2_SLOT_SERVICE, 0x06, 0x02, 0x01},
|
||||
{L2_SLOT_SERVICE, 0x07, 0x02, 0x01},
|
||||
{L2_SLOT_SERVICE, 0x08, 0x02, 0x01},
|
||||
{L2_SLOT_SERVICE, 0x09, 0x02, 0x01},
|
||||
{L2_SLOT_SERVICE, 0x0A, 0x02, 0x01},
|
||||
{L2_SLOT_SERVICE, 0x0B, 0x02, 0x01},
|
||||
{L2_SLOT_SERVICE, 0x0C, 0x02, 0x01},
|
||||
{L2_SLOT_SERVICE, 0x0D, 0x02, 0x01},
|
||||
{L2_SLOT_SERVICE, 0x0E, 0x02, 0x01},
|
||||
{L2_SLOT_SERVICE, 0x0F, 0x02, 0x01},
|
||||
{L2_SLOT_SERVICE, 0x10, 0x02, 0x01},
|
||||
{L2_SLOT_SERVICE, 0x11, 0x02, 0x01},
|
||||
{L2_SLOT_SERVICE, 0x12, 0x02, 0x01},
|
||||
{L2_SLOT_SERVICE, 0x13, 0x02, 0x01},
|
||||
{L2_SLOT_SERVICE, 0x14, 0x02, 0x01},
|
||||
|
||||
// SubFrame 2
|
||||
{L2_SLOT_SERVICE, 0x01, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x02, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x03, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x04, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x05, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x06, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x07, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x08, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x09, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x0A, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x0B, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x0C, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x0D, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x0E, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x0F, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x10, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x11, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x12, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x13, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x14, 0x01, 0x02},
|
||||
|
||||
// SubFrame 3
|
||||
{L2_SLOT_SERVICE, 0x01, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x02, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x03, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x04, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x05, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x06, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x07, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x08, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x09, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x0A, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x0B, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x0C, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x0D, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x0E, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x0F, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x10, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x11, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x12, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x13, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x14, 0x01, 0x02},
|
||||
|
||||
// SubFrame 4
|
||||
{L2_SLOT_SERVICE, 0x01, 0x02, 0x01},
|
||||
{L2_SLOT_SERVICE, 0x02, 0x02, 0x01},
|
||||
{L2_SLOT_SERVICE, 0x03, 0x02, 0x01},
|
||||
{L2_SLOT_SERVICE, 0x04, 0x02, 0x01},
|
||||
{L2_SLOT_SERVICE, 0x05, 0x02, 0x01},
|
||||
{L2_SLOT_SERVICE, 0x06, 0x02, 0x01},
|
||||
{L2_SLOT_SERVICE, 0x07, 0x02, 0x01},
|
||||
{L2_SLOT_SERVICE, 0x08, 0x02, 0x01},
|
||||
{L2_SLOT_SERVICE, 0x09, 0x02, 0x01},
|
||||
{L2_SLOT_SERVICE, 0x0A, 0x02, 0x01},
|
||||
{L2_SLOT_SERVICE, 0x0B, 0x02, 0x01},
|
||||
{L2_SLOT_SERVICE, 0x0C, 0x02, 0x01},
|
||||
{L2_SLOT_SERVICE, 0x0D, 0x02, 0x01},
|
||||
{L2_SLOT_SERVICE, 0x0E, 0x02, 0x01},
|
||||
{L2_SLOT_SERVICE, 0x0F, 0x02, 0x01},
|
||||
{L2_SLOT_SERVICE, 0x10, 0x02, 0x01},
|
||||
{L2_SLOT_SERVICE, 0x11, 0x02, 0x01},
|
||||
{L2_SLOT_SERVICE, 0x12, 0x02, 0x01},
|
||||
{L2_SLOT_SERVICE, 0x13, 0x02, 0x01},
|
||||
{L2_SLOT_SERVICE, 0x14, 0x02, 0x01},
|
||||
|
||||
// SubFrame 5
|
||||
{L2_SLOT_SERVICE, 0x01, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x02, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x03, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x04, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x05, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x06, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x07, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x08, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x09, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x0A, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x0B, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x0C, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x0D, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x0E, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x0F, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x10, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x11, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x12, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x13, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x14, 0x01, 0x02},
|
||||
|
||||
// SubFrame 6
|
||||
{L2_SLOT_SERVICE, 0x01, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x02, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x03, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x04, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x05, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x06, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x07, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x08, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x09, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x0A, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x0B, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x0C, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x0D, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x0E, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x0F, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x10, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x11, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x12, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x13, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x14, 0x01, 0x02},
|
||||
|
||||
// SubFrame 7
|
||||
{L2_SLOT_SERVICE, 0x01, 0x02, 0x01},
|
||||
{L2_SLOT_SERVICE, 0x02, 0x02, 0x01},
|
||||
{L2_SLOT_SERVICE, 0x03, 0x02, 0x01},
|
||||
{L2_SLOT_SERVICE, 0x04, 0x02, 0x01},
|
||||
{L2_SLOT_SERVICE, 0x05, 0x02, 0x01},
|
||||
{L2_SLOT_SERVICE, 0x06, 0x02, 0x01},
|
||||
{L2_SLOT_SERVICE, 0x07, 0x02, 0x01},
|
||||
{L2_SLOT_SERVICE, 0x08, 0x02, 0x01},
|
||||
{L2_SLOT_SERVICE, 0x09, 0x02, 0x01},
|
||||
{L2_SLOT_SERVICE, 0x0A, 0x02, 0x01},
|
||||
{L2_SLOT_SERVICE, 0x0B, 0x02, 0x01},
|
||||
{L2_SLOT_SERVICE, 0x0C, 0x02, 0x01},
|
||||
{L2_SLOT_SERVICE, 0x0D, 0x02, 0x01},
|
||||
{L2_SLOT_SERVICE, 0x0E, 0x02, 0x01},
|
||||
{L2_SLOT_SERVICE, 0x0F, 0x02, 0x01},
|
||||
{L2_SLOT_SERVICE, 0x10, 0x02, 0x01},
|
||||
{L2_SLOT_SERVICE, 0x11, 0x02, 0x01},
|
||||
{L2_SLOT_SERVICE, 0x12, 0x02, 0x01},
|
||||
{L2_SLOT_SERVICE, 0x13, 0x02, 0x01},
|
||||
{L2_SLOT_SERVICE, 0x14, 0x02, 0x01},
|
||||
|
||||
// SubFrame 8
|
||||
{L2_SLOT_SERVICE, 0x01, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x02, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x03, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x04, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x05, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x06, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x07, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x08, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x09, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x0A, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x0B, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x0C, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x0D, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x0E, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x0F, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x10, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x11, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x12, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x13, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x14, 0x01, 0x02},
|
||||
|
||||
// SubFrame 9
|
||||
{L2_SLOT_SERVICE, 0x01, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x02, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x03, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x04, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x05, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x06, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x07, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x08, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x09, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x0A, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x0B, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x0C, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x0D, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x0E, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x0F, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x10, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x11, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x12, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x13, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x14, 0x01, 0x02},
|
||||
|
||||
// SubFrame 10
|
||||
{L2_SLOT_SERVICE, 0x01, 0x02, 0x01},
|
||||
{L2_SLOT_SERVICE, 0x02, 0x02, 0x01},
|
||||
{L2_SLOT_SERVICE, 0x03, 0x02, 0x01},
|
||||
{L2_SLOT_SERVICE, 0x04, 0x02, 0x01},
|
||||
{L2_SLOT_SERVICE, 0x05, 0x02, 0x01},
|
||||
{L2_SLOT_SERVICE, 0x06, 0x02, 0x01},
|
||||
{L2_SLOT_SERVICE, 0x07, 0x02, 0x01},
|
||||
{L2_SLOT_SERVICE, 0x08, 0x02, 0x01},
|
||||
{L2_SLOT_SERVICE, 0x09, 0x02, 0x01},
|
||||
{L2_SLOT_SERVICE, 0x0A, 0x02, 0x01},
|
||||
{L2_SLOT_SERVICE, 0x0B, 0x02, 0x01},
|
||||
{L2_SLOT_SERVICE, 0x0C, 0x02, 0x01},
|
||||
{L2_SLOT_SERVICE, 0x0D, 0x02, 0x01},
|
||||
{L2_SLOT_SERVICE, 0x0E, 0x02, 0x01},
|
||||
{L2_SLOT_SERVICE, 0x0F, 0x02, 0x01},
|
||||
{L2_SLOT_SERVICE, 0x10, 0x02, 0x01},
|
||||
{L2_SLOT_SERVICE, 0x11, 0x02, 0x01},
|
||||
{L2_SLOT_SERVICE, 0x12, 0x02, 0x01},
|
||||
{L2_SLOT_SERVICE, 0x13, 0x02, 0x01},
|
||||
{L2_SLOT_SERVICE, 0x14, 0x02, 0x01}};
|
||||
|
||||
L2_RTNE_TABLE_ENTRY *normalTable[L2_RTNE_PERIOD] = {
|
||||
normalSub1, normalSub2, normalSub3, normalSub1, normalSub2, normalSub3,
|
||||
normalSub1, normalSub2, normalSub3, normalSub1, normalSub2, normalSub3,
|
||||
normalSub1, normalSub2, normalSub3, normalSub1, normalSub2, normalSub3,
|
||||
normalSub1, normalSub2, normalSub3, normalSub1, normalSub2, normalSub3,
|
||||
normalSub1, normalSub2, normalSub3, normalSub1, normalSub2, normalSub3,
|
||||
normalSub1, normalSub2, normalSub3, normalSub1, normalSub2, normalSub3,
|
||||
normalSub1, normalSub2, normalSub3, normalSub1, normalSub2, normalSub3,
|
||||
normalSub1, normalSub2, normalSub3, normalSub1, normalSub2, normalSub3,
|
||||
normalSub1, normalSub2, normalSub3, normalSub1, normalSub2, normalSub3,
|
||||
normalSub1, normalSub2, normalSub3, normalSub1, normalSub2, normalSub3};
|
||||
#else
|
||||
L2_RTNE_TABLE_ENTRY normalSub[L2_SLOTS_PER_FRAME] = {
|
||||
// SubFrame 1
|
||||
{L2_SLOT_SERVICE, 0x01, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x02, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x03, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x04, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x05, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x06, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x07, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x08, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x09, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x0A, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x0B, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x0C, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x0D, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x0E, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x0F, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x10, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x11, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x12, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x13, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x14, 0x01, 0x02},
|
||||
|
||||
// SubFrame 2
|
||||
{L2_SLOT_SERVICE, 0x01, 0x02, 0x01},
|
||||
{L2_SLOT_SERVICE, 0x02, 0x02, 0x01},
|
||||
{L2_SLOT_SERVICE, 0x03, 0x02, 0x01},
|
||||
{L2_SLOT_SERVICE, 0x04, 0x02, 0x01},
|
||||
{L2_SLOT_SERVICE, 0x05, 0x02, 0x01},
|
||||
{L2_SLOT_SERVICE, 0x06, 0x02, 0x01},
|
||||
{L2_SLOT_SERVICE, 0x07, 0x02, 0x01},
|
||||
{L2_SLOT_SERVICE, 0x08, 0x02, 0x01},
|
||||
{L2_SLOT_SERVICE, 0x09, 0x02, 0x01},
|
||||
{L2_SLOT_SERVICE, 0x0A, 0x02, 0x01},
|
||||
{L2_SLOT_SERVICE, 0x0B, 0x02, 0x01},
|
||||
{L2_SLOT_SERVICE, 0x0C, 0x02, 0x01},
|
||||
{L2_SLOT_SERVICE, 0x0D, 0x02, 0x01},
|
||||
{L2_SLOT_SERVICE, 0x0E, 0x02, 0x01},
|
||||
{L2_SLOT_SERVICE, 0x0F, 0x02, 0x01},
|
||||
{L2_SLOT_SERVICE, 0x10, 0x02, 0x01},
|
||||
{L2_SLOT_SERVICE, 0x11, 0x02, 0x01},
|
||||
{L2_SLOT_SERVICE, 0x12, 0x02, 0x01},
|
||||
{L2_SLOT_SERVICE, 0x13, 0x02, 0x01},
|
||||
{L2_SLOT_SERVICE, 0x14, 0x02, 0x01},
|
||||
|
||||
// SubFrame 3
|
||||
{L2_SLOT_SERVICE, 0x01, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x02, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x03, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x04, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x05, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x06, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x07, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x08, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x09, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x0A, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x0B, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x0C, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x0D, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x0E, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x0F, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x10, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x11, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x12, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x13, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x14, 0x01, 0x02},
|
||||
|
||||
// SubFrame 4
|
||||
{L2_SLOT_SERVICE, 0x01, 0x02, 0x01},
|
||||
{L2_SLOT_SERVICE, 0x02, 0x02, 0x01},
|
||||
{L2_SLOT_SERVICE, 0x03, 0x02, 0x01},
|
||||
{L2_SLOT_SERVICE, 0x04, 0x02, 0x01},
|
||||
{L2_SLOT_SERVICE, 0x05, 0x02, 0x01},
|
||||
{L2_SLOT_SERVICE, 0x06, 0x02, 0x01},
|
||||
{L2_SLOT_SERVICE, 0x07, 0x02, 0x01},
|
||||
{L2_SLOT_SERVICE, 0x08, 0x02, 0x01},
|
||||
{L2_SLOT_SERVICE, 0x09, 0x02, 0x01},
|
||||
{L2_SLOT_SERVICE, 0x0A, 0x02, 0x01},
|
||||
{L2_SLOT_SERVICE, 0x0B, 0x02, 0x01},
|
||||
{L2_SLOT_SERVICE, 0x0C, 0x02, 0x01},
|
||||
{L2_SLOT_SERVICE, 0x0D, 0x02, 0x01},
|
||||
{L2_SLOT_SERVICE, 0x0E, 0x02, 0x01},
|
||||
{L2_SLOT_SERVICE, 0x0F, 0x02, 0x01},
|
||||
{L2_SLOT_SERVICE, 0x10, 0x02, 0x01},
|
||||
{L2_SLOT_SERVICE, 0x11, 0x02, 0x01},
|
||||
{L2_SLOT_SERVICE, 0x12, 0x02, 0x01},
|
||||
{L2_SLOT_SERVICE, 0x13, 0x02, 0x01},
|
||||
{L2_SLOT_SERVICE, 0x14, 0x02, 0x01},
|
||||
|
||||
// SubFrame 5
|
||||
{L2_SLOT_SERVICE, 0x01, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x02, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x03, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x04, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x05, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x06, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x07, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x08, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x09, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x0A, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x0B, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x0C, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x0D, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x0E, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x0F, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x10, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x11, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x12, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x13, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x14, 0x01, 0x02},
|
||||
|
||||
// SubFrame 6
|
||||
{L2_SLOT_SERVICE, 0x01, 0x02, 0x01},
|
||||
{L2_SLOT_SERVICE, 0x02, 0x02, 0x01},
|
||||
{L2_SLOT_SERVICE, 0x03, 0x02, 0x01},
|
||||
{L2_SLOT_SERVICE, 0x04, 0x02, 0x01},
|
||||
{L2_SLOT_SERVICE, 0x05, 0x02, 0x01},
|
||||
{L2_SLOT_SERVICE, 0x06, 0x02, 0x01},
|
||||
{L2_SLOT_SERVICE, 0x07, 0x02, 0x01},
|
||||
{L2_SLOT_SERVICE, 0x08, 0x02, 0x01},
|
||||
{L2_SLOT_SERVICE, 0x09, 0x02, 0x01},
|
||||
{L2_SLOT_SERVICE, 0x0A, 0x02, 0x01},
|
||||
{L2_SLOT_SERVICE, 0x0B, 0x02, 0x01},
|
||||
{L2_SLOT_SERVICE, 0x0C, 0x02, 0x01},
|
||||
{L2_SLOT_SERVICE, 0x0D, 0x02, 0x01},
|
||||
{L2_SLOT_SERVICE, 0x0E, 0x02, 0x01},
|
||||
{L2_SLOT_SERVICE, 0x0F, 0x02, 0x01},
|
||||
{L2_SLOT_SERVICE, 0x10, 0x02, 0x01},
|
||||
{L2_SLOT_SERVICE, 0x11, 0x02, 0x01},
|
||||
{L2_SLOT_SERVICE, 0x12, 0x02, 0x01},
|
||||
{L2_SLOT_SERVICE, 0x13, 0x02, 0x01},
|
||||
{L2_SLOT_SERVICE, 0x14, 0x02, 0x01},
|
||||
|
||||
// SubFrame 7
|
||||
{L2_SLOT_SERVICE, 0x01, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x02, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x03, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x04, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x05, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x06, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x07, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x08, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x09, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x0A, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x0B, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x0C, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x0D, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x0E, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x0F, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x10, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x11, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x12, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x13, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x14, 0x01, 0x02},
|
||||
|
||||
// SubFrame 8
|
||||
{L2_SLOT_SERVICE, 0x01, 0x02, 0x01},
|
||||
{L2_SLOT_SERVICE, 0x02, 0x02, 0x01},
|
||||
{L2_SLOT_SERVICE, 0x03, 0x02, 0x01},
|
||||
{L2_SLOT_SERVICE, 0x04, 0x02, 0x01},
|
||||
{L2_SLOT_SERVICE, 0x05, 0x02, 0x01},
|
||||
{L2_SLOT_SERVICE, 0x06, 0x02, 0x01},
|
||||
{L2_SLOT_SERVICE, 0x07, 0x02, 0x01},
|
||||
{L2_SLOT_SERVICE, 0x08, 0x02, 0x01},
|
||||
{L2_SLOT_SERVICE, 0x09, 0x02, 0x01},
|
||||
{L2_SLOT_SERVICE, 0x0A, 0x02, 0x01},
|
||||
{L2_SLOT_SERVICE, 0x0B, 0x02, 0x01},
|
||||
{L2_SLOT_SERVICE, 0x0C, 0x02, 0x01},
|
||||
{L2_SLOT_SERVICE, 0x0D, 0x02, 0x01},
|
||||
{L2_SLOT_SERVICE, 0x0E, 0x02, 0x01},
|
||||
{L2_SLOT_SERVICE, 0x0F, 0x02, 0x01},
|
||||
{L2_SLOT_SERVICE, 0x10, 0x02, 0x01},
|
||||
{L2_SLOT_SERVICE, 0x11, 0x02, 0x01},
|
||||
{L2_SLOT_SERVICE, 0x12, 0x02, 0x01},
|
||||
{L2_SLOT_SERVICE, 0x13, 0x02, 0x01},
|
||||
{L2_SLOT_SERVICE, 0x14, 0x02, 0x01},
|
||||
|
||||
// SubFrame 9
|
||||
{L2_SLOT_SERVICE, 0x01, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x02, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x03, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x04, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x05, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x06, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x07, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x08, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x09, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x0A, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x0B, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x0C, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x0D, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x0E, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x0F, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x10, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x11, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x12, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x13, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x14, 0x01, 0x02},
|
||||
|
||||
// SubFrame 10
|
||||
{L2_SLOT_SERVICE, 0x01, 0x02, 0x01},
|
||||
{L2_SLOT_SERVICE, 0x02, 0x02, 0x01},
|
||||
{L2_SLOT_SERVICE, 0x03, 0x02, 0x01},
|
||||
{L2_SLOT_SERVICE, 0x04, 0x02, 0x01},
|
||||
{L2_SLOT_SERVICE, 0x05, 0x02, 0x01},
|
||||
{L2_SLOT_SERVICE, 0x06, 0x02, 0x01},
|
||||
{L2_SLOT_SERVICE, 0x07, 0x02, 0x01},
|
||||
{L2_SLOT_SERVICE, 0x08, 0x02, 0x01},
|
||||
{L2_SLOT_SERVICE, 0x09, 0x02, 0x01},
|
||||
{L2_SLOT_SERVICE, 0x0A, 0x02, 0x01},
|
||||
{L2_SLOT_SERVICE, 0x0B, 0x02, 0x01},
|
||||
{L2_SLOT_SERVICE, 0x0C, 0x02, 0x01},
|
||||
{L2_SLOT_SERVICE, 0x0D, 0x02, 0x01},
|
||||
{L2_SLOT_SERVICE, 0x0E, 0x02, 0x01},
|
||||
{L2_SLOT_SERVICE, 0x0F, 0x02, 0x01},
|
||||
{L2_SLOT_SERVICE, 0x10, 0x02, 0x01},
|
||||
{L2_SLOT_SERVICE, 0x11, 0x02, 0x01},
|
||||
{L2_SLOT_SERVICE, 0x12, 0x02, 0x01},
|
||||
{L2_SLOT_SERVICE, 0x13, 0x02, 0x01},
|
||||
{L2_SLOT_SERVICE, 0x14, 0x02, 0x01}};
|
||||
|
||||
L2_RTNE_TABLE_ENTRY *normalTable[L2_RTNE_PERIOD] = {
|
||||
normalSub, normalSub, normalSub, normalSub, normalSub, normalSub,
|
||||
normalSub, normalSub, normalSub, normalSub, normalSub, normalSub,
|
||||
normalSub, normalSub, normalSub, normalSub, normalSub, normalSub,
|
||||
normalSub, normalSub, normalSub, normalSub, normalSub, normalSub,
|
||||
normalSub, normalSub, normalSub, normalSub, normalSub, normalSub,
|
||||
normalSub, normalSub, normalSub, normalSub, normalSub, normalSub,
|
||||
normalSub, normalSub, normalSub, normalSub, normalSub, normalSub,
|
||||
normalSub, normalSub, normalSub, normalSub, normalSub, normalSub,
|
||||
normalSub, normalSub, normalSub, normalSub, normalSub, normalSub,
|
||||
normalSub, normalSub, normalSub, normalSub, normalSub, normalSub};
|
||||
#endif
|
||||
@@ -0,0 +1,354 @@
|
||||
/*****************************************************************************/
|
||||
/**
|
||||
* 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;
|
||||
}
|
||||
@@ -0,0 +1,524 @@
|
||||
/*****************************************************************************/
|
||||
/**
|
||||
* Description: Implementations for df datalink service net.
|
||||
* Date: 2021-11-16 00:08
|
||||
* LastEditTime: 2022-07-16 17:04
|
||||
* Revisions: Rev 0.1
|
||||
* History:
|
||||
* [2022-07-16]: New branch for DiJian.
|
||||
* [2021-11-16]: Initial version.
|
||||
*
|
||||
* Copyright(c) 2021 Beijing Institute of Technology.
|
||||
* Lab of Communication and Networking
|
||||
*
|
||||
******************************************************************************/
|
||||
|
||||
/***************************** Include Files *********************************/
|
||||
#include <string.h>
|
||||
#include <stdint.h>
|
||||
#include "error.h"
|
||||
#include "common.h"
|
||||
#include "circ_queue.h"
|
||||
#include "msg_def.h"
|
||||
#include "link_layer.h"
|
||||
#include "link_layer_types.h"
|
||||
#include "../config/app_config.h"
|
||||
|
||||
/************************** Constant Definitions *****************************/
|
||||
#define NUM_SERVICE_SLOTS 20
|
||||
#define NUM_RESPONSE_SLOTS 20
|
||||
#define NUM_NONE_SERVICE_SLOTS 0
|
||||
#define NUM_SERVICE_PRD_SLOTS 60
|
||||
/***************** Macros (Inline Functions) Definitions *********************/
|
||||
|
||||
/**************************** Type Definitions *******************************/
|
||||
|
||||
/************************** Function Prototypes ******************************/
|
||||
static int32_t Service_Packing(L2_INST *instPtr, L2_FRAME_TYPE type);
|
||||
/************************** Variable Definitions *****************************/
|
||||
|
||||
#ifdef DATALINK_ENABLE_BERTEST
|
||||
static uint8_t berTestData[SVC_PACK_LEN] = {
|
||||
0x27, 0x4E, 0xFB, 0x39, 0x11, 0xE7, 0x12, 0xDA,
|
||||
0xDF, 0xD2, 0x27, 0x8F, 0xC9, 0x42, 0xCD, 0x0C,
|
||||
0x76, 0x1C, 0x81, 0x87, 0x69, 0xC9, 0x60, 0xFD,
|
||||
0xC4, 0x27, 0xA5, 0x09, 0xE5, 0x71};
|
||||
#endif
|
||||
|
||||
/*****************************************************************************/
|
||||
/**
|
||||
* @brief Packing UDU Header to a bit array.
|
||||
*
|
||||
* @param bitarr point32_ter to bit array buffer.
|
||||
* dst Destination node address.
|
||||
* dataLen Length of UDU data.
|
||||
*
|
||||
* @return EPARAM
|
||||
* SUCCESS
|
||||
*
|
||||
******************************************************************************/
|
||||
static int32_t Service_Packing(L2_INST *instPtr, L2_FRAME_TYPE type)
|
||||
{
|
||||
#ifndef DATALINK_ENABLE_BERTEST
|
||||
ASSERT(instPtr != NULL);
|
||||
int32_t Status;
|
||||
uint8_t *buff;
|
||||
uint32_t packLen;
|
||||
switch (type)
|
||||
{
|
||||
case L2_SVC_FRAME:
|
||||
{
|
||||
const NODE_ENTRY *nodeEntry;
|
||||
int32_t timeDevi;
|
||||
packLen = SVC_PACK_LEN;
|
||||
|
||||
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);
|
||||
|
||||
// TODO: Check Pack TimeSync info, and better approch?.
|
||||
nodeEntry = GetNode(instPtr->nodeInfoList, L2_MEMBER_NODE_ID);
|
||||
if (nodeEntry != NULL)
|
||||
{
|
||||
// check if deviation info outdated.
|
||||
if (nodeEntry->ttlCnt <= NUM_SERVICE_PRD_SLOTS)
|
||||
{
|
||||
// Member node should adjust in the opposite way.
|
||||
timeDevi = -nodeEntry->timeDevi;
|
||||
}
|
||||
else
|
||||
{
|
||||
DBG_WARNING("Time deviation is outdated, omitted it.\n");
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
DBG_ERROR("Fail to get time deviation...\n");
|
||||
timeDevi = 0;
|
||||
}
|
||||
if (timeDevi > L2_MSG_SVC_TMSYNC_MAX || timeDevi < L2_MSG_SVC_TMSYNC_MIN)
|
||||
{
|
||||
do
|
||||
{
|
||||
timeDevi /= 2;
|
||||
} while (timeDevi > L2_MSG_SVC_TMSYNC_MAX || timeDevi < L2_MSG_SVC_TMSYNC_MIN);
|
||||
DBG_WARNING("Deviation is out of adjustable range, truncate it to %d.\n", timeDevi);
|
||||
}
|
||||
memcpy(buff + L2_MSG_SVC_TMSYNC, &timeDevi, L2_MSG_SVC_TMSYNC_LEN);
|
||||
|
||||
// Pack Seq ID
|
||||
buff[L2_MSG_SVC_SEQ_ID] = instPtr->serviceFsm.txSeqNum;
|
||||
|
||||
// Pack payload
|
||||
memcpy(buff + L2_MSG_SVC_PAYLOAD,
|
||||
instPtr->serviceFsm.txLog + L2_MSG_SVC_PAYLOAD_LEN * instPtr->serviceFsm.txSeqNum,
|
||||
L2_MSG_SVC_PAYLOAD_LEN);
|
||||
if (instPtr->config.myType == L2_HEAD_NODE)
|
||||
{
|
||||
// if last packet reset counting
|
||||
instPtr->serviceFsm.txSeqNum = (instPtr->serviceFsm.txSeqNum >= (PAYLOAD_FLJ - 1) / L2_MSG_SVC_PAYLOAD + 1)
|
||||
? 0
|
||||
: instPtr->serviceFsm.txSeqNum + 1;
|
||||
}
|
||||
else
|
||||
{
|
||||
// if last packet reset counting
|
||||
instPtr->serviceFsm.txSeqNum = (instPtr->serviceFsm.txSeqNum >= (PAYLOAD_LJ - 1) / L2_MSG_SVC_PAYLOAD + 1)
|
||||
? 0
|
||||
: instPtr->serviceFsm.txSeqNum + 1;
|
||||
}
|
||||
|
||||
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(instPtr->txQueue, buff) == false)
|
||||
{
|
||||
DBG_ERROR("Cannot push packet to queue..\n");
|
||||
return EQUEUE;
|
||||
}
|
||||
break;
|
||||
}
|
||||
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...\n");
|
||||
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;
|
||||
#else
|
||||
DBG_ERROR("Unexpected call of Service_Packing in BER test mode.\n");
|
||||
return EUNEXP;
|
||||
#endif
|
||||
}
|
||||
|
||||
/*****************************************************************************/
|
||||
/**
|
||||
* @brief Initialize scan FSM.
|
||||
*
|
||||
* @param instPtr
|
||||
*
|
||||
* @return
|
||||
*
|
||||
******************************************************************************/
|
||||
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
|
||||
instPtr->serviceFsm.bitTotal = 0;
|
||||
instPtr->serviceFsm.bitError = 0;
|
||||
#endif
|
||||
return SUCCESS;
|
||||
}
|
||||
|
||||
/*****************************************************************************/
|
||||
/**
|
||||
* @brief Handle service received packet. Send data up to MPC
|
||||
*
|
||||
* @param msg A point32_ter the the message frame.
|
||||
*
|
||||
* @return SUCCESS message has been successfully handled.
|
||||
* other error occurred.
|
||||
*
|
||||
******************************************************************************/
|
||||
int32_t L2_Service_PktHandler(L2_INST *instPtr, uint8_t *msg)
|
||||
{
|
||||
ASSERT(instPtr != NULL);
|
||||
ASSERT(msg != NULL);
|
||||
int32_t Status;
|
||||
switch (instPtr->frame.last)
|
||||
{
|
||||
case L2_SVC_FRAME:
|
||||
{
|
||||
if (instPtr->config.myType == L2_HEAD_NODE)
|
||||
{
|
||||
DBG_ERROR("Unexpected call, Only None-Head Node can handle L2_SVC_FRAME.\n");
|
||||
Status = EUNEXP;
|
||||
break;
|
||||
}
|
||||
|
||||
// Implement a auto time sync.
|
||||
uint32_t seqNum;
|
||||
L2_EVENT l2Event;
|
||||
l2Event.event = L2_EV_TIME_SYNC;
|
||||
seqNum = msg[L2_MSG_SVC_SEQ_ID];
|
||||
|
||||
#ifdef DATALINK_ENABLE_AUTOTIMESYNC
|
||||
l2Event.msg = NULL;
|
||||
AssertL2Event(instPtr, &l2Event);
|
||||
#else
|
||||
int64_t timeAdj;
|
||||
l2Event.msg = &timeAdj;
|
||||
// Get Time Deviation
|
||||
timeAdj = 0;
|
||||
memcpy(&timeAdj, msg + L2_MSG_SVC_TMSYNC, L2_MSG_SVC_TMSYNC_LEN);
|
||||
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 || instPtr->serviceFsm.seqNum != seqNum)
|
||||
{
|
||||
AssertL2Event(instPtr, &l2Event);
|
||||
}
|
||||
#endif
|
||||
#ifndef DATALINK_ENABLE_BERTEST
|
||||
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 == instPtr->serviceFsm.rxSeqNum)
|
||||
{
|
||||
SysMonitor_UpdateRecvPayloadLJRaw(instPtr->serviceFsm.rxLog);
|
||||
}
|
||||
instPtr->serviceFsm.rxSeqNum = 0;
|
||||
}
|
||||
else
|
||||
{
|
||||
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 == instPtr->serviceFsm.rxSeqNum)
|
||||
{
|
||||
SysMonitor_UpdateRecvPayloadFLJRaw(instPtr->serviceFsm.rxLog);
|
||||
}
|
||||
instPtr->serviceFsm.rxSeqNum = 0;
|
||||
}
|
||||
else
|
||||
{
|
||||
if (instPtr->serviceFsm.rxSeqNum == seqNum)
|
||||
{
|
||||
instPtr->serviceFsm.rxSeqNum++;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
#else
|
||||
for (int i = 0; i < SVC_PACK_LEN; i++)
|
||||
{
|
||||
instPtr->serviceFsm.bitError += count_ones(msg[i] ^ berTestData[i]);
|
||||
}
|
||||
instPtr->serviceFsm.bitTotal += SVC_PACK_LEN * 8;
|
||||
#endif
|
||||
instPtr->serviceFsm.pktRecved++;
|
||||
// Reset Node TTL
|
||||
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 (instPtr->config.myType != L2_HEAD_NODE)
|
||||
{
|
||||
DBG_ERROR("Unexpected call, Only Head Node can handle L2_SVC_FRAME.\n");
|
||||
Status = EUNEXP;
|
||||
break;
|
||||
}
|
||||
#ifndef DATALINK_ENABLE_BERTEST
|
||||
COORD coordTemp;
|
||||
float tmpVal;
|
||||
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);
|
||||
}
|
||||
|
||||
// TODO: Check
|
||||
// Update Time deviation
|
||||
int64_t devi, deviTrunc;
|
||||
Status = L2_TimeSync_GetDeviatoion(instPtr, &devi);
|
||||
if (Status != SUCCESS)
|
||||
{
|
||||
DBG_ERROR("Failed to compute slot deviation.\n");
|
||||
deviTrunc = 0;
|
||||
}
|
||||
else
|
||||
{
|
||||
if (devi > INT32_MAX || devi < INT32_MIN)
|
||||
{
|
||||
DBG_WARNING("Illegal deviation value: %lld, omit it.\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);
|
||||
}
|
||||
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++)
|
||||
{
|
||||
instPtr->serviceFsm.bitError += count_ones(msg[i] ^ berTestData[i]);
|
||||
}
|
||||
instPtr->serviceFsm.bitTotal += SVC_PACK_LEN * 8;
|
||||
#endif
|
||||
instPtr->serviceFsm.pktRecved++;
|
||||
// Reset Node TTL
|
||||
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", instPtr->frame.last);
|
||||
break;
|
||||
}
|
||||
return Status;
|
||||
}
|
||||
|
||||
/*****************************************************************************/
|
||||
/**
|
||||
* @brief Service FSM Routine.
|
||||
*
|
||||
* @param instPtr
|
||||
* @param schedEntry
|
||||
*
|
||||
* @return Slot has been used.
|
||||
*
|
||||
******************************************************************************/
|
||||
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 == instPtr->config.myId)
|
||||
{
|
||||
torState = L2_TOR_TX;
|
||||
}
|
||||
else if (schedEntry->dst == instPtr->config.myId)
|
||||
{
|
||||
torState = L2_TOR_RX;
|
||||
instPtr->serviceFsm.pktRecv++;
|
||||
}
|
||||
else
|
||||
{
|
||||
torState = L2_TOR_SUSPEND;
|
||||
}
|
||||
|
||||
switch (schedEntry->slotType)
|
||||
{
|
||||
case L2_SLOT_SERVICE:
|
||||
if (torState == L2_TOR_TX)
|
||||
{
|
||||
#ifndef DATALINK_ENABLE_BERTEST
|
||||
if (instPtr->serviceFsm.txSeqNum == 0)
|
||||
{
|
||||
if (instPtr->config.myType == L2_HEAD_NODE)
|
||||
{
|
||||
SysMonitor_GetSendPayloadFLJRaw(instPtr->serviceFsm.txLog);
|
||||
}
|
||||
else
|
||||
{
|
||||
SysMonitor_GetSendPayloadLJRaw(instPtr->serviceFsm.txLog);
|
||||
}
|
||||
}
|
||||
// Packing service data
|
||||
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(instPtr->config.phyInstRef->sendFrameBuff, berTestData, SVC_PACK_LEN);
|
||||
#endif
|
||||
}
|
||||
// FIXME: Steer antenna to point to destination.
|
||||
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(instPtr, L2_INFO_FRAME);
|
||||
if (CircQueuePop(instPtr->txQueue, (void **)(&sendData)) == true)
|
||||
{
|
||||
memcpy(instPtr->sendPktData, sendData, RSP_PACK_LEN);
|
||||
free(sendData);
|
||||
}
|
||||
else
|
||||
{
|
||||
DBG_ERROR("Failed to pop tx queue.\n");
|
||||
}
|
||||
}
|
||||
// FIXME: Steer antenna to point to destination.
|
||||
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
|
||||
default:
|
||||
DBG_ERROR("UNKNOWN slot type [%d].\n", schedEntry->slotType);
|
||||
return NUM_SLOTS_NONE;
|
||||
}
|
||||
#ifdef DATALINK_ENABLE_BERTEST
|
||||
uint32_t pktLoss;
|
||||
double errorRate, lossRate;
|
||||
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;
|
||||
}
|
||||
@@ -0,0 +1,126 @@
|
||||
/*****************************************************************************/
|
||||
/**
|
||||
* Description: Time sync implementation file.
|
||||
*
|
||||
* Date: 2021-12-04 16:32
|
||||
* LastEditors: FanHao
|
||||
* LastEditTime: 2022-07-10 20:43
|
||||
* Revisions: Rev 0.0
|
||||
* History:
|
||||
* [2022-07-10]: New branch for Dijian.
|
||||
* [2021-12-04]: 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 "link_layer.h"
|
||||
#include "../phy_layer/phy_reg.h"
|
||||
/************************** 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 SYS_CLK_FREQ 192000000.0 // Hz
|
||||
#define SIG_CLK_FREQ 245760000.0 // Hz
|
||||
#define CXR_FREQ 3800000000.0 // Hz
|
||||
|
||||
#define NORMAL_SLOT_CLK_NUM 960000.0 // 9600000= 5e-3*192e6
|
||||
#define COUNTER_10NS_MAX 500000 // how many 10ns in a 5ms
|
||||
|
||||
#define LIGHT_SPPED_AIR 299702547.0 // m/s
|
||||
#define LIGHT_SPPED_VAC 299792458.0 // m/s
|
||||
|
||||
// #define PHY_SYNCT2FEND_200K 904052UL
|
||||
// #define PHY_SYNCT2FEND_200K 924232UL
|
||||
|
||||
#define PHY_SYNCT2FEND_2K_20MS 276336UL // 1.919ms
|
||||
#define PHY_SYNCT2FEND_2K_100MS 11796336UL // 81.919ms
|
||||
|
||||
#define PHY_LOCAL_CNT_MAX 0x0000FFFFFFFFFFFFULL
|
||||
/***************** Macros (Inline Functions) Definitions *********************/
|
||||
/**************************** Type Definitions *******************************/
|
||||
/************************** Variable Definitions *****************************/
|
||||
/************************** Function Prototypes ******************************/
|
||||
|
||||
/*****************************************************************************/
|
||||
/**
|
||||
* @brief Apply local time adjustment by addition.
|
||||
*
|
||||
* @param instPtr.
|
||||
* @param adjustment
|
||||
*
|
||||
* @return
|
||||
*
|
||||
******************************************************************************/
|
||||
inline int32_t L2_TimeSync_LocalTimerAdd(L2_INST *instPtr, int64_t adjustment)
|
||||
{
|
||||
// read nextSendTime from physical layer
|
||||
Phy_SetTimeWord(instPtr->config.phyInstRef, adjustment, PHY_TIME_WORD_LOCAL_ADJ);
|
||||
PhyWrite(instPtr->config.phyInstRef, TM_ADJ_MODE, TM_ADJ_MODE_ADD);
|
||||
return SUCCESS;
|
||||
}
|
||||
|
||||
/*****************************************************************************/
|
||||
/**
|
||||
* @brief Align slot, base on physical layer information.
|
||||
*
|
||||
* @param instPtr.
|
||||
*
|
||||
* @return
|
||||
*
|
||||
******************************************************************************/
|
||||
int32_t L2_TimeSync_SlotAlign(L2_INST *instPtr)
|
||||
{
|
||||
int32_t Status;
|
||||
int64_t slotAdj;
|
||||
Status = L2_TimeSync_GetDeviatoion(instPtr, &slotAdj);
|
||||
if (Status != SUCCESS)
|
||||
{
|
||||
DBG_ERROR("Failed to compute slot deviation.\n");
|
||||
return Status;
|
||||
}
|
||||
return L2_TimeSync_LocalTimerAdd(instPtr, slotAdj);
|
||||
}
|
||||
|
||||
/*****************************************************************************/
|
||||
/**
|
||||
* @brief Compute slot deviation of received frame.
|
||||
*
|
||||
* @param instPtr.
|
||||
* @param devi
|
||||
*
|
||||
* @return
|
||||
*
|
||||
******************************************************************************/
|
||||
int32_t L2_TimeSync_GetDeviatoion(L2_INST *instPtr, int64_t *devi)
|
||||
{
|
||||
// read nextSendTime from physical layer
|
||||
int64_t currentTarget, lastFrameSync, slotMeasure;
|
||||
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(instPtr->config.phyInstRef))
|
||||
{
|
||||
case L1_2KBPS_20MS_FRAME:
|
||||
*devi = ((int64_t)PHY_SYNCT2FEND_2K_20MS) - slotMeasure;
|
||||
break;
|
||||
case L1_2KBPS_100MS_FRAME:
|
||||
*devi = ((int64_t)PHY_SYNCT2FEND_2K_100MS) - slotMeasure;
|
||||
break;
|
||||
default:
|
||||
DBG_ERROR("Unsupported frame type: %d.\n", Phy_GetLastDetectedFrameType(instPtr->config.phyInstRef));
|
||||
return EIGNORE;
|
||||
}
|
||||
return SUCCESS;
|
||||
}
|
||||
@@ -0,0 +1,348 @@
|
||||
/*****************************************************************************/
|
||||
/**
|
||||
* Description: Antenna configuration functions.
|
||||
*
|
||||
* Author: Fan Hao
|
||||
* Date: 2022-06-18 16:37
|
||||
* LastEditors: Fan Hao
|
||||
* LastEditTime: 2022-06-18 17:15
|
||||
* Revisions: Rev 0.0
|
||||
* History:
|
||||
* [2022-06-18]: Initial version.
|
||||
*
|
||||
* Copyright(c) 2022 Beijing Institute of Technology.
|
||||
* Lab of Communication and Networking
|
||||
*
|
||||
******************************************************************************/
|
||||
#include <string.h>
|
||||
#include <stdint.h>
|
||||
#include <math.h>
|
||||
#include "error.h"
|
||||
#include "../config//app_config.h"
|
||||
#include "phy_layer.h"
|
||||
|
||||
#define ANT_PI 3.14159265358979323846
|
||||
|
||||
#define ANT_COINFO_HEAD 0xAA
|
||||
#define ANT_COINFO_TAIL 0xCC
|
||||
|
||||
#define CTRL_WORD_DEFAULT 0x00
|
||||
|
||||
#define ANT_MODE_BEAM_MASK 0xF0
|
||||
#define ANT_MODE_NARROR_BEAM 0x00 // 8
|
||||
#define ANT_MODE_WIDE_BEAM_1 0x10 // 25
|
||||
#define ANT_MODE_WIDE_BEAM_2 0x20 // 60
|
||||
#define ANT_MODE_WIDE_BEAM_3 0x30 // 120
|
||||
|
||||
#define ANT_MODE_HIGH_POWER 0x00
|
||||
#define ANT_MODE_LOWAPOWER 0x04
|
||||
#define ANT_MODE_STANDBY 0x0F
|
||||
#define ATN_MODE_DEFAULT (ANT_MODE_WIDE_BEAM_1 | ANT_MODE_LOWAPOWER)
|
||||
#define ATN_MODE_HIGH (ANT_MODE_WIDE_BEAM_1 | ANT_MODE_HIGH_POWER)
|
||||
|
||||
#define DEFAULT_CENTRTAL_FREQ 0x06D7 // 26.25GHz
|
||||
|
||||
#define RXTXMODE_RX 0x00
|
||||
#define RXTXMODE_TX 0x01
|
||||
|
||||
#define RXTXMODE_DEFAULT 0x00
|
||||
|
||||
#define CALIBRMODE_NORMAL 0x00
|
||||
#define CALIBRMODE_SINGLE_TR_CAL 0x01
|
||||
|
||||
#define RF_CTRL_DEFAULT 0x00
|
||||
|
||||
// Pair by regex
|
||||
// Regex: (([0-9]+, ){8})
|
||||
uint16_t hopPattern_Config[L1_FRAME_MAX_HOPS] =
|
||||
{
|
||||
0
|
||||
};
|
||||
|
||||
uint16_t hopPattern_2KBPS_20MS[L1_FRAME_2KBPS_20MS_HOPS] =
|
||||
{
|
||||
1500
|
||||
};
|
||||
|
||||
// uint16_t hopPattern_2KBPS_100MS[L1_FRAME_2KBPS_100MS_HOPS] =
|
||||
// {
|
||||
// 0, 44, 88, 131, 175, 219, 263, 306,
|
||||
// 350, 394, 438, 481, 525, 569, 613, 656,
|
||||
// 700, 744, 788, 831, 875, 919, 963, 1006,
|
||||
// 1050, 1094, 1138, 1181, 1225, 1269, 1313, 1356,
|
||||
// 1400, 1444, 1488, 1531, 1575, 1619, 1663, 1706,
|
||||
// 1750, 1794, 1838, 1881, 1925, 1969, 2013, 2056,
|
||||
// 2100, 2144, 2188, 2231, 2275, 2319, 2363, 2406,
|
||||
// 2450, 2494, 2538, 2581, 2625, 2669, 2713, 2756,
|
||||
// 2800, 2844, 2888, 2931, 2975, 3019, 3063, 3106,
|
||||
// 3150, 3194, 3238, 3281, 3325, 3369, 3413, 3456,
|
||||
// 3500
|
||||
// };
|
||||
|
||||
uint16_t hopPattern_2KBPS_100MS[L1_FRAME_2KBPS_100MS_HOPS] =
|
||||
{
|
||||
1500
|
||||
};
|
||||
|
||||
uint16_t hopPattern_10KBPS_10MS[L1_FRAME_10KBPS_10MS_HOPS] =
|
||||
{
|
||||
1500
|
||||
};
|
||||
|
||||
uint16_t hopPattern_10KBPS_20MS[L1_FRAME_10KBPS_20MS_HOPS] =
|
||||
{
|
||||
1500, 1600, 1700, 1800, 1900, 2000, 1500, 1600,
|
||||
1700, 1800, 1900
|
||||
};
|
||||
|
||||
uint16_t hopPattern_40KBPS_20MS[L1_FRAME_40KBPS_20MS_HOPS] =
|
||||
{
|
||||
2400, 2400, 2400, 2400, 2400, 2400, 2400, 2400,
|
||||
2400, 2400, 2400, 2400, 2400, 2400, 2400, 2400,
|
||||
2400, 2400, 2400
|
||||
};
|
||||
|
||||
uint16_t hopPattern_40KBPS_10MS[L1_FRAME_40KBPS_10MS_HOPS] =
|
||||
{
|
||||
2400, 2400, 2400, 2400, 2400, 2400, 2400, 2400,
|
||||
2400
|
||||
};
|
||||
uint16_t hopPattern_200KBPS_5MS[L1_FRAME_200KBPS_5MS_HOPS] =
|
||||
{
|
||||
500, 1600, 1700, 1800, 1900, 2000, 1500, 1600,
|
||||
1700, 1800, 1900, 2000, 1500, 1600, 1700, 1800,
|
||||
1900, 2000, 1500, 1500
|
||||
};
|
||||
uint16_t hopPattern_1M2BPS_5MS[L1_FRAME_1M2BPS_5MS_HOPS] =
|
||||
{
|
||||
500, 600, 700, 800, 900, 1000, 500, 600,
|
||||
700, 800, 900, 1000, 500, 600, 700, 800,
|
||||
900, 1000, 500, 500
|
||||
};
|
||||
|
||||
// Pair by regex
|
||||
// Regex: ((\{\.oaxis = [0-9]+[ ,]+\.raxis = [0-9]+[ ]*\}, ){2})
|
||||
#ifdef PHY_BEAM_SET_PRESET
|
||||
PHY_ANT_ANGLE antSubnrrwPattern[L1_NUM_ANT_SUBNRRW_PATTERN] =
|
||||
{
|
||||
{.oaxis = 0 , .raxis = 222}, {.oaxis = 10, .raxis = 85 },
|
||||
{.oaxis = 14, .raxis = 307}, {.oaxis = 17, .raxis = 170},
|
||||
{.oaxis = 19, .raxis = 32 }, {.oaxis = 21, .raxis = 255},
|
||||
{.oaxis = 24, .raxis = 117}, {.oaxis = 25, .raxis = 340},
|
||||
{.oaxis = 27, .raxis = 202}, {.oaxis = 29, .raxis = 65 },
|
||||
{.oaxis = 31, .raxis = 287}, {.oaxis = 32, .raxis = 150},
|
||||
{.oaxis = 34, .raxis = 12 }, {.oaxis = 35, .raxis = 235},
|
||||
{.oaxis = 36, .raxis = 97 }, {.oaxis = 38, .raxis = 320},
|
||||
{.oaxis = 39, .raxis = 182}, {.oaxis = 40, .raxis = 45 },
|
||||
{.oaxis = 41, .raxis = 267}, {.oaxis = 43, .raxis = 130},
|
||||
{.oaxis = 44, .raxis = 352}, {.oaxis = 45, .raxis = 215},
|
||||
{.oaxis = 46, .raxis = 77 }, {.oaxis = 47, .raxis = 300},
|
||||
{.oaxis = 48, .raxis = 162}, {.oaxis = 49, .raxis = 25 },
|
||||
{.oaxis = 50, .raxis = 247}, {.oaxis = 51, .raxis = 110},
|
||||
{.oaxis = 52, .raxis = 332}, {.oaxis = 53, .raxis = 195},
|
||||
{.oaxis = 54, .raxis = 57 }, {.oaxis = 55, .raxis = 280},
|
||||
{.oaxis = 56, .raxis = 142}, {.oaxis = 57, .raxis = 5 },
|
||||
{.oaxis = 58, .raxis = 227}, {.oaxis = 59, .raxis = 90 },
|
||||
{.oaxis = 60, .raxis = 312}
|
||||
};
|
||||
PHY_ANT_ANGLE antNrrwPattern[L1_NUM_ANT_NRRW_PATTERN] =
|
||||
{
|
||||
{.oaxis = 0 , .raxis = 222}, {.oaxis = 7 , .raxis = 85 },
|
||||
{.oaxis = 10, .raxis = 307}, {.oaxis = 13, .raxis = 170},
|
||||
{.oaxis = 15, .raxis = 32 }, {.oaxis = 17, .raxis = 255},
|
||||
{.oaxis = 18, .raxis = 117}, {.oaxis = 20, .raxis = 340},
|
||||
{.oaxis = 21, .raxis = 202}, {.oaxis = 22, .raxis = 65 },
|
||||
{.oaxis = 24, .raxis = 287}, {.oaxis = 25, .raxis = 150},
|
||||
{.oaxis = 26, .raxis = 12 }, {.oaxis = 27, .raxis = 235},
|
||||
{.oaxis = 28, .raxis = 97 }, {.oaxis = 29, .raxis = 320},
|
||||
{.oaxis = 30, .raxis = 182}, {.oaxis = 31, .raxis = 45 },
|
||||
{.oaxis = 32, .raxis = 267}, {.oaxis = 33, .raxis = 130},
|
||||
{.oaxis = 34, .raxis = 352}, {.oaxis = 34, .raxis = 215},
|
||||
{.oaxis = 35, .raxis = 77 }, {.oaxis = 36, .raxis = 300},
|
||||
{.oaxis = 37, .raxis = 162}, {.oaxis = 38, .raxis = 25 },
|
||||
{.oaxis = 38, .raxis = 247}, {.oaxis = 39, .raxis = 110},
|
||||
{.oaxis = 40, .raxis = 332}, {.oaxis = 41, .raxis = 195},
|
||||
{.oaxis = 41, .raxis = 57 }, {.oaxis = 42, .raxis = 280},
|
||||
{.oaxis = 43, .raxis = 142}, {.oaxis = 44, .raxis = 5 },
|
||||
{.oaxis = 44, .raxis = 227}, {.oaxis = 45, .raxis = 90 },
|
||||
{.oaxis = 46, .raxis = 312}, {.oaxis = 46, .raxis = 175},
|
||||
{.oaxis = 47, .raxis = 37 }, {.oaxis = 48, .raxis = 260},
|
||||
{.oaxis = 48, .raxis = 122}, {.oaxis = 49, .raxis = 345},
|
||||
{.oaxis = 49, .raxis = 207}, {.oaxis = 50, .raxis = 70 },
|
||||
{.oaxis = 51, .raxis = 292}, {.oaxis = 51, .raxis = 155},
|
||||
{.oaxis = 52, .raxis = 17 }, {.oaxis = 53, .raxis = 240},
|
||||
{.oaxis = 53, .raxis = 102}, {.oaxis = 54, .raxis = 325},
|
||||
{.oaxis = 54, .raxis = 187}, {.oaxis = 55, .raxis = 50 },
|
||||
{.oaxis = 55, .raxis = 272}, {.oaxis = 56, .raxis = 135},
|
||||
{.oaxis = 57, .raxis = 357}, {.oaxis = 57, .raxis = 220},
|
||||
{.oaxis = 58, .raxis = 82 }, {.oaxis = 58, .raxis = 305},
|
||||
{.oaxis = 59, .raxis = 167}, {.oaxis = 59, .raxis = 30 },
|
||||
{.oaxis = 60, .raxis = 252}
|
||||
};
|
||||
#else
|
||||
PHY_ANT_ANGLE antSubnrrwPattern[NUM_ANT_SUBNRRW_PATTERN] =
|
||||
{
|
||||
0
|
||||
};
|
||||
PHY_ANT_ANGLE antNrrwPattern[NUM_ANT_NRRW_PATTERN] =
|
||||
{
|
||||
0
|
||||
};
|
||||
#endif
|
||||
/*****************************************************************************/
|
||||
/**
|
||||
* @brief
|
||||
*
|
||||
* @param antConfigPtr
|
||||
*
|
||||
* @return
|
||||
*
|
||||
******************************************************************************/
|
||||
int32_t Phy_AntConfigInit(PHY_ANT_COINFO *antConfigPtr)
|
||||
{
|
||||
ASSERT(antConfigPtr != NULL);
|
||||
|
||||
antConfigPtr->ctrlWord = 0x00;
|
||||
antConfigPtr->antMode = ATN_MODE_DEFAULT;
|
||||
|
||||
// FIXME: Dummy default value.
|
||||
// off axis angle setting
|
||||
antConfigPtr->oaxis[0] = 0;
|
||||
antConfigPtr->oaxis[1] = 0;
|
||||
// round axis angle setting
|
||||
antConfigPtr->raxis[0] = 0;
|
||||
antConfigPtr->raxis[1] = 0;
|
||||
// ***********************
|
||||
|
||||
antConfigPtr->antFreq[0] = (uint8_t)(DEFAULT_CENTRTAL_FREQ);
|
||||
antConfigPtr->antFreq[1] = (uint8_t)(DEFAULT_CENTRTAL_FREQ >> 8);
|
||||
|
||||
// FIXME: Dummy default value.
|
||||
// trSelect
|
||||
antConfigPtr->trSlect = 0;
|
||||
// Antenna attenuation setting
|
||||
antConfigPtr->antAtt = 20;
|
||||
// phase setting
|
||||
antConfigPtr->phase = 0;
|
||||
// ***********************
|
||||
|
||||
antConfigPtr->rxtxMode = RXTXMODE_DEFAULT;
|
||||
antConfigPtr->calibrMode = CALIBRMODE_NORMAL;
|
||||
antConfigPtr->antReserved = 0x00;
|
||||
|
||||
// FIXME: Dummy default value.
|
||||
// RF mode control
|
||||
antConfigPtr->rfCtrl = RF_CTRL_DEFAULT;
|
||||
// RF frequency
|
||||
antConfigPtr->rfFreq[0] = (uint8_t)(DEFAULT_CENTRTAL_FREQ);
|
||||
antConfigPtr->rfFreq[1] = (uint8_t)(DEFAULT_CENTRTAL_FREQ >> 8);
|
||||
// RF MGC control
|
||||
antConfigPtr->rfTxMgc = 0;
|
||||
// RF channel 1&2 calibration mgc
|
||||
antConfigPtr->rf12CalMgc = 0;
|
||||
// RF channel 2&3 calibration mgc
|
||||
antConfigPtr->rf34CalMgc = 0;
|
||||
// Receive channel 1-4 MGC
|
||||
antConfigPtr->rcvMgc = 0;
|
||||
antConfigPtr->rfReserved0 = 0x00;
|
||||
antConfigPtr->rfReserved1 = 0x00;
|
||||
// ***********************
|
||||
return SUCCESS;
|
||||
}
|
||||
|
||||
int32_t Phy_SetBeamNo(PHY_INST *instPtr, int32_t beamNo)
|
||||
{
|
||||
if (instPtr->beamWd.next == PHY_ANT_BEAM_8)
|
||||
{
|
||||
if (beamNo >= L1_NUM_ANT_NRRW_PATTERN)
|
||||
{
|
||||
return FAILURE;
|
||||
}
|
||||
else
|
||||
{
|
||||
instPtr->beamNo.next = beamNo;
|
||||
instPtr->antConfigPtr.oaxis[1] = (uint8_t)(antNrrwPattern[beamNo].oaxis);
|
||||
instPtr->antConfigPtr.oaxis[0] = (uint8_t)((antNrrwPattern[beamNo].oaxis) >> 8);
|
||||
instPtr->antConfigPtr.raxis[1] = (uint8_t)(antNrrwPattern[beamNo].raxis);
|
||||
instPtr->antConfigPtr.raxis[0] = (uint8_t)((antNrrwPattern[beamNo].raxis) >> 8);
|
||||
return SUCCESS;
|
||||
}
|
||||
}
|
||||
else if (instPtr->beamWd.next == PHY_ANT_BEAM_25)
|
||||
{
|
||||
if (beamNo >= L1_NUM_ANT_SUBNRRW_PATTERN)
|
||||
{
|
||||
return FAILURE;
|
||||
}
|
||||
else
|
||||
{
|
||||
instPtr->beamNo.next = beamNo;
|
||||
instPtr->antConfigPtr.oaxis[1] = (uint8_t)(antSubnrrwPattern[beamNo].oaxis);
|
||||
instPtr->antConfigPtr.oaxis[0] = (uint8_t)((antSubnrrwPattern[beamNo].oaxis) >> 8);
|
||||
instPtr->antConfigPtr.raxis[1] = (uint8_t)(antSubnrrwPattern[beamNo].raxis);
|
||||
instPtr->antConfigPtr.raxis[0] = (uint8_t)((antSubnrrwPattern[beamNo].raxis) >> 8);
|
||||
return SUCCESS;
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
// Beam number assign not support.
|
||||
// Assign default angle (0,0)
|
||||
instPtr->beamNo.next = 0;
|
||||
instPtr->antConfigPtr.oaxis[1] = 0;
|
||||
instPtr->antConfigPtr.oaxis[0] = 0;
|
||||
instPtr->antConfigPtr.raxis[1] = 0;
|
||||
instPtr->antConfigPtr.raxis[0] = 0;
|
||||
DBG_WARNING("Using default beam number...\n");
|
||||
return SUCCESS;
|
||||
}
|
||||
}
|
||||
|
||||
int32_t Phy_SetBeamWidth(PHY_INST *instPtr, PHY_ANT_BEAM_WIDTH beamWidth)
|
||||
{
|
||||
uint8_t beamConfig = instPtr->antConfigPtr.antMode & (~ANT_MODE_BEAM_MASK);
|
||||
switch (beamWidth)
|
||||
{
|
||||
case PHY_ANT_BEAM_120:
|
||||
beamConfig |= ANT_MODE_WIDE_BEAM_3;
|
||||
break;
|
||||
case PHY_ANT_BEAM_60:
|
||||
beamConfig |= ANT_MODE_WIDE_BEAM_2;
|
||||
break;
|
||||
case PHY_ANT_BEAM_25:
|
||||
beamConfig |= ANT_MODE_WIDE_BEAM_1;
|
||||
break;
|
||||
case PHY_ANT_BEAM_8:
|
||||
beamConfig |= ANT_MODE_NARROR_BEAM;
|
||||
break;
|
||||
default:
|
||||
DBG_ERROR("Beam width code: 0x%08X is illegal.\n\r", beamWidth);
|
||||
return EPARAM;
|
||||
break;
|
||||
}
|
||||
instPtr->antConfigPtr.antMode = beamConfig;
|
||||
instPtr->beamWd.next = beamWidth;
|
||||
return SUCCESS;
|
||||
}
|
||||
|
||||
// Common version radius double input
|
||||
int32_t Phy_SetBeamAngle(PHY_INST *phyInstPtr, double offAx, double rndAx)
|
||||
{
|
||||
uint16_t offAxDeg = 0;
|
||||
uint16_t rndAxDeg = 0;
|
||||
offAxDeg = (offAx >= 0) ? (((int32_t)(offAx * 180 / ANT_PI)) % 360) : (((int32_t)(offAx * 180 / ANT_PI)) % 360 + 360);
|
||||
rndAxDeg = (rndAx >= 0) ? (((int32_t)(rndAx * 180 / ANT_PI)) % 360) : (((int32_t)(rndAx * 180 / ANT_PI)) % 360 + 360);
|
||||
if (offAxDeg > 60)
|
||||
{
|
||||
DBG_ERROR("Angle out of range, off axis angle: %u (should be 0~60), round axis angle: %u (should be 0~360).\n",
|
||||
offAxDeg, rndAxDeg);
|
||||
return EPARAM;
|
||||
}
|
||||
offAxDeg = offAxDeg * 10;
|
||||
rndAxDeg = rndAxDeg * 10;
|
||||
phyInstPtr->antConfigPtr.oaxis[1] = (uint8_t)offAxDeg;
|
||||
phyInstPtr->antConfigPtr.oaxis[0] = (uint8_t)(offAxDeg >> 8);
|
||||
phyInstPtr->antConfigPtr.raxis[1] = (uint8_t)rndAxDeg;
|
||||
phyInstPtr->antConfigPtr.raxis[0] = (uint8_t)(rndAxDeg >> 8);
|
||||
return SUCCESS;
|
||||
}
|
||||
@@ -0,0 +1,921 @@
|
||||
/*****************************************************************************/
|
||||
/**
|
||||
* Description: Implementation of physical control functions
|
||||
*
|
||||
* Date: 2021-11-16 00:08
|
||||
* LastEditTime: 2023-04-23 17:48
|
||||
* Revisions: Rev 0.2
|
||||
* History:
|
||||
* [2021-11-16]: Initial version.
|
||||
* [2022-06-18]: New branch for glink control purpose:
|
||||
* -Add poll mode, physical layer will start Tx/Rx procedure
|
||||
* at specific time stamp.
|
||||
* -Add 61/37 beam switch
|
||||
* -Code simplification.
|
||||
* [2023-04-24]: Porting for DUIFEI.
|
||||
*
|
||||
* Copyright(c) 2021 Beijing Institute of Technology.
|
||||
* Lab of Communication and Networking
|
||||
*
|
||||
******************************************************************************/
|
||||
|
||||
/***************************** Include Files *********************************/
|
||||
#include <string.h>
|
||||
#include <stdlib.h>
|
||||
#include "xbram.h"
|
||||
#include "xadcps.h"
|
||||
#include "phy_layer.h"
|
||||
#include "util.h"
|
||||
#include "error.h"
|
||||
#include "../config/app_config.h"
|
||||
|
||||
/************************** Constant Definitions *****************************/
|
||||
#define TOR_BUFF_MAX_SIZE 1024 // 8192bits
|
||||
#define TMWORD_FRAC_MAX 500000
|
||||
#define ANTCONFIG_MAX_LEN (8192 / 2)
|
||||
#define PAYLOAD_MAX_LEN (4096 / 2)
|
||||
|
||||
#define INIT_BEAM_NO 0
|
||||
#define INIT_ANT_WD PHY_ANT_BEAM_120
|
||||
#define INIT_ANT_NO 0
|
||||
|
||||
#define L1_FRAME_NONE_LEN UINT32_MAX // byte
|
||||
#define L1_FRAME_2KBPS_20MS_LEN 0 // byte
|
||||
#define L1_FRAME_2KBPS_100MS_LEN (240 / 8) // byte
|
||||
#define L1_FRAME_10KBPS_10MS_LEN 0 // byte
|
||||
#define L1_FRAME_10KBPS_20MS_LEN (240 / 8) // byte
|
||||
#define L1_FRAME_40KBPS_20MS_LEN (2160 / 8) // byte
|
||||
#define L1_FRAME_40KBPS_10MS_LEN (720 / 8) // byte
|
||||
#define L1_FRAME_200KBPS_5MS_LEN (2048 / 8) // byte
|
||||
#define L1_FRAME_1M2BPS_5MS_LEN (8184 / 8) // byte
|
||||
|
||||
#define L1_FRAME_NONE_COMP UINT32_MAX // *10ns
|
||||
#define L1_FRAME_2KBPS_20MS_COMP 1622400 // *10ns
|
||||
#define L1_FRAME_2KBPS_100MS_COMP 1622400 // *10ns
|
||||
#define L1_FRAME_10KBPS_10MS_COMP 747366 // *10ns
|
||||
#define L1_FRAME_10KBPS_20MS_COMP 747366 // *10ns
|
||||
#define L1_FRAME_40KBPS_20MS_COMP 97066 // *10ns
|
||||
#define L1_FRAME_40KBPS_10MS_COMP 97066 // *10ns
|
||||
#define L1_FRAME_200KBPS_5MS_COMP 17067 // *10ns
|
||||
#define L1_FRAME_1M2BPS_5MS_COMP 17067 // *10ns
|
||||
|
||||
/***************** Macros (Inline Functions) Definitions *********************/
|
||||
/************************** Function Prototypes ******************************/
|
||||
static void Phy_RcdShift(PHY_RCD *rcd, int32_t netxVal);
|
||||
static int32_t SetSendData(PHY_INST *instPtr, L1_FRAME_TYPE frameType, uint8_t *buff);
|
||||
|
||||
/**************************** Type Definitions *******************************/
|
||||
|
||||
/************************** Variable Definitions *****************************/
|
||||
|
||||
/*****************************************************************************/
|
||||
/**
|
||||
* @brief Get received data (base on PS Record), store it to data buffer.
|
||||
*
|
||||
* @param instPtr
|
||||
* rcvBuffPtr
|
||||
*
|
||||
* @return SUCCESS
|
||||
* FAILURE last frame type cannot be recognized.
|
||||
*
|
||||
******************************************************************************/
|
||||
int32_t Phy_RetrieveRecvData(PHY_INST *instPtr, uint8_t *buff)
|
||||
{
|
||||
ASSERT(instPtr != NULL);
|
||||
uint32_t frameLen, recvBramAddr;
|
||||
frameLen = Phy_GetFrameLen(instPtr->frameType.last);
|
||||
if (frameLen == L1_FRAME_NONE_LEN)
|
||||
{
|
||||
return FAILURE;
|
||||
}
|
||||
else
|
||||
{
|
||||
// Ping-pong flow.
|
||||
recvBramAddr = instPtr->rxDataPtr + ((PhyRead(instPtr, RECV_DATA_SATE) & RECV_DATA_OFFSET_MASK) >> RECV_DATA_OFFSET_SHIFT);
|
||||
memcpy(buff, (uint8_t *)recvBramAddr, frameLen);
|
||||
}
|
||||
return SUCCESS;
|
||||
}
|
||||
|
||||
/*****************************************************************************/
|
||||
/**
|
||||
* @brief Setup data will be sent in next slot.
|
||||
*
|
||||
* @param instPtr A pointer to physical layer control instance.
|
||||
*
|
||||
* @return FAILURE Error occurred.
|
||||
* SUCCESS Setting data successful.
|
||||
*
|
||||
******************************************************************************/
|
||||
static int32_t SetSendData(PHY_INST *instPtr, L1_FRAME_TYPE frameType, uint8_t *buff)
|
||||
{
|
||||
ASSERT(instPtr != NULL);
|
||||
uint32_t frameLen, sendCmd;
|
||||
frameLen = Phy_GetFrameLen(frameType);
|
||||
|
||||
if (frameLen == L1_FRAME_NONE_LEN)
|
||||
{
|
||||
return FAILURE;
|
||||
}
|
||||
else
|
||||
{
|
||||
// Implement a ping-pong flow, offset always is 0 or half of buffer depth.
|
||||
instPtr->sendOffset = (instPtr->sendOffset == PAYLOAD_MAX_LEN) ? 0 : PAYLOAD_MAX_LEN;
|
||||
memcpy((uint8_t *)(instPtr->txDataPtr + instPtr->sendOffset), buff, frameLen);
|
||||
// Trigger Physical layer to start fetching data.
|
||||
sendCmd = frameLen & SEND_DATA_CMD_LEN_MASK;
|
||||
sendCmd |= (instPtr->sendOffset << SEND_DATA_CMD_OFFSET_SHIFT) & SEND_DATA_CMD_OFFSET_MASK;
|
||||
PhyWrite(instPtr, SEND_DATA_CMD, sendCmd);
|
||||
}
|
||||
return SUCCESS;
|
||||
}
|
||||
|
||||
/*****************************************************************************/
|
||||
/**
|
||||
* @brief Check whether a frame was detected. (Do not check if the frame was
|
||||
* deoced.)
|
||||
*
|
||||
* @param instPtr
|
||||
*
|
||||
* @return true
|
||||
* false
|
||||
*
|
||||
******************************************************************************/
|
||||
inline bool Phy_IsFrameDetected(PHY_INST *instPtr)
|
||||
{
|
||||
ASSERT(instPtr != NULL);
|
||||
if (instPtr->TOR.last == PHY_TOR_RX && instPtr->isAcq && instPtr->isFrameSync)
|
||||
{
|
||||
return true;
|
||||
}
|
||||
else
|
||||
{
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
/*****************************************************************************/
|
||||
/**
|
||||
* @brief Check whether a frame was received. (Do not check if the frame was
|
||||
* crashed.)
|
||||
*
|
||||
* @param instPtr
|
||||
*
|
||||
* @return true
|
||||
* false
|
||||
*
|
||||
******************************************************************************/
|
||||
inline bool Phy_IsFrameReceived(PHY_INST *instPtr)
|
||||
{
|
||||
ASSERT(instPtr != NULL);
|
||||
if (instPtr->TOR.last == PHY_TOR_RX && instPtr->isAcq && instPtr->isFrameSync && instPtr->isDecode)
|
||||
{
|
||||
return true;
|
||||
}
|
||||
else
|
||||
{
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
/*****************************************************************************/
|
||||
/**
|
||||
* @brief Check whether a frame was valid. (if the frame was crashed, it will
|
||||
* be regarded as not valid.
|
||||
*
|
||||
* @param instPtr
|
||||
*
|
||||
* @return true
|
||||
* false
|
||||
*
|
||||
******************************************************************************/
|
||||
inline bool Phy_IsFrameValid(PHY_INST *instPtr)
|
||||
{
|
||||
ASSERT(instPtr != NULL);
|
||||
if (instPtr->TOR.last == PHY_TOR_RX && instPtr->isAcq && instPtr->isFrameSync && instPtr->isDecode && instPtr->isValid)
|
||||
{
|
||||
return true;
|
||||
}
|
||||
else
|
||||
{
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
/*****************************************************************************/
|
||||
/**
|
||||
* @brief Get frame type of the frame in receive data buffer (Base on PS Record).
|
||||
*
|
||||
* @param instPtr A pointer to physical layer control instance.
|
||||
*
|
||||
* @return Layer2 frame type. Refer "frame_def.h"
|
||||
*
|
||||
******************************************************************************/
|
||||
inline L1_FRAME_TYPE Phy_GetLastDetectedFrameType(PHY_INST *instPtr)
|
||||
{
|
||||
ASSERT(instPtr != NULL);
|
||||
if (Phy_IsFrameDetected(instPtr) == false)
|
||||
{
|
||||
return L1_NONE_FRAME;
|
||||
}
|
||||
else
|
||||
{
|
||||
return instPtr->frameType.last;
|
||||
}
|
||||
}
|
||||
|
||||
/*****************************************************************************/
|
||||
/**
|
||||
* @brief Get frame type of the frame in receive data buffer (Base on PS Record).
|
||||
*
|
||||
* @param instPtr A pointer to physical layer control instance.
|
||||
*
|
||||
* @return Layer2 frame type. Refer "frame_def.h"
|
||||
*
|
||||
******************************************************************************/
|
||||
inline L1_FRAME_TYPE Phy_GetLastReceivedFrameType(PHY_INST *instPtr)
|
||||
{
|
||||
ASSERT(instPtr != NULL);
|
||||
if (Phy_IsFrameReceived(instPtr) == false)
|
||||
{
|
||||
return L1_NONE_FRAME;
|
||||
}
|
||||
else
|
||||
{
|
||||
return instPtr->frameType.last;
|
||||
}
|
||||
}
|
||||
|
||||
/*****************************************************************************/
|
||||
/**
|
||||
* @brief Get frame type of the frame was sent in last slot.
|
||||
*
|
||||
* @param instPtr A pointer to physical layer control instance.
|
||||
*
|
||||
* @return Layer2 frame type. Refer "frame_def.h"
|
||||
*
|
||||
******************************************************************************/
|
||||
inline L1_FRAME_TYPE Phy_GetLastSentFrameType(PHY_INST *instPtr)
|
||||
{
|
||||
ASSERT(instPtr != NULL);
|
||||
if (instPtr->TOR.last != PHY_TOR_TX)
|
||||
{
|
||||
return L1_NONE_FRAME;
|
||||
}
|
||||
else
|
||||
{
|
||||
return instPtr->frameType.last;
|
||||
}
|
||||
}
|
||||
|
||||
/*****************************************************************************/
|
||||
/**
|
||||
* @brief Shift l1 status record.(newval->next->curr->last)
|
||||
*
|
||||
* @param instPtr A pointer to physical layer control instance.
|
||||
*
|
||||
* @return NONE.
|
||||
*
|
||||
******************************************************************************/
|
||||
static void Phy_RcdShift(PHY_RCD *rcd, int32_t netxVal)
|
||||
{
|
||||
ASSERT(rcd != NULL);
|
||||
rcd->last = rcd->curr;
|
||||
rcd->curr = rcd->next;
|
||||
rcd->next = netxVal;
|
||||
}
|
||||
|
||||
/*****************************************************************************/
|
||||
/**
|
||||
* @brief Get corresponding frame length in byte of L1 frame.
|
||||
*
|
||||
* @param frameType
|
||||
*
|
||||
* @return Corresponding frame length
|
||||
* L1_FRAME_NONE_LEN if the frame type code cannot be recognized.
|
||||
*
|
||||
******************************************************************************/
|
||||
uint32_t Phy_GetFrameLen(L1_FRAME_TYPE frameType)
|
||||
{
|
||||
switch (frameType)
|
||||
{
|
||||
case L1_2KBPS_20MS_FRAME:
|
||||
// FIXME: for DUIFEI net setup
|
||||
return L1_FRAME_2KBPS_100MS_LEN;
|
||||
// return L1_FRAME_2KBPS_20MS_LEN;
|
||||
case L1_2KBPS_100MS_FRAME:
|
||||
return L1_FRAME_2KBPS_100MS_LEN;
|
||||
case L1_10KBPS_10MS_FRAME:
|
||||
return L1_FRAME_10KBPS_10MS_LEN;
|
||||
case L1_10KBPS_20MS_FRAME:
|
||||
return L1_FRAME_10KBPS_20MS_LEN;
|
||||
case L1_40KBPS_20MS_FRAME:
|
||||
return L1_FRAME_40KBPS_20MS_LEN;
|
||||
case L1_40KBPS_10MS_FRAME:
|
||||
return L1_FRAME_40KBPS_10MS_LEN;
|
||||
case L1_200KBPS_5MS_FRAME:
|
||||
return L1_FRAME_200KBPS_5MS_LEN;
|
||||
case L1_1M2BPS_5MS_FRAME:
|
||||
return L1_FRAME_1M2BPS_5MS_LEN;
|
||||
default:
|
||||
return L1_FRAME_NONE_LEN;
|
||||
}
|
||||
}
|
||||
|
||||
/*****************************************************************************/
|
||||
/**
|
||||
* @brief Update Physical Layer reception status.
|
||||
*
|
||||
* @param instPtr
|
||||
*
|
||||
* @return
|
||||
*
|
||||
******************************************************************************/
|
||||
void Phy_UpdateState(PHY_INST *instPtr)
|
||||
{
|
||||
ASSERT(instPtr != NULL);
|
||||
|
||||
Phy_RcdShift(&(instPtr->TOR), PHY_TOR_SUSPEND);
|
||||
Phy_RcdShift(&(instPtr->frameType), L1_NONE_FRAME);
|
||||
Phy_RcdShift(&(instPtr->antNo), INIT_ANT_NO);
|
||||
Phy_RcdShift(&(instPtr->beamNo), INIT_BEAM_NO);
|
||||
Phy_RcdShift(&(instPtr->beamWd), INIT_ANT_WD);
|
||||
/* Update Frame Sate. */
|
||||
uint32_t txrxSate = PhyRead(instPtr, TXRX_STATE);
|
||||
instPtr->isAcq = ((txrxSate & TXRX_STATE_ACQ_MASK) == TXRX_STATE_ACQ_SUCCESS) ? true : false;
|
||||
instPtr->isFrameSync = ((txrxSate & TXRX_STATE_FSYNC_MASK) == TXRX_STATE_FSYNC_SUCCESS) ? true : false;
|
||||
instPtr->isDecode = ((txrxSate & TXRX_STATE_DECODE_MASK) == TXRX_STATE_DECODE_SUCCESS) ? true : false;
|
||||
|
||||
if (Phy_IsFrameReceived(instPtr))
|
||||
{
|
||||
DBG_INFO("Physical layer finished reception procedure, retrieving received data...\n");
|
||||
instPtr->isValid = true;
|
||||
}
|
||||
else
|
||||
{
|
||||
instPtr->isValid = false;
|
||||
}
|
||||
return;
|
||||
}
|
||||
|
||||
/*****************************************************************************/
|
||||
/**
|
||||
* @brief Initialize Physical Layer instance, allocate memory for buffers,
|
||||
* apply a self test.
|
||||
*
|
||||
* @param instPtr
|
||||
* phyConfig
|
||||
*
|
||||
* @return
|
||||
*
|
||||
******************************************************************************/
|
||||
int32_t Phy_InstInit(PHY_INST **instPtr, PHY_CONFIG *phyConfig)
|
||||
{
|
||||
ASSERT(instPtr != NULL);
|
||||
ASSERT(phyConfig != NULL);
|
||||
int32_t Status = SUCCESS;
|
||||
*instPtr = (PHY_INST *)malloc(sizeof(PHY_INST));
|
||||
if ((*instPtr) == NULL)
|
||||
{
|
||||
return ENOMEM;
|
||||
}
|
||||
|
||||
memcpy(&((*instPtr)->phyCofig), phyConfig, sizeof(PHY_CONFIG));
|
||||
(*instPtr)->isPhyEn = false;
|
||||
(*instPtr)->isAcq = false;
|
||||
(*instPtr)->isFrameSync = false;
|
||||
(*instPtr)->isDecode = false;
|
||||
(*instPtr)->isValid = false;
|
||||
(*instPtr)->isPresetHop = true;
|
||||
(*instPtr)->antConfigOffset = 0;
|
||||
(*instPtr)->sendOffset = 0;
|
||||
|
||||
memset(&((*instPtr))->TOR, PHY_TOR_SUSPEND, sizeof(PHY_RCD));
|
||||
memset(&((*instPtr)->frameType), L1_NONE_FRAME, sizeof(PHY_RCD));
|
||||
memset(&((*instPtr)->antNo), (int32_t)INIT_BEAM_NO, sizeof(PHY_RCD));
|
||||
memset(&((*instPtr)->beamNo), (int32_t)INIT_ANT_NO, sizeof(PHY_RCD));
|
||||
memset(&((*instPtr)->beamWd), (int32_t)INIT_ANT_WD, sizeof(PHY_RCD));
|
||||
memset(&((*instPtr)->hwSate), 0x00, sizeof(HWSTATE));
|
||||
|
||||
Status = Phy_AntConfigInit(&((*instPtr)->antConfigPtr));
|
||||
if (Status != SUCCESS)
|
||||
{
|
||||
goto init_error;
|
||||
}
|
||||
|
||||
XBram_Config *bramConfigPtr;
|
||||
XBram antConfigBram;
|
||||
XBram txDataBram;
|
||||
XBram rxDataBram;
|
||||
bramConfigPtr = XBram_LookupConfig(phyConfig->antBramId);
|
||||
if (bramConfigPtr == NULL)
|
||||
{
|
||||
Status = EDCONF;
|
||||
goto init_error;
|
||||
}
|
||||
Status = XBram_CfgInitialize(&antConfigBram, bramConfigPtr,
|
||||
bramConfigPtr->CtrlBaseAddress);
|
||||
if (Status != SUCCESS)
|
||||
{
|
||||
goto init_error;
|
||||
}
|
||||
/*
|
||||
* Execute the BRAM driver selftest.
|
||||
*/
|
||||
Status = XBram_SelfTest(&antConfigBram, 0);
|
||||
if (Status != SUCCESS)
|
||||
{
|
||||
goto init_error;
|
||||
}
|
||||
|
||||
bramConfigPtr = XBram_LookupConfig(phyConfig->txBramId);
|
||||
if (bramConfigPtr == NULL)
|
||||
{
|
||||
Status = EDCONF;
|
||||
goto init_error;
|
||||
}
|
||||
Status = XBram_CfgInitialize(&txDataBram, bramConfigPtr,
|
||||
bramConfigPtr->CtrlBaseAddress);
|
||||
if (Status != SUCCESS)
|
||||
{
|
||||
goto init_error;
|
||||
}
|
||||
/*
|
||||
* Execute the BRAM driver selftest.
|
||||
*/
|
||||
Status = XBram_SelfTest(&txDataBram, 0);
|
||||
if (Status != SUCCESS)
|
||||
{
|
||||
goto init_error;
|
||||
}
|
||||
|
||||
bramConfigPtr = XBram_LookupConfig(phyConfig->rxBramId);
|
||||
if (bramConfigPtr == NULL)
|
||||
{
|
||||
Status = EDCONF;
|
||||
goto init_error;
|
||||
}
|
||||
Status = XBram_CfgInitialize(&rxDataBram, bramConfigPtr,
|
||||
bramConfigPtr->CtrlBaseAddress);
|
||||
if (Status != SUCCESS)
|
||||
{
|
||||
goto init_error;
|
||||
}
|
||||
/*
|
||||
* Execute the BRAM driver selftest.
|
||||
*/
|
||||
Status = XBram_SelfTest(&rxDataBram, 0);
|
||||
if (Status != SUCCESS)
|
||||
{
|
||||
goto init_error;
|
||||
}
|
||||
|
||||
(*instPtr)->txDataPtr = txDataBram.Config.MemBaseAddress;
|
||||
(*instPtr)->rxDataPtr = rxDataBram.Config.MemBaseAddress;
|
||||
(*instPtr)->antConfigDataPtr = antConfigBram.Config.MemBaseAddress;
|
||||
|
||||
/* Initialize the XAdc driver. */
|
||||
XAdcPs_Config *xadcConfigPtr;
|
||||
|
||||
xadcConfigPtr = XAdcPs_LookupConfig(phyConfig->hwXadcId);
|
||||
if (xadcConfigPtr == NULL)
|
||||
{
|
||||
Status = EDCONF;
|
||||
goto init_error;
|
||||
}
|
||||
XAdcPs_CfgInitialize(&((*instPtr)->hwXadc), xadcConfigPtr, xadcConfigPtr->BaseAddress);
|
||||
|
||||
/* Self Test the XADC/ADC device */
|
||||
Status = XAdcPs_SelfTest(&((*instPtr)->hwXadc));
|
||||
if (Status != SUCCESS)
|
||||
{
|
||||
goto init_error;
|
||||
}
|
||||
|
||||
/* Disable the Channel Sequencer before configuring the Sequence registers. */
|
||||
XAdcPs_SetSequencerMode(&((*instPtr)->hwXadc), XADCPS_SEQ_MODE_SAFE);
|
||||
|
||||
return SUCCESS;
|
||||
init_error:
|
||||
free(*instPtr);
|
||||
return Status;
|
||||
}
|
||||
|
||||
/*****************************************************************************/
|
||||
/**
|
||||
* @brief Start physical layer
|
||||
*
|
||||
* @param instPtr a pointer to physical instance
|
||||
*
|
||||
* @return NONE
|
||||
*
|
||||
******************************************************************************/
|
||||
void Phy_Start(PHY_INST *instPtr)
|
||||
{
|
||||
ASSERT(instPtr != NULL);
|
||||
uint32_t phyMainCtrl = PhyRead(instPtr, PHY_MAIN_CTRL);
|
||||
if ((phyMainCtrl & PHY_MAIN_CTRL_EN_MASK) == PHY_MAIN_CTRL_EN_EN)
|
||||
{
|
||||
DBG_WARNING("Physical layer was already enabled.\n");
|
||||
}
|
||||
instPtr->isPhyEn = true;
|
||||
// Enable CRC check automatically
|
||||
PhyWrite(instPtr, PHY_MAIN_CTRL, (phyMainCtrl | PHY_MAIN_CTRL_EN_EN));
|
||||
}
|
||||
|
||||
/*****************************************************************************/
|
||||
/**
|
||||
* @brief Halt physical layer operation
|
||||
*
|
||||
* @param instPtr a pointer to physical instance
|
||||
*
|
||||
* @return NONE
|
||||
*
|
||||
******************************************************************************/
|
||||
void Phy_Halt(PHY_INST *instPtr)
|
||||
{
|
||||
ASSERT(instPtr != NULL);
|
||||
uint32_t phyMainCtrl = PhyRead(instPtr, PHY_MAIN_CTRL);
|
||||
if ((phyMainCtrl & PHY_MAIN_CTRL_EN_MASK) == PHY_MAIN_CTRL_EN_DSBL)
|
||||
{
|
||||
DBG_WARNING("Physical layer was allready halted.\n");
|
||||
}
|
||||
instPtr->isPhyEn = false;
|
||||
PhyWrite(instPtr, PHY_MAIN_CTRL, (phyMainCtrl & (~PHY_MAIN_CTRL_EN_MASK)));
|
||||
}
|
||||
|
||||
/*****************************************************************************/
|
||||
/**
|
||||
* @brief Update Physical Layer mode.
|
||||
*
|
||||
* @param instPtr
|
||||
* modeCod
|
||||
*
|
||||
* @return
|
||||
*
|
||||
******************************************************************************/
|
||||
int32_t Phy_SetMode(PHY_INST *instPtr, PHY_MODE modeCod)
|
||||
{
|
||||
ASSERT(instPtr != NULL);
|
||||
uint32_t phyMainCtrl = PhyRead(instPtr, PHY_MAIN_CTRL) & ~PHY_MAIN_CTRL_MODE_MASK;
|
||||
switch (modeCod)
|
||||
{
|
||||
case PHY_MODE_NONE:
|
||||
phyMainCtrl |= PHY_MAIN_CTRL_MODE_NONE;
|
||||
break;
|
||||
case PHY_MODE_LISTEN:
|
||||
phyMainCtrl |= PHY_MAIN_CTRL_MODE_LISTEN;
|
||||
break;
|
||||
case PHY_MODE_SCHEDULE:
|
||||
phyMainCtrl |= PHY_MAIN_CTRL_MODE_SCHEDULE;
|
||||
break;
|
||||
case PHY_MODE_PULL:
|
||||
phyMainCtrl |= PHY_MAIN_CTRL_MODE_PULL;
|
||||
break;
|
||||
default:
|
||||
return FAILURE;
|
||||
break;
|
||||
}
|
||||
PhyWrite(instPtr, PHY_MAIN_CTRL, phyMainCtrl | PHY_MAIN_CTRL_EN_EN);
|
||||
return SUCCESS;
|
||||
}
|
||||
|
||||
/*****************************************************************************/
|
||||
/**
|
||||
* @brief Setup next slot operation.
|
||||
*
|
||||
* @param instPtr
|
||||
* txrxMode
|
||||
* l2FrameType
|
||||
*
|
||||
* @return
|
||||
*
|
||||
******************************************************************************/
|
||||
int32_t Phy_SetupNextSlot(PHY_INST *instPtr, PHY_TOR_TYPE txrxMode,
|
||||
L1_FRAME_TYPE frameType, uint8_t *buff)
|
||||
{
|
||||
ASSERT(instPtr != NULL);
|
||||
int32_t Status = SUCCESS;
|
||||
uint32_t phyModeCtrl, antConfigBramAddr, hopNum;
|
||||
uint16_t *hopPattern;
|
||||
// Implement a ping-pong flow, offset always is 0 or half of buffer depth.
|
||||
instPtr->antConfigOffset = (instPtr->antConfigOffset == ANTCONFIG_MAX_LEN) ? 0 : ANTCONFIG_MAX_LEN;
|
||||
antConfigBramAddr = instPtr->antConfigDataPtr + instPtr->antConfigOffset;
|
||||
|
||||
instPtr->TOR.next = txrxMode;
|
||||
instPtr->frameType.next = frameType;
|
||||
|
||||
switch (txrxMode)
|
||||
{
|
||||
case PHY_TOR_SUSPEND:
|
||||
phyModeCtrl = PHY_MODE_CTRL_TOR_SUSPEND;
|
||||
break;
|
||||
case PHY_TOR_RX:
|
||||
phyModeCtrl = PHY_MODE_CTRL_TOR_RX;
|
||||
break;
|
||||
case PHY_TOR_TX:
|
||||
phyModeCtrl = PHY_MODE_CTRL_TOR_TX;
|
||||
break;
|
||||
default:
|
||||
return EPARAM;
|
||||
}
|
||||
if (instPtr->isPresetHop == false)
|
||||
{
|
||||
hopPattern = hopPattern_Config;
|
||||
}
|
||||
else
|
||||
{
|
||||
switch (frameType)
|
||||
{
|
||||
case L1_2KBPS_20MS_FRAME:
|
||||
phyModeCtrl |= PHY_MODE_CTRL_FRAME_2KBPS_20MS;
|
||||
hopNum = L1_FRAME_2KBPS_20MS_HOPS;
|
||||
// FIXME: For DUIFEI net setup.
|
||||
// hopPattern = (txrxMode == TOR_TX) ? hopPattern_2KBPS_20MS;
|
||||
hopPattern = hopPattern_2KBPS_100MS;
|
||||
break;
|
||||
case L1_2KBPS_100MS_FRAME:
|
||||
phyModeCtrl |= PHY_MODE_CTRL_FRAME_2KBPS_100MS;
|
||||
hopNum = L1_FRAME_2KBPS_100MS_HOPS;
|
||||
hopPattern = hopPattern_2KBPS_100MS;
|
||||
break;
|
||||
case L1_10KBPS_10MS_FRAME:
|
||||
phyModeCtrl |= PHY_MODE_CTRL_FRAME_10KBPS_10MS;
|
||||
hopNum = L1_FRAME_10KBPS_10MS_HOPS;
|
||||
hopPattern = hopPattern_10KBPS_10MS;
|
||||
break;
|
||||
case L1_10KBPS_20MS_FRAME:
|
||||
phyModeCtrl |= PHY_MODE_CTRL_FRAME_10KBPS_20MS;
|
||||
hopNum = L1_FRAME_10KBPS_20MS_HOPS;
|
||||
hopPattern = hopPattern_10KBPS_20MS;
|
||||
break;
|
||||
case L1_40KBPS_20MS_FRAME:
|
||||
phyModeCtrl |= PHY_MODE_CTRL_FRAME_200KBPS_5MS;
|
||||
hopNum = L1_FRAME_200KBPS_5MS_HOPS;
|
||||
hopPattern = hopPattern_200KBPS_5MS;
|
||||
break;
|
||||
case L1_40KBPS_10MS_FRAME:
|
||||
phyModeCtrl |= PHY_MODE_CTRL_FRAME_40KBPS_10MS;
|
||||
hopNum = L1_FRAME_40KBPS_10MS_HOPS;
|
||||
hopPattern = hopPattern_40KBPS_10MS;
|
||||
break;
|
||||
case L1_200KBPS_5MS_FRAME:
|
||||
phyModeCtrl |= PHY_MODE_CTRL_FRAME_200KBPS_5MS;
|
||||
hopNum = L1_FRAME_200KBPS_5MS_HOPS;
|
||||
hopPattern = hopPattern_200KBPS_5MS;
|
||||
break;
|
||||
case L1_1M2BPS_5MS_FRAME:
|
||||
phyModeCtrl |= PHY_MODE_CTRL_FRAME_1M2BPS_5MS;
|
||||
hopNum = L1_FRAME_1M2BPS_5MS_HOPS;
|
||||
hopPattern = hopPattern_1M2BPS_5MS;
|
||||
break;
|
||||
default:
|
||||
return EPARAM;
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
PhyWrite(instPtr, PHY_MODE_CTRL, phyModeCtrl);
|
||||
|
||||
// TODO: Set antenna stand by, when suspend.
|
||||
const uint8_t *dataSrcPtr = (uint8_t *)(&(instPtr->antConfigPtr));
|
||||
for (int32_t i = 0; i < hopNum; i++)
|
||||
{
|
||||
instPtr->antConfigPtr.antFreq[1] = (uint8_t)(hopPattern[i] & 0x00FF);
|
||||
instPtr->antConfigPtr.antFreq[0] = (uint8_t)((hopPattern[i] >> 8) & 0x00FF);
|
||||
instPtr->antConfigPtr.rfFreq[1] = (uint8_t)(hopPattern[i] & 0x00FF);
|
||||
instPtr->antConfigPtr.rfFreq[0] = (uint8_t)((hopPattern[i] >> 8) & 0x00FF);
|
||||
memcpy((uint8_t *)(antConfigBramAddr + i * ANT_COINFO_LENGTH),
|
||||
dataSrcPtr, sizeof(PHY_ANT_COINFO));
|
||||
}
|
||||
uint32_t antCtrl = (instPtr->antConfigOffset & ANT_CONFIG_CMD_OFFSET_MASK) | ((hopNum << ANT_CONFIG_CMD_NUM_SHIFT) & ANT_CONFIG_CMD_NUM_MASK) | ((ANT_COINFO_LENGTH << ANT_CONFIG_CMD_LEN_SHIFT) & ANT_CONFIG_CMD_LEN_MASK);
|
||||
PhyWrite(instPtr, ANT_CONFIG_CMD, antCtrl);
|
||||
|
||||
// FIXME: For DUIFEI network setup. Should only set send data in send slots.
|
||||
if (txrxMode == PHY_TOR_TX)
|
||||
{
|
||||
Status = SetSendData(instPtr, frameType, buff);
|
||||
if (Status != SUCCESS)
|
||||
{
|
||||
return Status;
|
||||
}
|
||||
}
|
||||
|
||||
// Trigger physical configuration.
|
||||
PhyWrite(instPtr, SLOT_CONFIG_CMD, SLOT_CONFIG_CMD_VAULE); // Phy start fetching data.
|
||||
return SUCCESS;
|
||||
}
|
||||
|
||||
/*****************************************************************************/
|
||||
/**
|
||||
* @brief Get the corresponding frequency hopping pattern length of a
|
||||
* specified L1 frame type.
|
||||
*
|
||||
* @param frameType a L1 frame type enumeration.
|
||||
*
|
||||
* @return SUCCESS
|
||||
* FAILURE frame type cannot be recognized
|
||||
*
|
||||
******************************************************************************/
|
||||
uint32_t Phy_GetHopLength(L1_FRAME_TYPE frameType)
|
||||
{
|
||||
switch (frameType)
|
||||
{
|
||||
case L1_2KBPS_20MS_FRAME:
|
||||
return L1_FRAME_2KBPS_20MS_HOPS;
|
||||
case L1_2KBPS_100MS_FRAME:
|
||||
return L1_FRAME_2KBPS_100MS_HOPS;
|
||||
case L1_10KBPS_10MS_FRAME:
|
||||
return L1_FRAME_10KBPS_10MS_HOPS;
|
||||
case L1_10KBPS_20MS_FRAME:
|
||||
return L1_FRAME_10KBPS_20MS_HOPS;
|
||||
case L1_40KBPS_20MS_FRAME:
|
||||
return L1_FRAME_40KBPS_20MS_HOPS;
|
||||
case L1_40KBPS_10MS_FRAME:
|
||||
return L1_FRAME_40KBPS_10MS_HOPS;
|
||||
case L1_200KBPS_5MS_FRAME:
|
||||
return L1_FRAME_200KBPS_5MS_HOPS;
|
||||
case L1_1M2BPS_5MS_FRAME:
|
||||
return L1_FRAME_1M2BPS_5MS_HOPS;
|
||||
default:
|
||||
return L1_FRAME_NONE_LEN;
|
||||
}
|
||||
}
|
||||
|
||||
/*****************************************************************************/
|
||||
/**
|
||||
* @brief Get the time from frame sync to slot end of different type.
|
||||
*
|
||||
* @param frameType
|
||||
*
|
||||
* @return 32bit unsigned integer (clock periods)
|
||||
*
|
||||
******************************************************************************/
|
||||
uint32_t Phy_GetTimeComp(L1_FRAME_TYPE frameType)
|
||||
{
|
||||
switch (frameType)
|
||||
{
|
||||
case L1_2KBPS_20MS_FRAME:
|
||||
return L1_FRAME_2KBPS_20MS_COMP;
|
||||
case L1_2KBPS_100MS_FRAME:
|
||||
return L1_FRAME_2KBPS_100MS_COMP;
|
||||
case L1_10KBPS_10MS_FRAME:
|
||||
return L1_FRAME_10KBPS_10MS_COMP;
|
||||
case L1_10KBPS_20MS_FRAME:
|
||||
return L1_FRAME_10KBPS_20MS_COMP;
|
||||
case L1_40KBPS_20MS_FRAME:
|
||||
return L1_FRAME_40KBPS_20MS_COMP;
|
||||
case L1_40KBPS_10MS_FRAME:
|
||||
return L1_FRAME_40KBPS_10MS_COMP;
|
||||
case L1_200KBPS_5MS_FRAME:
|
||||
return L1_FRAME_200KBPS_5MS_COMP;
|
||||
case L1_1M2BPS_5MS_FRAME:
|
||||
return L1_FRAME_1M2BPS_5MS_COMP;
|
||||
default:
|
||||
return L1_FRAME_NONE_LEN;
|
||||
}
|
||||
}
|
||||
|
||||
/*****************************************************************************/
|
||||
/**
|
||||
* @brief Get latest frequency deviation estimation.
|
||||
*
|
||||
* @param instPtr
|
||||
*
|
||||
* @return 32bit signed integer (Hz)
|
||||
*
|
||||
******************************************************************************/
|
||||
int32_t Phy_GetFreqEst(PHY_INST *instPtr)
|
||||
{
|
||||
uint32_t temp;
|
||||
int32_t acqCh, fsCh;
|
||||
double freqEst;
|
||||
temp = PhyRead(instPtr, COASE_FREQEST);
|
||||
acqCh = temp & 0x000000FF;
|
||||
acqCh = 80 - acqCh;
|
||||
fsCh = (temp >> 8) & 0x000000FF;
|
||||
fsCh = fsCh - 128;
|
||||
freqEst = acqCh * (31.25e3 / 4) + fsCh * (31.25e3 / 256);
|
||||
return ((int32_t)freqEst);
|
||||
}
|
||||
|
||||
/*****************************************************************************/
|
||||
/**
|
||||
* @brief Setup a frequency hopping pattern.
|
||||
*
|
||||
* @param patternBuff a pointer to hopping pattern buffer
|
||||
* @param patternLen a integer indicate the length of hopping pattern.
|
||||
*
|
||||
* @return SUCCESS
|
||||
* FAILURE pattern length exceeds maximum length.
|
||||
*
|
||||
******************************************************************************/
|
||||
int32_t Phy_SetHopPattern(void *patternBuff, uint32_t patternLen)
|
||||
{
|
||||
ASSERT(patternBuff != NULL);
|
||||
if (patternLen > L1_FRAME_MAX_HOPS)
|
||||
{
|
||||
return FAILURE;
|
||||
}
|
||||
memcpy(hopPattern_Config, patternBuff, patternLen * sizeof(uint16_t));
|
||||
return SUCCESS;
|
||||
}
|
||||
|
||||
/*****************************************************************************/
|
||||
/**
|
||||
* @brief Setup a specific timer word register in physical controller.
|
||||
*
|
||||
* @param instPtr
|
||||
* @param tmWord
|
||||
* @param tmType
|
||||
*
|
||||
* @return SUCCESS
|
||||
* FAILURE pattern length exceeds maximum length.
|
||||
*
|
||||
******************************************************************************/
|
||||
int32_t Phy_SetTimeWord(PHY_INST *instPtr, uint64_t tmWord, PHY_TIME_WORD_TYPE tmType)
|
||||
{
|
||||
ASSERT(instPtr != NULL);
|
||||
switch (tmType)
|
||||
{
|
||||
case PHY_TIME_WORD_POLL:
|
||||
PhyWrite(instPtr, POLLTIME_L, (uint32_t)tmWord);
|
||||
PhyWrite(instPtr, POLLTIME_H, (uint32_t)(tmWord >> 32));
|
||||
break;
|
||||
case PHY_TIME_WORD_LOCAL_ADJ:
|
||||
PhyWrite(instPtr, LOCALTM_ADJ_L, (uint32_t)tmWord);
|
||||
PhyWrite(instPtr, LOCALTM_ADJ_H, (uint32_t)(tmWord >> 32));
|
||||
break;
|
||||
case PHY_TIME_WORD_STEP:
|
||||
PhyWrite(instPtr, CNT_STEP_OPT, (uint32_t)tmWord);
|
||||
break;
|
||||
default:
|
||||
return EPARAM;
|
||||
}
|
||||
return SUCCESS;
|
||||
}
|
||||
|
||||
/*****************************************************************************/
|
||||
/**
|
||||
* @brief Setup a specific timer word register in physical controller.
|
||||
*
|
||||
* @param instPtr a pointer to hopping pattern buffer
|
||||
* @param patternLen a integer indicate the length of hopping pattern.
|
||||
*
|
||||
* @return SUCCESS
|
||||
* FAILURE pattern length exceeds maximum length.
|
||||
*
|
||||
******************************************************************************/
|
||||
void Phy_UpdateHwSate(PHY_INST *instPtr)
|
||||
{
|
||||
ASSERT(instPtr != NULL);
|
||||
uint32_t rawData;
|
||||
|
||||
// -- Update Chip voltage, unit mV
|
||||
rawData = XAdcPs_GetAdcData(&(instPtr->hwXadc), XADCPS_CH_VCCPINT);
|
||||
instPtr->hwSate.spVolt = (uint16_t)(XAdcPs_RawToVoltage(rawData) * 1000);
|
||||
|
||||
// -- Update Chip temperature, unit 0.01 deg
|
||||
rawData = XAdcPs_GetAdcData(&(instPtr->hwXadc), XADCPS_CH_TEMP);
|
||||
instPtr->hwSate.spTemp = (uint16_t)(XAdcPs_RawToTemperature(rawData) * 100);
|
||||
|
||||
// FIXME: Chip status determine.
|
||||
instPtr->hwSate.chipSt = 0;
|
||||
|
||||
/*** Voltage and Status ***/
|
||||
rawData = PhyRead(instPtr, ANT_STATUS);
|
||||
// -- Update Antenna Status Code: 0xAA - normal, 0x55 - error
|
||||
instPtr->hwSate.antSt = ((rawData & ANT_STATUS_CODE_MASK) == 0) ? 0xAA : 0x55;
|
||||
// -- Update Antenna Voltage Status: 1 - normal, 0 - error
|
||||
// [0] - ANT voltage status
|
||||
// [2] - RF voltage status
|
||||
// [4] - NIL voltage status
|
||||
instPtr->hwSate.antVolt = (rawData & ANT_STATUS_ANT_VOLT_MASK) ? 0x01 : 0x00;
|
||||
instPtr->hwSate.antVolt |= (rawData & ANT_STATUS_RF_VOLT_MASK) ? 0x04 : 0x00;
|
||||
instPtr->hwSate.antVolt |= (rawData & ANT_STATUS_NIL_VOLT_MASK) ? 0x10 : 0x00;
|
||||
|
||||
/*** Temperature and Lock Status ***/
|
||||
rawData = PhyRead(instPtr, RF_STATUS);
|
||||
//-- Update Antenna Temperature (signed 8bit integer, 1 centigrade per LSB)
|
||||
instPtr->hwSate.antTemp = (uint8_t)(rawData & RF_STATUS_TEMP_MASK);
|
||||
// -- Update Antenna PLL Lock Status: 1 - normal, 0 - error
|
||||
// [0] - BASEBAND lock status
|
||||
// [2] - PLLA lock status
|
||||
// [4] - PLLB lock status
|
||||
instPtr->hwSate.antPll = (rawData & RF_STATUS_BDLOCK_MASK) ? 0x01 : 0x00;
|
||||
instPtr->hwSate.antPll |= (rawData & RF_STATUS_PLLA_MASK) ? 0x04 : 0x00;
|
||||
instPtr->hwSate.antPll |= (rawData & RF_STATUS_PLLB_MASK) ? 0x10 : 0x00;
|
||||
|
||||
/*** MGCs ***/
|
||||
rawData = PhyRead(instPtr, RF_MGC);
|
||||
// -- Update Antenna MGC (0~15 dB)
|
||||
// [3:0] - Channel 1 MGC
|
||||
// [7:4] - Channel 2 MGC
|
||||
// [11:8] - Channel 3 MGC
|
||||
// [15:12] - Channel 4 MGC
|
||||
instPtr->hwSate.antMgc = (rawData & RF_MGC_CH1_MASK);
|
||||
instPtr->hwSate.antMgc |= (rawData & RF_MGC_CH2_MASK) >> 4;
|
||||
instPtr->hwSate.antMgc |= (rawData & RF_MGC_CH3_MASK) >> 8;
|
||||
instPtr->hwSate.antMgc |= (rawData & RF_MGC_CH4_MASK) >> 12;
|
||||
SysMonitor_UpdateHardwareStatus(&(instPtr->hwSate));
|
||||
}
|
||||
@@ -0,0 +1,249 @@
|
||||
/*****************************************************************************/
|
||||
/**
|
||||
* Description: Physical layer control definitions and function prototypes.
|
||||
*
|
||||
* Date: 2021-12-04 16:32
|
||||
* LastEditTime: 2023-04-24 17:41
|
||||
* Revisions: Rev 0.1
|
||||
* History:
|
||||
* [2021-12-08]: Initial version.
|
||||
* [2022-06-18]: New branch for glink control purpose:
|
||||
* -Add poll mode, physical layer will start Tx/Rx procedure
|
||||
* at specific time stamp.
|
||||
* -Add 61/37 beam switch
|
||||
* -Code simplification.
|
||||
* [2023-04-24]: Porting for DUIFEI.
|
||||
*
|
||||
* Copyright(c) 2021 Beijing Institute of Technology.
|
||||
* Lab of Communication and Networking
|
||||
*
|
||||
******************************************************************************/
|
||||
|
||||
#ifndef PHY_CTRL_H_
|
||||
#define PHY_CTRL_H_
|
||||
|
||||
/***************************** Include Files *********************************/
|
||||
#include <stdint.h>
|
||||
#include "common.h"
|
||||
#include "xbram.h"
|
||||
#include "xadcps.h"
|
||||
#include "phy_reg.h"
|
||||
#include "../../system_monitor.h"
|
||||
/************************** Constant Definitions *****************************/
|
||||
#define L1_FRAME_NONE_HOPS UINT32_MAX
|
||||
#define L1_FRAME_MAX_HOPS 81
|
||||
#define L1_FRAME_2KBPS_20MS_HOPS 1
|
||||
#define L1_FRAME_2KBPS_100MS_HOPS 81
|
||||
#define L1_FRAME_10KBPS_10MS_HOPS 1
|
||||
#define L1_FRAME_10KBPS_20MS_HOPS 11
|
||||
#define L1_FRAME_40KBPS_20MS_HOPS 19
|
||||
#define L1_FRAME_40KBPS_10MS_HOPS 9
|
||||
#define L1_FRAME_200KBPS_5MS_HOPS 20
|
||||
#define L1_FRAME_1M2BPS_5MS_HOPS 20
|
||||
|
||||
#define L1_NUM_ANT_SUBNRRW_PATTERN 37
|
||||
#define L1_NUM_ANT_NRRW_PATTERN 61
|
||||
|
||||
#define ANT_COINFO_LENGTH 32 // bytes
|
||||
/***************** Macros (Inline Functions) Definitions *********************/
|
||||
/**************************** Type Definitions *******************************/
|
||||
typedef enum
|
||||
{
|
||||
L1_NONE_FRAME = 0,
|
||||
L1_2KBPS_20MS_FRAME = 1,
|
||||
L1_2KBPS_100MS_FRAME = 2,
|
||||
L1_10KBPS_10MS_FRAME = 3,
|
||||
L1_10KBPS_20MS_FRAME = 4,
|
||||
L1_40KBPS_20MS_FRAME = 5,
|
||||
L1_40KBPS_10MS_FRAME = 6,
|
||||
L1_200KBPS_5MS_FRAME = 7,
|
||||
L1_1M2BPS_5MS_FRAME = 8
|
||||
} L1_FRAME_TYPE;
|
||||
|
||||
typedef struct
|
||||
{
|
||||
/*** Ant Control ***/
|
||||
/* Antenna control frame head 0xAA */
|
||||
/* default 0, reserved for further use */
|
||||
uint8_t ctrlWord;
|
||||
/* high 4 bits specified beam mode.
|
||||
* low 4 bits specified power control */
|
||||
uint8_t antMode;
|
||||
/* off-axis angle range is 0~60,
|
||||
* 0.1 degree each LSB */
|
||||
uint8_t oaxis[2];
|
||||
/* round-axis angle range is 0~360,
|
||||
* 0.1 degree each LSB */
|
||||
uint8_t raxis[2];
|
||||
/* 0~3500Mhz, step 1MHz */
|
||||
uint8_t antFreq[2];
|
||||
/* in single tr mode, 0~63 specified tr channel
|
||||
* in single subarray mode, 0~3 specified the subarray
|
||||
* Only in debug mode*/
|
||||
uint8_t trSlect;
|
||||
/* Antenna attenuation 0~31, 0.5dB each LSB, Only in debug mode */
|
||||
uint8_t antAtt;
|
||||
/* 0~63, 5.625 degree each LSB, Only in debug mode*/
|
||||
uint8_t phase;
|
||||
/* Only for debug */
|
||||
/* default 0x00 */
|
||||
uint8_t rxtxMode;
|
||||
/* 0x00 Normal, 0x01 single tr calibration */
|
||||
uint8_t calibrMode;
|
||||
/* reserved for further use */
|
||||
uint8_t antReserved;
|
||||
|
||||
/*** RF Control ***/
|
||||
/* [7:4]RF enable: 0-On, 1-Off(Default); [3:0]Work mode: 0(default) */
|
||||
uint8_t rfCtrl;
|
||||
/* 0~3500Mhz, step 1MHz */
|
||||
uint8_t rfFreq[2];
|
||||
/* RF MGC control: 0~31, 1dB each LSB*/
|
||||
uint8_t rfTxMgc;
|
||||
/* RF channel 1&2 calibration mgc: [7:4] Channle1 - max 15db, 1dB each LSB
|
||||
* [3:0] Channle2 - max 15db, 1dB each LSB*/
|
||||
uint8_t rf12CalMgc;
|
||||
/* RF channel 3&4 calibration mgc: [7:4] Channle3 - max 15db, 1dB each LSB
|
||||
* [3:0] Channle4 - max 15db, 1dB each LSB*/
|
||||
uint8_t rf34CalMgc;
|
||||
/* Receive channel 1-4 MGC: 0~31, 1dB each LSB */
|
||||
uint8_t rcvMgc;
|
||||
/* Receive monitor, 0-normal, 1-switch to monitor port */
|
||||
uint8_t rfMonitor;
|
||||
/* reserved for further use */
|
||||
uint8_t rfReserved0;
|
||||
uint8_t rfReserved1;
|
||||
} PHY_ANT_COINFO;
|
||||
|
||||
typedef struct
|
||||
{
|
||||
/* off-axis angle range is 0~60,
|
||||
* 0.1 degree each LSB */
|
||||
uint16_t oaxis;
|
||||
/* round-axis angle range is 0~360,
|
||||
* 0.1 degree each LSB */
|
||||
uint16_t raxis;
|
||||
} PHY_ANT_ANGLE;
|
||||
|
||||
typedef enum
|
||||
{
|
||||
PHY_TIME_WORD_NONE = 0,
|
||||
PHY_TIME_WORD_POLL = 1,
|
||||
PHY_TIME_WORD_LISTEN_TIMEOUT = 2,
|
||||
PHY_TIME_WORD_LOCAL_ADJ = 3,
|
||||
PHY_TIME_WORD_STEP = 4,
|
||||
} PHY_TIME_WORD_TYPE;
|
||||
|
||||
typedef enum
|
||||
{
|
||||
PHY_ANT_BEAM_NONE = 0,
|
||||
PHY_ANT_BEAM_120 = 1,
|
||||
PHY_ANT_BEAM_60 = 2,
|
||||
PHY_ANT_BEAM_25 = 3,
|
||||
PHY_ANT_BEAM_8 = 4
|
||||
} PHY_ANT_BEAM_WIDTH;
|
||||
|
||||
typedef enum
|
||||
{
|
||||
PHY_TOR_SUSPEND = 0,
|
||||
PHY_TOR_TX = 1,
|
||||
PHY_TOR_RX = 2
|
||||
} PHY_TOR_TYPE;
|
||||
|
||||
typedef enum
|
||||
{
|
||||
PHY_MODE_NONE = 0,
|
||||
PHY_MODE_LISTEN = 1,
|
||||
PHY_MODE_SCHEDULE = 2,
|
||||
PHY_MODE_PULL = 3
|
||||
} PHY_MODE;
|
||||
|
||||
typedef struct
|
||||
{
|
||||
uint32_t devId;
|
||||
uint32_t txBramId;
|
||||
uint32_t rxBramId;
|
||||
uint32_t antBramId;
|
||||
uint32_t hwXadcId;
|
||||
uint32_t baseAddr;
|
||||
} PHY_CONFIG;
|
||||
|
||||
typedef struct
|
||||
{
|
||||
int32_t last;
|
||||
int32_t curr;
|
||||
int32_t next;
|
||||
} PHY_RCD;
|
||||
|
||||
typedef struct
|
||||
{
|
||||
PHY_CONFIG phyCofig; // Device address configs
|
||||
bool isPhyEn; // if physical layer running
|
||||
bool isAcq; // if last reception acquisition success
|
||||
bool isFrameSync; // if last reception frame sync success
|
||||
bool isDecode; // if last reception decode success
|
||||
bool isValid; // if last reception frame data valid (crc check pass)
|
||||
bool isPresetHop; // if using preset frequency hop
|
||||
PHY_RCD antNo; // antenna number record
|
||||
PHY_RCD beamNo; // beam number record
|
||||
PHY_RCD beamWd; // beam width record
|
||||
PHY_RCD TOR; // tx rx state record
|
||||
PHY_RCD frameType; // frame type record
|
||||
uint32_t antConfigOffset; // antenna configuration bram offset (to implement ping-pong data flow)
|
||||
uint32_t sendOffset; // antenna configuration bram offset (to implement ping-pong data flow)
|
||||
PHY_ANT_COINFO antConfigPtr; // antenna configuration pointer
|
||||
uintptr_t txDataPtr;
|
||||
uintptr_t rxDataPtr;
|
||||
uintptr_t antConfigDataPtr;
|
||||
XAdcPs hwXadc;
|
||||
HWSTATE hwSate;
|
||||
} PHY_INST;
|
||||
|
||||
/************************** Variable Definitions *****************************/
|
||||
extern uint16_t hopPattern_Config[L1_FRAME_MAX_HOPS];
|
||||
extern uint16_t hopPattern_2KBPS_20MS[L1_FRAME_2KBPS_20MS_HOPS];
|
||||
extern uint16_t hopPattern_2KBPS_100MS[L1_FRAME_2KBPS_100MS_HOPS];
|
||||
extern uint16_t hopPattern_10KBPS_10MS[L1_FRAME_10KBPS_10MS_HOPS];
|
||||
extern uint16_t hopPattern_10KBPS_20MS[L1_FRAME_10KBPS_20MS_HOPS];
|
||||
extern uint16_t hopPattern_40KBPS_10MS[L1_FRAME_40KBPS_10MS_HOPS];
|
||||
extern uint16_t hopPattern_40KBPS_20MS[L1_FRAME_40KBPS_20MS_HOPS];
|
||||
|
||||
extern uint16_t hopPattern_200KBPS_5MS[L1_FRAME_200KBPS_5MS_HOPS];
|
||||
extern uint16_t hopPattern_1M2BPS_5MS[L1_FRAME_1M2BPS_5MS_HOPS];
|
||||
|
||||
extern PHY_ANT_ANGLE antSubnrrwPattern[L1_NUM_ANT_SUBNRRW_PATTERN];
|
||||
extern PHY_ANT_ANGLE antNrrwPattern[L1_NUM_ANT_NRRW_PATTERN];
|
||||
|
||||
/************************** Function Prototypes ******************************/
|
||||
int32_t Phy_InstInit(PHY_INST **instPtr, PHY_CONFIG *phyConfig);
|
||||
void Phy_UpdateState(PHY_INST *instPtr);
|
||||
void Phy_UpdateHwSate(PHY_INST *instPtr);
|
||||
|
||||
void Phy_Start(PHY_INST *instPtr);
|
||||
void Phy_Halt(PHY_INST *instPtr);
|
||||
int32_t Phy_SetMode(PHY_INST *instPtr, PHY_MODE modeCod);
|
||||
|
||||
int32_t Phy_SetupNextSlot(PHY_INST *instPtr, PHY_TOR_TYPE txrxMode, L1_FRAME_TYPE frameType, uint8_t *buff);
|
||||
int32_t Phy_RetrieveRecvData(PHY_INST *instPtr, uint8_t *buff);
|
||||
|
||||
bool Phy_IsFrameDetected(PHY_INST *instPtr);
|
||||
bool Phy_IsFrameReceived(PHY_INST *instPtr);
|
||||
bool Phy_IsFrameValid(PHY_INST *instPtr);
|
||||
uint32_t Phy_GetFrameLen(L1_FRAME_TYPE frameType);
|
||||
|
||||
L1_FRAME_TYPE Phy_GetLastDetectedFrameType(PHY_INST *instPtr);
|
||||
L1_FRAME_TYPE Phy_GetLastReceivedFrameType(PHY_INST *instPtr);
|
||||
L1_FRAME_TYPE Phy_GetLastSentFrameType(PHY_INST *instPtr);
|
||||
|
||||
int32_t Phy_SetHopPattern(void *patternBuff, uint32_t patternLen);
|
||||
uint32_t Phy_GetHopLength(L1_FRAME_TYPE frameType);
|
||||
uint32_t Phy_GetTimeComp(L1_FRAME_TYPE frameType);
|
||||
int32_t Phy_GetFreqEst(PHY_INST *instPtr);
|
||||
int32_t Phy_SetTimeWord(PHY_INST *instPtr, uint64_t tmWord, PHY_TIME_WORD_TYPE tmType);
|
||||
|
||||
int32_t Phy_AntConfigInit(PHY_ANT_COINFO *antCoinfoPtr);
|
||||
int32_t Phy_SetBeamWidth(PHY_INST *instPtr, PHY_ANT_BEAM_WIDTH beamWidth);
|
||||
int32_t Phy_SetBeamNo(PHY_INST *instPtr, int32_t beamNo);
|
||||
int32_t Phy_SetBeamAngle(PHY_INST *phyInstPtr, double offAx, double rndAx);
|
||||
|
||||
#endif /* PHY_CTRL_H_ */
|
||||
@@ -0,0 +1,334 @@
|
||||
/*****************************************************************************/
|
||||
/**
|
||||
* Description: Physical layer register definitions
|
||||
*
|
||||
* Author: Fan Hao
|
||||
* Date: 2021-11-16 00:08
|
||||
* LastEditors: Fan Hao
|
||||
* LastEditTime: 2023-04-24 17:32
|
||||
* Revisions: Rev 0.1
|
||||
* History:
|
||||
* [2021-11-16]: Initial version.
|
||||
* [2022-06-18]: New branch for glink control purpose:
|
||||
* -Add poll modephysical layer will start Tx/Rx procedure
|
||||
* at specific time stamp.
|
||||
* [2023-04-24]: Porting for DUIFEI.
|
||||
*
|
||||
* Copyright(c) 2021 Beijing Institute of Technology.
|
||||
* Lab of Communication and Networking
|
||||
*
|
||||
******************************************************************************/
|
||||
|
||||
#ifndef PHY_CTRLREG_H_
|
||||
#define PHY_CTRLREG_H_
|
||||
|
||||
/***************************** Include Files *********************************/
|
||||
#include "util.h"
|
||||
|
||||
/***************** Macros (Inline Functions) Definitions *********************/
|
||||
|
||||
// Read Physical Layer register (0x00000000~0x000000FF)
|
||||
#define PhyRead(instPtr, addr) IO_In32(instPtr->phyCofig.baseAddr + addr)
|
||||
// Write Physical Layer register (0x000000100~0x00000200)
|
||||
#define PhyWrite(instPtr, addr, value) IO_Out32(instPtr->phyCofig.baseAddr + addr, value)
|
||||
|
||||
/************************** Constant Definitions *****************************/
|
||||
/************************* Ps2Pl Regs PS Write Only ***************************/
|
||||
/**
|
||||
* Register: PHY_MAIN_CTRL
|
||||
*/
|
||||
#define PHY_MAIN_CTRL 0X00000000U
|
||||
#define PHY_MAIN_CTRL_EN_SHIFT 0
|
||||
#define PHY_MAIN_CTRL_EN_WIDTH 1
|
||||
#define PHY_MAIN_CTRL_EN_MASK 0x00000001U
|
||||
#define PHY_MAIN_CTRL_MODE_SHIFT 8
|
||||
#define PHY_MAIN_CTRL_MODE_WIDTH 8
|
||||
#define PHY_MAIN_CTRL_MODE_MASK 0x0000FF00U
|
||||
|
||||
#define PHY_MAIN_CTRL_EN_EN 0x00000001U
|
||||
#define PHY_MAIN_CTRL_EN_DSBL 0x00000000U
|
||||
#define PHY_MAIN_CTRL_MODE_NONE 0x00000000U
|
||||
#define PHY_MAIN_CTRL_MODE_LISTEN 0x00000100U
|
||||
#define PHY_MAIN_CTRL_MODE_SCHEDULE 0x00000200U
|
||||
#define PHY_MAIN_CTRL_MODE_PULL 0x00000300U
|
||||
|
||||
/**
|
||||
* Register: TXRX_CTRL
|
||||
*/
|
||||
#define PHY_MODE_CTRL 0x00000004U
|
||||
#define PHY_MODE_CTRL_TOR_SHIFT 0
|
||||
#define PHY_MODE_CTRL_TOR_WIDTH 2
|
||||
#define PHY_MODE_CTRL_TOR_MASK 0x00000003U
|
||||
#define PHY_MODE_CTRL_FRAME_SHIFT 16
|
||||
#define PHY_MODE_CTRL_FRAME_WIDTH 8
|
||||
#define PHY_MODE_CTRL_FRAME_MASK 0x00FF0000U
|
||||
|
||||
#define PHY_MODE_CTRL_TOR_SUSPEND 0x00000000U
|
||||
#define PHY_MODE_CTRL_TOR_TX 0x00000001U
|
||||
#define PHY_MODE_CTRL_TOR_RX 0x00000002U
|
||||
|
||||
#define PHY_MODE_CTRL_FRAME_NONE 0x00000000U
|
||||
#define PHY_MODE_CTRL_FRAME_2KBPS_20MS 0x00000100U
|
||||
#define PHY_MODE_CTRL_FRAME_2KBPS_100MS 0x00000200U
|
||||
#define PHY_MODE_CTRL_FRAME_10KBPS_10MS 0x00000300U // Initial Scan Frame
|
||||
#define PHY_MODE_CTRL_FRAME_10KBPS_20MS 0x00000400U // Benchmark Frame
|
||||
#define PHY_MODE_CTRL_FRAME_40KBPS_20MS 0x00000500U
|
||||
#define PHY_MODE_CTRL_FRAME_40KBPS_10MS 0x00000600U
|
||||
#define PHY_MODE_CTRL_FRAME_200KBPS_5MS 0x00000700U // 2-Way ACQ Frame, Management Frame, Service Frame
|
||||
#define PHY_MODE_CTRL_FRAME_1M2BPS_5MS 0x00000800U // Bistatic Synthetic Aperture Radar Frame
|
||||
|
||||
/**
|
||||
* Register: POLLTIME_L
|
||||
*/
|
||||
#define POLLTIME_L 0x00000008U
|
||||
#define POLLTIME_L_SHIFT 0
|
||||
#define POLLTIME_L_WIDTH 32
|
||||
#define POLLTIME_L_MASK 0xFFFFFFFFU
|
||||
|
||||
/**
|
||||
* Register: POLLTIME_H
|
||||
*/
|
||||
#define POLLTIME_H 0x0000000CU
|
||||
#define POLLTIME_H_SHIFT 0
|
||||
#define POLLTIME_H_WIDTH 32
|
||||
#define POLLTIME_H_MASK 0xFFFFFFFFU
|
||||
|
||||
/**
|
||||
* Register: TM_ADJ_MODE
|
||||
*/
|
||||
#define TM_ADJ_MODE 0x00000010U
|
||||
#define TM_ADJ_MODE_SHIFT 0
|
||||
#define TM_ADJ_MODE_WIDTH 8
|
||||
#define TM_ADJ_MODE_MASK 0x000000FFU
|
||||
#define TM_ADJ_MODE_NONE 0x00000000U
|
||||
#define TM_ADJ_MODE_ADD 0x00000001U
|
||||
#define TM_ADJ_MODE_SET 0x00000002U
|
||||
|
||||
#define TM_ADJ_MODE_LOCAL_SHIFT 8
|
||||
#define TM_ADJ_MODE_LOCAL_WIDTH 8
|
||||
#define TM_ADJ_MODE_LOCAL_MASK 0x0000FF00U
|
||||
#define TM_ADJ_MODE_LOCAL_NONE 0x00000000U
|
||||
#define TM_ADJ_MODE_LOCAL_ADD 0x00000100U
|
||||
#define TM_ADJ_MODE_LOCAL_SET 0x00000200U
|
||||
|
||||
/**
|
||||
* Register: LOCALTM_ADJ_L
|
||||
*/
|
||||
#define LOCALTM_ADJ_L 0x00000014U
|
||||
#define LOCALTM_ADJ_L_SHIFT 0
|
||||
#define LOCALTM_ADJ_L_WIDTH 32
|
||||
#define LOCALTM_ADJ_L_MASK 0xFFFFFFFFU
|
||||
|
||||
/**
|
||||
* Register: LOCALTM_ADJ_H
|
||||
*/
|
||||
#define LOCALTM_ADJ_H 0x00000018U
|
||||
#define LOCALTM_ADJ_H_SHIFT 0
|
||||
#define LOCALTM_ADJ_H_WIDTH 32
|
||||
#define LOCALTM_ADJ_H_MASK 0xFFFFFFFFU
|
||||
|
||||
/**
|
||||
* Register: CNT_STEP_TOPT
|
||||
*/
|
||||
#define CNT_STEP_OPT 0x0000001CU
|
||||
#define CNT_STEP_OPT_SHIFT 0
|
||||
#define CNT_STEP_OPT_WIDTH 32
|
||||
#define CNT_STEP_OPT_MASK 0xFFFFFFFFU
|
||||
|
||||
/**
|
||||
* Register: SEND_DATA_CMD
|
||||
*/
|
||||
#define SEND_DATA_CMD 0x00000020U
|
||||
#define SEND_DATA_CMD_LEN_SHIFT 0
|
||||
#define SEND_DATA_CMD_LEN_WIDTH 16
|
||||
#define SEND_DATA_CMD_LEN_MASK 0x0000FFFFU
|
||||
#define SEND_DATA_CMD_OFFSET_SHIFT 16
|
||||
#define SEND_DATA_CMD_OFFSET_WIDTH 16
|
||||
#define SEND_DATA_CMD_OFFSET_MASK 0xFFFF0000U
|
||||
|
||||
/**
|
||||
* Register: ANT_CONFIG_CMD
|
||||
*/
|
||||
#define ANT_CONFIG_CMD 0x00000024U
|
||||
#define ANT_CONFIG_CMD_OFFSET_SHIFT 0
|
||||
#define ANT_CONFIG_CMD_OFFSET_WIDTH 16
|
||||
#define ANT_CONFIG_CMD_OFFSET_MASK 0x0000FFFFU
|
||||
#define ANT_CONFIG_CMD_LEN_SHIFT 16
|
||||
#define ANT_CONFIG_CMD_LEN_WIDTH 8
|
||||
#define ANT_CONFIG_CMD_LEN_MASK 0x00FF0000U
|
||||
#define ANT_CONFIG_CMD_NUM_SHIFT 24
|
||||
#define ANT_CONFIG_CMD_NUM_WIDTH 8
|
||||
#define ANT_CONFIG_CMD_NUM_MASK 0xFF000000U
|
||||
|
||||
/**
|
||||
* Register: SLOT_CONFIG_CMD
|
||||
*/
|
||||
#define SLOT_CONFIG_CMD 0x00000028U
|
||||
#define SLOT_CONFIG_CMD_SHIFT 0
|
||||
#define SLOT_CONFIG_CMD_WIDTH 32
|
||||
#define SLOT_CONFIG_CMD_MASK 0xFFFFFFFFU
|
||||
#define SLOT_CONFIG_CMD_VAULE 0x4846414E
|
||||
|
||||
/************************* Ps2Pl Regs PS Read Only ***************************/
|
||||
/**
|
||||
* Register: TXRX_STATE
|
||||
*/
|
||||
#define TXRX_STATE 0x00000100U
|
||||
#define TXRX_STATE_ACQ_SHIFT 0
|
||||
#define TXRX_STATE_ACQ_WIDTH 8
|
||||
#define TXRX_STATE_ACQ_MASK 0x000000FFU
|
||||
#define TXRX_STATE_ACQ_SUCCESS 0x00000001U
|
||||
#define TXRX_STATE_ACQ_FAIL 0x00000002U
|
||||
|
||||
#define TXRX_STATE_FSYNC_SHIFT 8
|
||||
#define TXRX_STATE_FSYNC_WIDTH 8
|
||||
#define TXRX_STATE_FSYNC_MASK 0x0000FF00U
|
||||
#define TXRX_STATE_FSYNC_SUCCESS 0x00000100U
|
||||
#define TXRX_STATE_FSYNC_FAIL 0x00000200U
|
||||
|
||||
#define TXRX_STATE_DECODE_SHIFT 16
|
||||
#define TXRX_STATE_DECODE_WIDTH 8
|
||||
#define TXRX_STATE_DECODE_MASK 0x00FF0000U
|
||||
#define TXRX_STATE_DECODE_SUCCESS 0x00010000U
|
||||
#define TXRX_STATE_DECODE_FAIL 0x00020000U
|
||||
|
||||
/**
|
||||
* Register: RECV_SATE
|
||||
*/
|
||||
#define RECV_DATA_SATE 0x00000104U
|
||||
#define RECV_DATA_LEN_SHIFT 0
|
||||
#define RECV_DATA_LEN_WIDTH 16
|
||||
#define RECV_DATA_LEN_MASK 0x0000FFFFU
|
||||
#define RECV_DATA_OFFSET_SHIFT 16
|
||||
#define RECV_DATA_OFFSET_WIDTH 16
|
||||
#define RECV_DATA_OFFSET_MASK 0xFFFFF0000U
|
||||
|
||||
/**
|
||||
* Register: FSTIME_L
|
||||
*/
|
||||
#define FSTIME_L 0x00000108U
|
||||
#define FSTIME_L_SHIFT 0
|
||||
#define FSTIME_L_WIDTH 32
|
||||
#define FSTIME_L_MASK 0xFFFFFFFFU
|
||||
|
||||
/**
|
||||
* Register: FSTIME_INT1
|
||||
*/
|
||||
#define FSTIME_H 0x0000010CU
|
||||
#define FSTIME_H_SHIFT 0
|
||||
#define FSTIME_H_WIDTH 32
|
||||
#define FSTIME_H_MASK 0xFFFFFFFFU
|
||||
|
||||
/**
|
||||
* Register: TM_TARGET_L
|
||||
*/
|
||||
#define TM_TARGET_L 0x00000110U
|
||||
#define TM_TARGET_L_SHIFT 0
|
||||
#define TM_TARGET_L_WIDTH 32
|
||||
#define TM_TARGET_L_MASK 0xFFFFFFFFU
|
||||
|
||||
/**
|
||||
* Register: TM_TARGET_H
|
||||
*/
|
||||
#define TM_TARGET_H 0x00000114U
|
||||
#define TM_TARGET_H_SHIFT 0
|
||||
#define TM_TARGET_H_WIDTH 32
|
||||
#define TM_TARGET_H_MASK 0xFFFFFFFFU
|
||||
|
||||
/**
|
||||
* Register: LOCALTM_L
|
||||
*/
|
||||
#define TM_LOCAL_L 0x00000118U
|
||||
#define TM_LOCAL_L_SHIFT 0
|
||||
#define TM_LOCAL_L_WIDTH 32
|
||||
#define TM_LOCAL_L_MASK 0xFFFFFFFFU
|
||||
|
||||
/**
|
||||
* Register: LOCALTM_H
|
||||
*/
|
||||
#define TM_LOCAL_H 0x0000011CU
|
||||
#define TM_LOCAL_H_SHIFT 0
|
||||
#define TM_LOCAL_H_WIDTH 32
|
||||
#define TM_LOCAL_H_MASK 0xFFFFFFFFU
|
||||
|
||||
/**
|
||||
* Register: COASEFREQEST
|
||||
*/
|
||||
#define COASE_FREQEST 0x00000120U
|
||||
#define COASE_FREQEST_SHIFT 0
|
||||
#define COASE_FREQEST_WIDTH 32
|
||||
#define COASE_FREQEST_MASK 0xFFFFFFFFU
|
||||
|
||||
/**
|
||||
* Register: INTR_TYPE
|
||||
*/
|
||||
#define INTR_TYPE 0x00000124U
|
||||
|
||||
/**
|
||||
* Register: ANT_STATUS_0
|
||||
*/
|
||||
#define ANT_STATUS 0x00000128U
|
||||
#define ANT_STATUS_CODE_SHIFT 0
|
||||
#define ANT_STATUS_CODE_WIDTH 8
|
||||
#define ANT_STATUS_CODE_MASK 0x000000FFU
|
||||
#define ANT_STATUS_NIL_VOLT_SHIFT 8
|
||||
#define ANT_STATUS_NIL_VOLT_WIDTH 8
|
||||
#define ANT_STATUS_NIL_VOLT_MASK 0x0000FF00U
|
||||
#define ANT_STATUS_RF_VOLT_SHIFT 16
|
||||
#define ANT_STATUS_RF_VOLT_WIDTH 8
|
||||
#define ANT_STATUS_RF_VOLT_MASK 0x00FF0000U
|
||||
#define ANT_STATUS_ANT_VOLT_SHIFT 24
|
||||
#define ANT_STATUS_ANT_VOLT_WIDTH 8
|
||||
#define ANT_STATUS_ANT_VOLT_MASK 0xFF000000U
|
||||
|
||||
/**
|
||||
* Register: ANT_FRAME_CNT
|
||||
*/
|
||||
#define ANT_FRAME_CNT 0x0000012CU
|
||||
#define ANT_FRAME_CNT_SHIFT 0
|
||||
#define ANT_FRAME_CNT_WIDTH 32
|
||||
#define ANT_FRAME_CNT_MASK 0xFFFFFFFFU
|
||||
|
||||
/**
|
||||
* Register: RF_STATUS
|
||||
*/
|
||||
#define RF_STATUS 0x00000130U
|
||||
#define RF_STATUS_TEMP_SHIFT 0
|
||||
#define RF_STATUS_TEMP_WIDTH 8
|
||||
#define RF_STATUS_TEMP_MASK 0x000000FFU
|
||||
#define RF_STATUS_BDLOCK_SHIFT 8
|
||||
#define RF_STATUS_BDLOCK_WIDTH 8
|
||||
#define RF_STATUS_BDLOCK_MASK 0x0000FF00U
|
||||
#define RF_STATUS_PLLA_SHIFT 16
|
||||
#define RF_STATUS_PLLA_WIDTH 8
|
||||
#define RF_STATUS_PLLA_MASK 0x00FF0000U
|
||||
#define RF_STATUS_PLLB_SHIFT 24
|
||||
#define RF_STATUS_PLLB_WIDTH 8
|
||||
#define RF_STATUS_PLLB_MASK 0xFF000000U
|
||||
|
||||
/**
|
||||
* Register: RF_MGC
|
||||
*/
|
||||
#define RF_MGC 0x00000134U
|
||||
#define RF_MGC_CH1_SHIFT 0
|
||||
#define RF_MGC_CH1_WIDTH 4
|
||||
#define RF_MGC_CH1_MASK 0x0000000FU
|
||||
#define RF_MGC_CH2_SHIFT 8
|
||||
#define RF_MGC_CH2_WIDTH 4
|
||||
#define RF_MGC_CH2_MASK 0x00000F00U
|
||||
#define RF_MGC_CH3_SHIFT 16
|
||||
#define RF_MGC_CH3_WIDTH 4
|
||||
#define RF_MGC_CH3_MASK 0x000F0000U
|
||||
#define RF_MGC_CH4_SHIFT 24
|
||||
#define RF_MGC_CH4_WIDTH 4
|
||||
#define RF_MGC_CH4_MASK 0x0F000000U
|
||||
|
||||
/**
|
||||
* Register: RF_FRAME_CNT
|
||||
*/
|
||||
#define RF_FRAME_CNT 0x00000138U
|
||||
#define RF_FRAME_CNT_SHIFT 0
|
||||
#define RF_FRAME_CNT_WIDTH 32
|
||||
#define RF_FRAME_CNT_MASK 0xFFFFFFFFU
|
||||
|
||||
#endif /* PHY_CTRLREG_H_ */
|
||||
Reference in New Issue
Block a user