forked from gatsby/datalink_repo
Initial version.
This commit is contained in:
Gatsby Van
@@ -0,0 +1,132 @@
|
||||
/*****************************************************************************/
|
||||
/**
|
||||
* 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.
|
||||
* dataLen length of specified data.
|
||||
* crcType type of crc
|
||||
* 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.
|
||||
* dataLen length of specified data.
|
||||
* crcPos position of crc insert position
|
||||
* 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.
|
||||
* dataLen length of specified data.
|
||||
* 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,195 @@
|
||||
//
|
||||
// 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,81 @@
|
||||
//
|
||||
// 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,57 @@
|
||||
//
|
||||
// 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,54 @@
|
||||
#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,413 @@
|
||||
//
|
||||
// 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 = 0b110100000000000000000000000000};
|
||||
const SOFT_DOUBLE EPS_DOUBLE = {.raw = 0b11110010110000000000000000000000000000000000000000000000000000};
|
||||
|
||||
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,142 @@
|
||||
/*****************************************************************************/
|
||||
/**
|
||||
* 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, envent) Linklayer_EventHandler(inst, envent)
|
||||
/**************************** 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 **l2InstPtr, L2_CONFIG *config);
|
||||
int32_t Linklayer_Proc(L2_INST *l2InstPtr);
|
||||
int32_t Linklayer_EventHandler(L2_INST *l2InstPtr, L2_EVENT* l2Event);
|
||||
int32_t Linklayer_PktHandler(L2_INST *l2InstPtr);
|
||||
int32_t L2_SteerBeam2Node(L2_INST *l2InstPtr, uint8_t nodeId);
|
||||
int32_t L2_SetupNextSlot(L2_INST *l2InstPtr, L2_TOR_TYPE txrxMode, L2_FRAME_TYPE frameType);
|
||||
|
||||
// Event
|
||||
L2_SCHD_EVENT* L2_CreatNewSchdEvent(L2_INST *l2InstPtr, 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 *l2InstPtr, L2_SCHD_EVENT* schdEvent, uint32_t frameNo, uint32_t slotNo);
|
||||
int32_t L2_ScheduleEventAfter(L2_INST *l2InstPtr, L2_SCHD_EVENT* schdEvent, uint32_t numSlots);
|
||||
int32_t L2_RecheduleEventAt(L2_INST *l2InstPtr, L2_SCHD_EVENT* schdEvent, uint32_t frameNo, uint32_t slotNo);
|
||||
int32_t L2_RecheduleEventAfter(L2_INST *l2InstPtr, L2_SCHD_EVENT* schdEvent, uint32_t numSlots);
|
||||
|
||||
// Framing
|
||||
void L2_FrameStateUpdate(L2_INST *l2InstPtr);
|
||||
L2_FRAME_TYPE L2_GetLastRcvFrameType(L2_INST *l2InstPtr);
|
||||
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 *l2InstPtr);
|
||||
bool L2_isDataAvail(L2_INST *l2InstPtr);
|
||||
bool L2_isCxn2Net(L2_INST *l2InstPtr);
|
||||
int32_t L2_GetFrameLen(L2_FRAME_TYPE l2FrameType);
|
||||
uint32_t L2_GetLastFrameNo(L2_INST *l2InstPtr);
|
||||
uint32_t L2_GetLastSlotNo(L2_INST *l2InstPtr);
|
||||
uint32_t L2_GetCurrentFrameNo(L2_INST *l2InstPtr);
|
||||
uint32_t L2_GetCurrentSlotNo(L2_INST *l2InstPtr);
|
||||
uint32_t L2_GetNextFrameNo(L2_INST *l2InstPtr);
|
||||
uint32_t L2_GetNextSlotNo(L2_INST *l2InstPtr);
|
||||
|
||||
// Scanning
|
||||
int32_t L2_Scan_Init(L2_INST *l2InstPtr);
|
||||
int32_t L2_Scan_PktHandler(L2_INST *l2InstPtr, uint8_t *msg);
|
||||
int32_t L2_Scan_Proc(L2_INST *l2InstPtr);
|
||||
int32_t L2_Scan_Routine(L2_INST *l2InstPtr, L2_RTNE_TABLE_ENTRY *schedEntry);
|
||||
|
||||
// Service
|
||||
int32_t L2_Service_Init(L2_INST *l2InstPtr);
|
||||
int32_t L2_Service_PktHandler(L2_INST *l2InstPtr, uint8_t *msg);
|
||||
int32_t L2_Service_Proc(L2_INST *l2InstPtr);
|
||||
int32_t L2_Service_Routine(L2_INST *l2InstPtr, L2_RTNE_TABLE_ENTRY *schedEntry);
|
||||
|
||||
// Time Sync
|
||||
int32_t L2_TimeSync_LocalTimerAdd(L2_INST *l2InstPtr, int64_t adjustment);
|
||||
int32_t L2_TimeSync_SlotAlign(L2_INST *l2InstPtr);
|
||||
int32_t L2_TimeSync_GetDeviatoion(L2_INST *l2InstPtr, int64_t *devi);
|
||||
|
||||
#endif /* __LINK_LAYER_H_ */
|
||||
@@ -0,0 +1,199 @@
|
||||
/*****************************************************************************/
|
||||
/**
|
||||
* 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,477 @@
|
||||
/*****************************************************************************/
|
||||
/**
|
||||
* 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,64 @@
|
||||
/*****************************************************************************/
|
||||
/**
|
||||
* 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,536 @@
|
||||
/*****************************************************************************/
|
||||
/**
|
||||
* 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,318 @@
|
||||
/*****************************************************************************/
|
||||
/**
|
||||
* 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 *l2InstPtr, 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 *l2InstPtr, L2_FRAME_TYPE type) {
|
||||
ASSERT(l2InstPtr != NULL);
|
||||
int32_t Status;
|
||||
uint8_t *buff;
|
||||
uint32_t packLen;
|
||||
switch (type) {
|
||||
case L2_INFO_FRAME:{
|
||||
packLen = RSP_PACK_LEN;
|
||||
SysMonitor_GetSysinfo(&(l2InstPtr->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(l2InstPtr);
|
||||
buff[L2_MSG_SLOT_NO] = L2_GetNextSlotNo(l2InstPtr);
|
||||
memcpy(buff+L2_MSG_INFO_POSX, &(l2InstPtr->sysInfo.posX), sizeof(float));
|
||||
memcpy(buff+L2_MSG_INFO_POSY, &(l2InstPtr->sysInfo.posY), sizeof(float));
|
||||
memcpy(buff+L2_MSG_INFO_POSZ, &(l2InstPtr->sysInfo.posZ), sizeof(float));
|
||||
memcpy(buff+L2_MSG_INFO_SPEEDX, &(l2InstPtr->sysInfo.vX), sizeof(float));
|
||||
memcpy(buff+L2_MSG_INFO_SPEEDY, &(l2InstPtr->sysInfo.vY), sizeof(float));
|
||||
memcpy(buff+L2_MSG_INFO_SPEEDZ, &(l2InstPtr->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(l2InstPtr->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 l2InstPtr
|
||||
*
|
||||
* @return
|
||||
*
|
||||
******************************************************************************/
|
||||
int32_t L2_Scan_Init(L2_INST *l2InstPtr) {
|
||||
ASSERT(l2InstPtr != NULL);
|
||||
l2InstPtr->scanFsm.sacanCnt = 0;
|
||||
l2InstPtr->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 *l2InstPtr, uint8_t *msg) {
|
||||
ASSERT(l2InstPtr != NULL);
|
||||
ASSERT(msg != NULL);
|
||||
int32_t Status;
|
||||
switch (l2InstPtr->frame.last) {
|
||||
case L2_SCAN_FRAME: {
|
||||
if (l2InstPtr->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(l2InstPtr->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 (l2InstPtr->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(l2InstPtr->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(l2InstPtr->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(l2InstPtr->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(l2InstPtr, &devi);
|
||||
if (Status != SUCCESS) {
|
||||
DBG_ERROR("Failed to compute slot deviation.\n");
|
||||
UpdateNodeTimeDeviation(l2InstPtr->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(l2InstPtr->nodeInfoList, L2_MEMBER_NODE_ID, (uint32_t)deviTrunc);
|
||||
}
|
||||
break;
|
||||
}
|
||||
default:
|
||||
DBG_ERROR("Unsupported frame type: %ld.\n", l2InstPtr->frame.last);
|
||||
Status = EUNEXP;
|
||||
break;
|
||||
}
|
||||
|
||||
// Net setup finished.
|
||||
if(L2_GetMainState(l2InstPtr) == 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(l2InstPtr, &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(l2InstPtr, &l2Event);
|
||||
}
|
||||
return Status;
|
||||
}
|
||||
|
||||
/*****************************************************************************/
|
||||
/**
|
||||
* @brief Execute a member node scan net procedure.
|
||||
*
|
||||
* @param l2InstPtr A pointer to scan net fsm.
|
||||
*
|
||||
* @return used slot (5ms slot).
|
||||
*
|
||||
******************************************************************************/
|
||||
int32_t L2_Scan_Proc(L2_INST *l2InstPtr) {
|
||||
int32_t Status;
|
||||
/* l2InstPtr->nextState == NORMAL_STATE indicates network set up procedure was finished.
|
||||
* Skip 5ms for nodes for node configuration update.
|
||||
*/
|
||||
DBG_INFO("-------- Scan Process --------\n");
|
||||
if (l2InstPtr->nextState == NORMAL_STATE) {
|
||||
DBG_INFO("NETSET DONE, halt 20ms before normal routine.\n");
|
||||
if(l2InstPtr->config.myType != L2_HEAD_NODE) {
|
||||
Phy_SetBeamAngle(l2InstPtr->config.phyInstRef, 0.0, 0.0);
|
||||
Phy_SetBeamWidth(l2InstPtr->config.phyInstRef, PHY_ANT_BEAM_120);
|
||||
Phy_SetMode(l2InstPtr->config.phyInstRef, PHY_MODE_SCHEDULE);
|
||||
L2_SetupNextSlot(l2InstPtr, 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 (l2InstPtr->config.myType == L2_HEAD_NODE) {
|
||||
// Apply a scan procedure
|
||||
if (l2InstPtr->scanFsm.sacanCnt % 2 == 0){
|
||||
// Antenna configure
|
||||
Phy_SetBeamWidth(l2InstPtr->config.phyInstRef, PHY_ANT_BEAM_8);
|
||||
Phy_SetBeamNo(l2InstPtr->config.phyInstRef, l2InstPtr->scanFsm.beamNo);
|
||||
Phy_SetMode(l2InstPtr->config.phyInstRef, PHY_MODE_SCHEDULE);
|
||||
L2_SetupNextSlot(l2InstPtr, L2_TOR_TX, L2_SCAN_FRAME);
|
||||
}
|
||||
else {
|
||||
// Antenna configure
|
||||
Phy_SetBeamWidth(l2InstPtr->config.phyInstRef, PHY_ANT_BEAM_8);
|
||||
Phy_SetBeamNo(l2InstPtr->config.phyInstRef, l2InstPtr->scanFsm.beamNo);
|
||||
Phy_SetMode(l2InstPtr->config.phyInstRef, PHY_MODE_SCHEDULE);
|
||||
L2_SetupNextSlot(l2InstPtr, L2_TOR_RX, L2_INFO_FRAME);
|
||||
// Update Scan beam number
|
||||
l2InstPtr->scanFsm.beamNo = (l2InstPtr->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(l2InstPtr, L2_INFO_FRAME);
|
||||
if (Status != SUCCESS) {
|
||||
DBG_INFO("Failed to pack response.\n");
|
||||
}
|
||||
if (CircQueuePop(l2InstPtr->txQueue, (void **)(&sendData)) == true) {
|
||||
memcpy(l2InstPtr->sendPktData, sendData, RSP_PACK_LEN);
|
||||
free(sendData);
|
||||
}
|
||||
else {
|
||||
DBG_INFO("Failed to pop tx Queue.\n");
|
||||
}
|
||||
// Antenna configure
|
||||
Phy_SetBeamAngle(l2InstPtr->config.phyInstRef, 0.0, 0.0);
|
||||
Phy_SetBeamWidth(l2InstPtr->config.phyInstRef, PHY_ANT_BEAM_120);
|
||||
Phy_SetMode(l2InstPtr->config.phyInstRef, PHY_MODE_LISTEN);
|
||||
L2_SetupNextSlot(l2InstPtr, L2_TOR_RX, L2_SCAN_FRAME);
|
||||
}
|
||||
|
||||
// Increase scan counter (for debug purpose).
|
||||
l2InstPtr->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 *l2InstPtr, L2_RTNE_TABLE_ENTRY *schedEntry) {
|
||||
DBG_INFO("-------- Scan Routine --------\n");
|
||||
ASSERT(false); // Not support.
|
||||
return 0;
|
||||
}
|
||||
@@ -0,0 +1,464 @@
|
||||
/*****************************************************************************/
|
||||
/**
|
||||
* 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 *l2InstPtr, 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 *l2InstPtr, L2_FRAME_TYPE type) {
|
||||
#ifndef DATALINK_ENABLE_BERTEST
|
||||
ASSERT(l2InstPtr != 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(l2InstPtr);
|
||||
buff[L2_MSG_SLOT_NO] = L2_GetNextSlotNo(l2InstPtr);
|
||||
|
||||
// TODO: Check Pack TimeSync info, and better approch?.
|
||||
nodeEntry = GetNode(l2InstPtr->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] = l2InstPtr->serviceFsm.txSeqNum;
|
||||
|
||||
// Pack payload
|
||||
memcpy(buff+L2_MSG_SVC_PAYLOAD,
|
||||
l2InstPtr->serviceFsm.txLog + L2_MSG_SVC_PAYLOAD_LEN*l2InstPtr->serviceFsm.txSeqNum,
|
||||
L2_MSG_SVC_PAYLOAD_LEN);
|
||||
if (l2InstPtr->config.myType == L2_HEAD_NODE) {
|
||||
// if last packet reset counting
|
||||
l2InstPtr->serviceFsm.txSeqNum = (l2InstPtr->serviceFsm.txSeqNum
|
||||
>= (PAYLOAD_FLJ-1)/L2_MSG_SVC_PAYLOAD+1)
|
||||
? 0 : l2InstPtr->serviceFsm.txSeqNum + 1;
|
||||
}
|
||||
else{
|
||||
// if last packet reset counting
|
||||
l2InstPtr->serviceFsm.txSeqNum = (l2InstPtr->serviceFsm.txSeqNum
|
||||
>= (PAYLOAD_LJ-1)/L2_MSG_SVC_PAYLOAD+1)
|
||||
? 0 : l2InstPtr->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(l2InstPtr->txQueue, buff) == false){
|
||||
DBG_ERROR("Cannot push packet to queue..\n");
|
||||
return EQUEUE;
|
||||
}
|
||||
break;
|
||||
}
|
||||
case L2_INFO_FRAME:{
|
||||
packLen = RSP_PACK_LEN;
|
||||
SysMonitor_GetSysinfo(&(l2InstPtr->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(l2InstPtr);
|
||||
buff[L2_MSG_SLOT_NO] = L2_GetNextSlotNo(l2InstPtr);
|
||||
memcpy(buff+L2_MSG_INFO_POSX, &(l2InstPtr->sysInfo.posX), sizeof(float));
|
||||
memcpy(buff+L2_MSG_INFO_POSY, &(l2InstPtr->sysInfo.posY), sizeof(float));
|
||||
memcpy(buff+L2_MSG_INFO_POSZ, &(l2InstPtr->sysInfo.posZ), sizeof(float));
|
||||
memcpy(buff+L2_MSG_INFO_SPEEDX, &(l2InstPtr->sysInfo.vX), sizeof(float));
|
||||
memcpy(buff+L2_MSG_INFO_SPEEDY, &(l2InstPtr->sysInfo.vY), sizeof(float));
|
||||
memcpy(buff+L2_MSG_INFO_SPEEDZ, &(l2InstPtr->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(l2InstPtr->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 l2InstPtr
|
||||
*
|
||||
* @return
|
||||
*
|
||||
******************************************************************************/
|
||||
int32_t L2_Service_Init(L2_INST *l2InstPtr) {
|
||||
ASSERT(l2InstPtr != NULL);
|
||||
l2InstPtr->serviceFsm.txSeqNum = 0;
|
||||
l2InstPtr->serviceFsm.rxSeqNum = 0;
|
||||
memset((&l2InstPtr->serviceFsm.rxLog), 0x00, sizeof(PAYLOAD));
|
||||
memset((&l2InstPtr->serviceFsm.txLog), 0x00, sizeof(PAYLOAD));
|
||||
l2InstPtr->serviceFsm.pktRecv = 0;
|
||||
l2InstPtr->serviceFsm.pktRecved = 0;
|
||||
#ifdef DATALINK_ENABLE_BERTEST
|
||||
l2InstPtr->serviceFsm.bitTotal = 0;
|
||||
l2InstPtr->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 *l2InstPtr, uint8_t *msg) {
|
||||
ASSERT(l2InstPtr != NULL);
|
||||
ASSERT(msg != NULL);
|
||||
int32_t Status;
|
||||
switch (l2InstPtr->frame.last) {
|
||||
case L2_SVC_FRAME: {
|
||||
if (l2InstPtr->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(l2InstPtr, &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 || l2InstPtr->serviceFsm.seqNum != seqNum) {
|
||||
AssertL2Event(l2InstPtr, &l2Event);
|
||||
}
|
||||
#endif
|
||||
#ifndef DATALINK_ENABLE_BERTEST
|
||||
memcpy(l2InstPtr->serviceFsm.rxLog + L2_MSG_SVC_PAYLOAD_LEN*seqNum, msg + L2_MSG_SVC_PAYLOAD, L2_MSG_SVC_PAYLOAD_LEN);
|
||||
if (l2InstPtr->config.myType == L2_HEAD_NODE) {
|
||||
// if last packet reset counting
|
||||
if(seqNum >= (PAYLOAD_FLJ-1)/L2_MSG_SVC_PAYLOAD+1) {
|
||||
if(seqNum == l2InstPtr->serviceFsm.rxSeqNum) {
|
||||
SysMonitor_UpdateRecvPayloadLJRaw(l2InstPtr->serviceFsm.rxLog);
|
||||
}
|
||||
l2InstPtr->serviceFsm.rxSeqNum = 0;
|
||||
}
|
||||
else {
|
||||
if(l2InstPtr->serviceFsm.rxSeqNum == seqNum){
|
||||
l2InstPtr->serviceFsm.rxSeqNum++;
|
||||
}
|
||||
}
|
||||
}
|
||||
else{
|
||||
// if last packet reset counting
|
||||
if(seqNum >= (PAYLOAD_LJ-1)/L2_MSG_SVC_PAYLOAD+1) {
|
||||
if(seqNum == l2InstPtr->serviceFsm.rxSeqNum) {
|
||||
SysMonitor_UpdateRecvPayloadFLJRaw(l2InstPtr->serviceFsm.rxLog);
|
||||
}
|
||||
l2InstPtr->serviceFsm.rxSeqNum = 0;
|
||||
}
|
||||
else {
|
||||
if(l2InstPtr->serviceFsm.rxSeqNum == seqNum){
|
||||
l2InstPtr->serviceFsm.rxSeqNum++;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
#else
|
||||
for(int i = 0; i < SVC_PACK_LEN; i++) {
|
||||
l2InstPtr->serviceFsm.bitError += count_ones(msg[i]^berTestData[i]);
|
||||
}
|
||||
l2InstPtr->serviceFsm.bitTotal += SVC_PACK_LEN*8;
|
||||
#endif
|
||||
l2InstPtr->serviceFsm.pktRecved++;
|
||||
// Reset Node TTL
|
||||
Status = RstNodeTTL(l2InstPtr->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 (l2InstPtr->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(l2InstPtr->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(l2InstPtr->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(l2InstPtr, &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(l2InstPtr->nodeInfoList, L2_MEMBER_NODE_ID, (uint32_t)deviTrunc);
|
||||
}
|
||||
Status = UpdateNodeTimeDeviation(l2InstPtr->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++) {
|
||||
l2InstPtr->serviceFsm.bitError += count_ones(msg[i]^berTestData[i]);
|
||||
}
|
||||
l2InstPtr->serviceFsm.bitTotal += SVC_PACK_LEN*8;
|
||||
#endif
|
||||
l2InstPtr->serviceFsm.pktRecved++;
|
||||
// Reset Node TTL
|
||||
Status = RstNodeTTL(l2InstPtr->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", l2InstPtr->frame.last);
|
||||
break;
|
||||
}
|
||||
return Status;
|
||||
}
|
||||
|
||||
/*****************************************************************************/
|
||||
/**
|
||||
* @brief Service FSM Routine.
|
||||
*
|
||||
* @param l2InstPtr
|
||||
* @param schedEntry
|
||||
*
|
||||
* @return Slot has been used.
|
||||
*
|
||||
******************************************************************************/
|
||||
int32_t L2_Service_Routine(L2_INST *l2InstPtr, L2_RTNE_TABLE_ENTRY *schedEntry) {
|
||||
ASSERT(l2InstPtr != NULL);
|
||||
DBG_INFO("-------- ServiceNet Routine --------\n");
|
||||
uint8_t *sendData;
|
||||
int32_t usedSlots;
|
||||
L2_TOR_TYPE torState;
|
||||
if(schedEntry->src == l2InstPtr->config.myId){
|
||||
torState = L2_TOR_TX;
|
||||
}
|
||||
else if(schedEntry->dst == l2InstPtr->config.myId){
|
||||
torState = L2_TOR_RX;
|
||||
l2InstPtr->serviceFsm.pktRecv++;
|
||||
}
|
||||
else{
|
||||
torState = L2_TOR_SUSPEND;
|
||||
}
|
||||
|
||||
switch(schedEntry->slotType){
|
||||
case L2_SLOT_SERVICE:
|
||||
if(torState == L2_TOR_TX){
|
||||
#ifndef DATALINK_ENABLE_BERTEST
|
||||
if (l2InstPtr->serviceFsm.txSeqNum == 0) {
|
||||
if(l2InstPtr->config.myType == L2_HEAD_NODE){
|
||||
SysMonitor_GetSendPayloadFLJRaw(l2InstPtr->serviceFsm.txLog);
|
||||
}
|
||||
else {
|
||||
SysMonitor_GetSendPayloadLJRaw(l2InstPtr->serviceFsm.txLog);
|
||||
}
|
||||
}
|
||||
// Packing service data
|
||||
Service_Packing(l2InstPtr, L2_SVC_FRAME);
|
||||
if (CircQueuePop(l2InstPtr->txQueue, (void **)(&sendData)) == true) {
|
||||
memcpy(l2InstPtr->sendPktData, sendData, SVC_PACK_LEN);
|
||||
free(sendData);
|
||||
}
|
||||
else {
|
||||
DBG_ERROR("Failed to pop tx queue.\n");
|
||||
}
|
||||
#else
|
||||
memcpy(l2InstPtr->config.phyInstRef->sendFrameBuff, berTestData, SVC_PACK_LEN);
|
||||
#endif
|
||||
}
|
||||
// FIXME: Steer antenna to point to destination.
|
||||
Phy_SetBeamAngle(l2InstPtr->config.phyInstRef, 0.0, 0.0);
|
||||
Phy_SetBeamWidth(l2InstPtr->config.phyInstRef, PHY_ANT_BEAM_8);
|
||||
Phy_SetMode(l2InstPtr->config.phyInstRef, PHY_MODE_SCHEDULE);
|
||||
L2_SetupNextSlot(l2InstPtr, 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(l2InstPtr, L2_INFO_FRAME);
|
||||
if (CircQueuePop(l2InstPtr->txQueue, (void **)(&sendData)) == true) {
|
||||
memcpy(l2InstPtr->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(l2InstPtr->config.phyInstRef, 0.0, 0.0);
|
||||
Phy_SetBeamWidth(l2InstPtr->config.phyInstRef, PHY_ANT_BEAM_120);
|
||||
Phy_SetMode(l2InstPtr->config.phyInstRef, PHY_MODE_SCHEDULE);
|
||||
L2_SetupNextSlot(l2InstPtr, 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 = l2InstPtr->serviceFsm.pktRecv - l2InstPtr->serviceFsm.pktRecved;
|
||||
lossRate = (l2InstPtr->serviceFsm.pktRecv == 0) ? 0 : (double)pktLoss / (double)l2InstPtr->serviceFsm.pktRecv;
|
||||
errorRate = (l2InstPtr->serviceFsm.bitTotal == 0) ? 0 : (double)l2InstPtr->serviceFsm.bitError / (double)l2InstPtr->serviceFsm.bitTotal;
|
||||
DBG_PRINT("Packet Loss Rate -- Total: %lu, Loss: %lu.\n", l2InstPtr->serviceFsm.pktRecv, pktLoss);
|
||||
DBG_PRINT("Bit Error Rate -- Total:%lu, Error:%lu.\n", l2InstPtr->serviceFsm.bitTotal, l2InstPtr->serviceFsm.bitError);
|
||||
#endif
|
||||
return usedSlots;
|
||||
}
|
||||
@@ -0,0 +1,123 @@
|
||||
/*****************************************************************************/
|
||||
/**
|
||||
* 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 l2InstPtr.
|
||||
* @param adjustment
|
||||
*
|
||||
* @return
|
||||
*
|
||||
******************************************************************************/
|
||||
inline int32_t L2_TimeSync_LocalTimerAdd(L2_INST *l2InstPtr, int64_t adjustment) {
|
||||
// read nextSendTime from physical layer
|
||||
Phy_SetTimeWord(l2InstPtr->config.phyInstRef, adjustment, PHY_TIME_WORD_LOCAL_ADJ);
|
||||
PhyWrite(l2InstPtr->config.phyInstRef, TM_ADJ_MODE, TM_ADJ_MODE_ADD);
|
||||
return SUCCESS;
|
||||
}
|
||||
|
||||
/*****************************************************************************/
|
||||
/**
|
||||
* @brief Align slot, base on physical layer information.
|
||||
*
|
||||
* @param l2InstPtr.
|
||||
*
|
||||
* @return
|
||||
*
|
||||
******************************************************************************/
|
||||
int32_t L2_TimeSync_SlotAlign(L2_INST *l2InstPtr) {
|
||||
int32_t Status;
|
||||
int64_t slotAdj;
|
||||
Status = L2_TimeSync_GetDeviatoion(l2InstPtr, &slotAdj);
|
||||
if (Status != SUCCESS) {
|
||||
DBG_ERROR("Failed to compute slot deviation.\n");
|
||||
return Status;
|
||||
}
|
||||
return L2_TimeSync_LocalTimerAdd(l2InstPtr, slotAdj);
|
||||
}
|
||||
|
||||
/*****************************************************************************/
|
||||
/**
|
||||
* @brief Compute slot deviation of received frame.
|
||||
*
|
||||
* @param l2InstPtr.
|
||||
* @param devi
|
||||
*
|
||||
* @return
|
||||
*
|
||||
******************************************************************************/
|
||||
int32_t L2_TimeSync_GetDeviatoion(L2_INST *l2InstPtr, int64_t *devi) {
|
||||
// read nextSendTime from physical layer
|
||||
int64_t currentTarget, lastFrameSync, slotMeasure;
|
||||
currentTarget = PhyRead(l2InstPtr->config.phyInstRef, TM_TARGET_L);
|
||||
currentTarget |= ((int64_t)PhyRead(l2InstPtr->config.phyInstRef, TM_TARGET_H)) << 32;
|
||||
lastFrameSync = PhyRead(l2InstPtr->config.phyInstRef, FSTIME_L);
|
||||
lastFrameSync |= ((int64_t)PhyRead(l2InstPtr->config.phyInstRef, FSTIME_H)) << 32;
|
||||
slotMeasure = (currentTarget > lastFrameSync) ? (currentTarget - lastFrameSync)
|
||||
: (currentTarget - lastFrameSync + PHY_LOCAL_CNT_MAX);
|
||||
switch (Phy_GetLastDetectedFrameType(l2InstPtr->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(l2InstPtr->config.phyInstRef));
|
||||
return EIGNORE;
|
||||
}
|
||||
return SUCCESS;
|
||||
}
|
||||
|
||||
|
||||
Reference in New Issue
Block a user