forked from 0xha0/df_datalink
mitigate: 整个地检和节点工程, 增加自动编译
更新了AD9361的profile
This commit is contained in:
@@ -0,0 +1,189 @@
|
||||
/*****************************************************************************/
|
||||
/**
|
||||
* @file framing.h
|
||||
* @author FanHao ([email protected])
|
||||
* @brief DF Linklayer framing functions.
|
||||
* @version 0.0
|
||||
* @date 2023-05-06
|
||||
*
|
||||
* @copyright Copyright(c) 2024 Beijing Institute of Technology.\n
|
||||
* Lab of Communication and Network
|
||||
*
|
||||
******************************************************************************/
|
||||
|
||||
/**
|
||||
* @addtogroup LinkLayer
|
||||
* @{
|
||||
*/
|
||||
|
||||
/**
|
||||
* @addtogroup LinkLayer_Framing
|
||||
* @{
|
||||
*/
|
||||
/***************************** Include Files *********************************/
|
||||
#include <stdint.h>
|
||||
#include <string.h>
|
||||
#include "error.h"
|
||||
#include "common.h"
|
||||
#include "msg_def.h"
|
||||
#include "link_layer.h"
|
||||
|
||||
/************************** Constant Definitions *****************************/
|
||||
|
||||
/*****************************************************************************/
|
||||
/**
|
||||
* @brief Insert a data chunk to a bit array.
|
||||
*
|
||||
* @param frame pointer to target bit array.
|
||||
* @param offset offset of insert start position.
|
||||
* @param bytes number of insert bytes
|
||||
* @param chunk pointer to data chunk
|
||||
*
|
||||
* @return NONE
|
||||
*
|
||||
******************************************************************************/
|
||||
void L2_FrameInsertChunk(uint8_t *frame, size_t offset, size_t bytes, const void *chunk)
|
||||
{
|
||||
ASSERT(frame != NULL);
|
||||
ASSERT(chunk != NULL);
|
||||
memcpy(frame + offset, chunk, bytes);
|
||||
}
|
||||
|
||||
/*****************************************************************************/
|
||||
/**
|
||||
* @brief Get a data chunk to a frame buff.
|
||||
*
|
||||
* @param frame pointer to target bit array.
|
||||
* @param offset offset of data chunk start position.
|
||||
* @param bytes number of data bytes
|
||||
* @param chunk pointer to data chunk
|
||||
*
|
||||
* @return NONE
|
||||
*
|
||||
******************************************************************************/
|
||||
void L2_FrameGetChunk(uint8_t *frame, size_t offset, size_t bytes, void *chunk)
|
||||
{
|
||||
ASSERT(frame != NULL);
|
||||
ASSERT(chunk != NULL);
|
||||
memcpy(chunk, frame + offset, bytes);
|
||||
}
|
||||
|
||||
/*****************************************************************************/
|
||||
/**
|
||||
* @brief Generate crc of specified data.
|
||||
*
|
||||
* @param dataBuff pointer to data buffer.
|
||||
* @param dataLen length of specified data.
|
||||
* @param crcType type of crc
|
||||
* @param crc a pointer to 32 bit crc container.
|
||||
*
|
||||
* @return FAILURE crc type not support.
|
||||
* SUCCESS
|
||||
*
|
||||
******************************************************************************/
|
||||
int32_t L2_CrcGen(uint8_t *dataBuff, uint32_t dataLen, L2_CRC_TYPE crcType, uint32_t *crc)
|
||||
{
|
||||
ASSERT(dataBuff != NULL);
|
||||
int32_t Status;
|
||||
switch (crcType)
|
||||
{
|
||||
case L2_CRC16_1021:
|
||||
Status = crc16_gen(dataBuff, dataLen, 0x1021, (uint16_t *)crc);
|
||||
break;
|
||||
case L2_CRC16_8005:
|
||||
Status = crc16_gen(dataBuff, dataLen, 0x8005, (uint16_t *)crc);
|
||||
break;
|
||||
case L2_CRC8_31:
|
||||
Status = crc8_gen(dataBuff, dataLen, 0x31, (uint8_t *)crc);
|
||||
break;
|
||||
default:
|
||||
Status = EPARAM;
|
||||
break;
|
||||
}
|
||||
return Status;
|
||||
}
|
||||
|
||||
/*****************************************************************************/
|
||||
/**
|
||||
* @brief Append a crc of specified data, to a specified position.
|
||||
*
|
||||
* @param dataBuff pointer to data buffer.
|
||||
* @param dataLen length of specified data.
|
||||
* @param crcPos position of crc insert position
|
||||
* @param crcType type of crc
|
||||
*
|
||||
* @return FAILURE crc type not support.
|
||||
* SUCCESS
|
||||
*
|
||||
******************************************************************************/
|
||||
int32_t L2_CrcAppend(uint8_t *dataBuff, uint32_t dataLen, uint32_t crcPos, L2_CRC_TYPE crcType)
|
||||
{
|
||||
ASSERT(dataBuff != NULL);
|
||||
int32_t Status;
|
||||
uint32_t crcVal;
|
||||
uint8_t *crcPtr;
|
||||
crcPtr = dataBuff + crcPos;
|
||||
Status = L2_CrcGen(dataBuff, dataLen, crcType, &crcVal);
|
||||
if (Status != SUCCESS)
|
||||
{
|
||||
return Status;
|
||||
}
|
||||
switch (crcType)
|
||||
{
|
||||
case L2_CRC16_1021:
|
||||
*(crcPtr + 1) = (uint8_t)(crcVal >> 8);
|
||||
*crcPtr = (uint8_t)(crcVal);
|
||||
break;
|
||||
case L2_CRC16_8005:
|
||||
*(crcPtr + 1) = (uint8_t)(crcVal >> 8);
|
||||
*crcPtr = (uint8_t)(crcVal);
|
||||
break;
|
||||
case L2_CRC8_31:
|
||||
*crcPtr = (uint8_t)crcVal;
|
||||
break;
|
||||
default:
|
||||
Status = EPARAM;
|
||||
break;
|
||||
}
|
||||
return Status;
|
||||
}
|
||||
|
||||
/*****************************************************************************/
|
||||
/**
|
||||
* @brief Check data validation by crc.
|
||||
*
|
||||
* @param dataBuff pointer to data buffer.
|
||||
* @param dataLen length of specified data.
|
||||
* @param crcType type of crc
|
||||
*
|
||||
* @return FAILURE crc type not support.
|
||||
* SUCCESS
|
||||
*
|
||||
******************************************************************************/
|
||||
bool L2_CrcCheck(uint8_t *dataBuff, uint32_t dataLen, L2_CRC_TYPE crcType)
|
||||
{
|
||||
ASSERT(dataBuff != NULL);
|
||||
int32_t Status;
|
||||
uint32_t crcVal;
|
||||
crcVal = 0U;
|
||||
Status = L2_CrcGen(dataBuff, dataLen, crcType, &crcVal);
|
||||
if (Status == SUCCESS)
|
||||
{
|
||||
if (crcVal == 0)
|
||||
{
|
||||
return true;
|
||||
}
|
||||
else
|
||||
{
|
||||
return false;
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
/** @} LinkLayer_Framing*/
|
||||
|
||||
/** @} LinkLayer*/
|
||||
@@ -0,0 +1,244 @@
|
||||
/*****************************************************************************/
|
||||
/**
|
||||
* @file coord.c
|
||||
* @author FanHao ([email protected])
|
||||
* @version 0.0
|
||||
* @date 2023-05-06
|
||||
*
|
||||
* @copyright Copyright(c) 2024 Beijing Institute of Technology.\n
|
||||
* Lab of Communication and Network
|
||||
*
|
||||
******************************************************************************/
|
||||
/**
|
||||
* @addtogroup LinkLayer
|
||||
* @{
|
||||
*/
|
||||
|
||||
/**
|
||||
* @addtogroup LinkLayerGeometry
|
||||
* @{
|
||||
*/
|
||||
|
||||
/**
|
||||
* @addtogroup LinkLayerGeometry_Coord
|
||||
* @{
|
||||
*/
|
||||
|
||||
#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)
|
||||
{
|
||||
if (t == 0.0) {
|
||||
*a = COORD_NIL;
|
||||
return;
|
||||
}
|
||||
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)
|
||||
{
|
||||
if (t == 0.0) {
|
||||
return COORD_NIL;
|
||||
}
|
||||
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;
|
||||
}
|
||||
|
||||
/** @} LinkLayerGeometry_Coord */
|
||||
|
||||
/** @} LinkLayerGeometry */
|
||||
|
||||
/** @} LinkLayer */
|
||||
@@ -0,0 +1,102 @@
|
||||
/*****************************************************************************/
|
||||
/**
|
||||
* @file coord.h
|
||||
* @author FanHao ([email protected])
|
||||
* @version 0.0
|
||||
* @date 2023-05-06
|
||||
*
|
||||
* @copyright Copyright(c) 2024 Beijing Institute of Technology.\n
|
||||
* Lab of Communication and Network
|
||||
*
|
||||
******************************************************************************/
|
||||
|
||||
#ifndef _COORD_H
|
||||
#define _COORD_H
|
||||
/**
|
||||
* @addtogroup LinkLayer
|
||||
* @{
|
||||
*/
|
||||
|
||||
/**
|
||||
* @defgroup LinkLayerGeometry
|
||||
* @{
|
||||
*/
|
||||
|
||||
/**
|
||||
* @defgroup LinkLayerGeometry_Coord
|
||||
* @{
|
||||
*/
|
||||
|
||||
/**
|
||||
* @struct COORD
|
||||
* @brief Structure for storing 3D coordinates.
|
||||
*/
|
||||
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
|
||||
|
||||
/** @} LinkLayerGeometry_Coord */
|
||||
|
||||
/** @} LinkLayerGeometry */
|
||||
|
||||
/** @} LinkLayer */
|
||||
@@ -0,0 +1,88 @@
|
||||
/*****************************************************************************/
|
||||
/**
|
||||
* @file eulerangles.c
|
||||
* @author FanHao ([email protected])
|
||||
* @version 0.0
|
||||
* @date 2023-05-06
|
||||
*
|
||||
* @copyright Copyright(c) 2024 Beijing Institute of Technology.\n
|
||||
* Lab of Communication and Network
|
||||
*
|
||||
******************************************************************************/
|
||||
|
||||
/**
|
||||
* @addtogroup LinkLayer
|
||||
* @{
|
||||
*/
|
||||
|
||||
/**
|
||||
* @addtogroup LinkLayerGeometry
|
||||
* @{
|
||||
*/
|
||||
|
||||
/**
|
||||
* @addtogroup LinkLayerGeometry_EulerAngles
|
||||
* @{
|
||||
*/
|
||||
|
||||
#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.0 * EUL_PI);
|
||||
e->beta = fmod(e->beta, 2.0 * EUL_PI);
|
||||
e->gamma = fmod(e->gamma, 2.0 * EUL_PI);
|
||||
}
|
||||
|
||||
/** @brief Normalize EulerAngles. */
|
||||
EULERANGLES EulerAngles_GetNormalize(EULERANGLES e)
|
||||
{
|
||||
EulerAngles_Normalize(&e);
|
||||
return e;
|
||||
}
|
||||
|
||||
/** @} LinkLayerGeometry_EulerAngles */
|
||||
|
||||
/** @} LinkLayerGeometry */
|
||||
|
||||
/** @} LinkLayer */
|
||||
@@ -0,0 +1,93 @@
|
||||
/*****************************************************************************/
|
||||
/**
|
||||
* @file eulerangles.h
|
||||
* @author FanHao ([email protected])
|
||||
* @version 0.0
|
||||
* @date 2023-05-06
|
||||
*
|
||||
* @copyright Copyright(c) 2024 Beijing Institute of Technology.\n
|
||||
* Lab of Communication and Network
|
||||
*
|
||||
******************************************************************************/
|
||||
|
||||
/**
|
||||
* @addtogroup LinkLayer
|
||||
* @{
|
||||
*/
|
||||
|
||||
/**
|
||||
* @addtogroup LinkLayerGeometry
|
||||
* @{
|
||||
*/
|
||||
|
||||
/**
|
||||
* @defgroup LinkLayerGeometry_EulerAngles
|
||||
* @{
|
||||
*/
|
||||
|
||||
#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).
|
||||
*/
|
||||
|
||||
/**
|
||||
* @struct EULERANGLES
|
||||
* @brief Structure for standard quaternion
|
||||
*/
|
||||
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
|
||||
|
||||
/** @} LinkLayerGeometry_EulerAngles */
|
||||
|
||||
/** @} LinkLayerGeometry */
|
||||
|
||||
/** @} LinkLayer */
|
||||
@@ -0,0 +1,499 @@
|
||||
/*****************************************************************************/
|
||||
/**
|
||||
* @file quaternion.c
|
||||
* @author FanHao ([email protected])
|
||||
* @version 0.0
|
||||
* @date 2023-05-06
|
||||
*
|
||||
* @copyright Copyright(c) 2024 Beijing Institute of Technology.\n
|
||||
* Lab of Communication and Network
|
||||
*
|
||||
******************************************************************************/
|
||||
|
||||
/**
|
||||
* @addtogroup LinkLayer
|
||||
* @{
|
||||
*/
|
||||
|
||||
/**
|
||||
* @addtogroup LinkLayerGeometry
|
||||
* @{
|
||||
*/
|
||||
|
||||
/**
|
||||
* @addtogroup LinkLayerGeometry_Quaternion
|
||||
* @{
|
||||
*/
|
||||
|
||||
#include <math.h>
|
||||
#include <stdint.h>
|
||||
#include <stdbool.h>
|
||||
#include "coord.h"
|
||||
#include "eulerangles.h"
|
||||
#include "quaternion.h"
|
||||
|
||||
typedef union
|
||||
{
|
||||
float value;
|
||||
uint32_t raw;
|
||||
} SOFT_FLOAT;
|
||||
|
||||
typedef union
|
||||
{
|
||||
double value;
|
||||
uint64_t raw;
|
||||
} SOFT_DOUBLE;
|
||||
|
||||
const SOFT_FLOAT EPS_FLOAT = {.raw = 0x34000000};
|
||||
const SOFT_DOUBLE EPS_DOUBLE = {.raw = 0x3CB0000000000000};
|
||||
|
||||
const QUATERNION QUATERNION_IDENTITY = {1.0, 0.0, 0.0, 0.0};
|
||||
const QUATERNION QUATERNION_NIL = {NAN, NAN, NAN, NAN};
|
||||
|
||||
/** @brief Subtracts two quaternions. */
|
||||
QUATERNION Quaternion_Sub(QUATERNION q1, QUATERNION q2)
|
||||
{
|
||||
QUATERNION ret;
|
||||
ret.s = q1.s - q2.s;
|
||||
ret.i = q1.i - q2.i;
|
||||
ret.j = q1.j - q2.j;
|
||||
ret.k = q1.k - q2.k;
|
||||
return ret;
|
||||
}
|
||||
|
||||
/** @brief Adds two quaternions. */
|
||||
QUATERNION Quaternion_Add(QUATERNION q1, QUATERNION q2)
|
||||
{
|
||||
QUATERNION ret;
|
||||
ret.s = q1.s + q2.s;
|
||||
ret.i = q1.i + q2.i;
|
||||
ret.j = q1.j + q2.j;
|
||||
ret.k = q1.k + q2.k;
|
||||
return ret;
|
||||
}
|
||||
|
||||
/** @brief Multiplies a quaternion by a real number. */
|
||||
void Quaternion_ScalarMult(QUATERNION *q, double t)
|
||||
{
|
||||
q->s *= t;
|
||||
q->i *= t;
|
||||
q->j *= t;
|
||||
q->k *= t;
|
||||
}
|
||||
|
||||
/** @brief Multiplies a quaternion by a real number. */
|
||||
QUATERNION Quaternion_GetScalarMult(QUATERNION a, double t)
|
||||
{
|
||||
Quaternion_ScalarMult(&a, t);
|
||||
return a;
|
||||
}
|
||||
|
||||
/** @brief Divides a quaternion by a real number. */
|
||||
void Quaternion_ScalarDiv(QUATERNION *q, double t)
|
||||
{
|
||||
if (t == 0.0) {
|
||||
*q = QUATERNION_NIL;
|
||||
return;
|
||||
}
|
||||
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)
|
||||
{
|
||||
if ((q.s > 1.0) || (q.s < -1.0)) {
|
||||
return COORD_NIL;
|
||||
}
|
||||
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)
|
||||
{
|
||||
if ((q.s > 1.0) || (q.s < -1.0)) {
|
||||
*axis = COORD_NIL;
|
||||
*angle = NAN;
|
||||
return;
|
||||
}
|
||||
|
||||
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)
|
||||
{
|
||||
if ((swing == NULL) || (twist == NULL))
|
||||
{
|
||||
return;
|
||||
}
|
||||
|
||||
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.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;
|
||||
break;
|
||||
}
|
||||
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.0 - 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);
|
||||
}
|
||||
}
|
||||
|
||||
/** @} LinkLayerGeometry_Quaternion */
|
||||
|
||||
/** @} LinkLayerGeometry */
|
||||
|
||||
/** @} LinkLayer */
|
||||
@@ -0,0 +1,144 @@
|
||||
/*****************************************************************************/
|
||||
/**
|
||||
* @file quaternion.h
|
||||
* @author FanHao ([email protected])
|
||||
* @version 0.0
|
||||
* @date 2023-05-06
|
||||
*
|
||||
* @copyright Copyright(c) 2024 Beijing Institute of Technology.\n
|
||||
* Lab of Communication and Network
|
||||
*
|
||||
******************************************************************************/
|
||||
|
||||
/**
|
||||
* @addtogroup LinkLayer
|
||||
* @{
|
||||
*/
|
||||
|
||||
/**
|
||||
* @addtogroup LinkLayerGeometry
|
||||
* @{
|
||||
*/
|
||||
|
||||
/**
|
||||
* @defgroup LinkLayerGeometry_Quaternion
|
||||
* @{
|
||||
*/
|
||||
|
||||
#ifndef __QUATERNION_H
|
||||
#define __QUATERNION_H
|
||||
|
||||
#include "coord.h"
|
||||
#include "eulerangles.h"
|
||||
|
||||
/**
|
||||
* @struct QUATERNION
|
||||
* @brief Structure for standard quaternion
|
||||
*/
|
||||
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 q, 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 q, 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
|
||||
|
||||
/** @} LinkLayerGeometry_Quaternion */
|
||||
|
||||
/** @} LinkLayerGeometry */
|
||||
|
||||
/** @} LinkLayer */
|
||||
File diff suppressed because it is too large
Load Diff
@@ -0,0 +1,231 @@
|
||||
/*****************************************************************************/
|
||||
/**
|
||||
* @file link_layer.h
|
||||
* @author FanHao ([email protected])
|
||||
* @brief DF Linklayer header file.
|
||||
* @version 0.1
|
||||
* @date 2023-05-06
|
||||
*
|
||||
* @copyright Copyright(c) 2024 Beijing Institute of Technology.\n
|
||||
* Lab of Communication and Network
|
||||
*
|
||||
******************************************************************************/
|
||||
|
||||
/**
|
||||
* @defgroup LinkLayer
|
||||
* @{
|
||||
*/
|
||||
|
||||
#ifndef __LINK_LAYER_H_
|
||||
#define __LINK_LAYER_H_
|
||||
|
||||
/***************************** Include Files *********************************/
|
||||
#include <stdint.h>
|
||||
#include "link_layer_types.h"
|
||||
#include "schdevent.h"
|
||||
#include "node_list.h"
|
||||
#include "circ_queue.h"
|
||||
#include "../phy_layer/phy_layer.h"
|
||||
#include "../../system_monitor.h"
|
||||
/************************** Constant Definitions *****************************/
|
||||
/***************** Macros (Inline Functions) Definitions *********************/
|
||||
#define AssertL2Event(instPtr, event) (Linklayer_EventHandler((instPtr), (event)))
|
||||
/**************************** Type Definitions *******************************/
|
||||
|
||||
/**
|
||||
* @struct L2_CONFIG
|
||||
* @brief A struct holds all linklayer initialization parameters
|
||||
*/
|
||||
typedef struct
|
||||
{
|
||||
L2_NODE_TYPE myType; ///< node type
|
||||
uint8_t clustId; ///< node cluster id
|
||||
uint8_t myId; ///< node id
|
||||
uint8_t headId; ///< head node id
|
||||
uint8_t netScale; ///< net scale
|
||||
L2_SVC_RATE serviceRate; ///< service data rate
|
||||
PHY_INST *phyInstPtr; ///< pointer to system physical layer instance
|
||||
TMC_INST *tmcInstPtr; ///< pointer to system timer controller layer instance
|
||||
} L2_CONFIG;
|
||||
|
||||
/**
|
||||
* @struct L2_LOG
|
||||
* @brief A struct holds linklayer operating log
|
||||
*/
|
||||
typedef struct
|
||||
{
|
||||
uint32_t frameRx; ///< total rx frame count
|
||||
uint32_t frameTx; ///< total tx frame count
|
||||
uint32_t frameRecv; ///< total received frame count
|
||||
uint32_t frameCorrect; ///< total correctly received frame count
|
||||
|
||||
uint32_t advRxCnt; ///< advertise rx frame count
|
||||
uint32_t advTxCnt; ///< advertise tx frame count
|
||||
uint32_t advRecvCnt; ///< advertise received frame count
|
||||
uint32_t advCorrectCnt; ///< advertise correctly received frame count
|
||||
|
||||
uint32_t ackRxCnt; ///< acknowledgement rx frame count
|
||||
uint32_t ackTxCnt; ///< acknowledgement tx frame count
|
||||
uint32_t ackRecvCnt; ///< acknowledgement received frame count
|
||||
uint32_t ackCorrectCnt; ///< acknowledgement correctly received frame count
|
||||
|
||||
uint32_t disRxCnt; ///< distribution rx frame count
|
||||
uint32_t disTxCnt; ///< distribution tx frame count
|
||||
uint32_t disRecvCnt; ///< distribution received frame count
|
||||
uint32_t disCorrectCnt;///< distribution correctly received frame count
|
||||
|
||||
uint32_t mngRxCnt; ///< management rx frame count
|
||||
uint32_t mngTxCnt; ///< management tx frame count
|
||||
uint32_t mngRecvCnt; ///< management received frame count
|
||||
uint32_t mngCorrectCnt; ///< management correctly received frame count
|
||||
|
||||
uint32_t rttRxCnt; ///< rtt rx frame count
|
||||
uint32_t rttTxCnt; ///< rtt tx frame count
|
||||
uint32_t rttRecvCnt; ///< rtt received frame count
|
||||
uint32_t rttCorrectCnt; ///< rtt correctly received frame count
|
||||
|
||||
uint32_t svcRxCnt; ///< service rx frame count
|
||||
uint32_t svcTxCnt; ///< service tx frame count
|
||||
uint32_t svcRecvCnt; ///< service received frame count
|
||||
uint32_t svcCorrectCnt; ///< service correctly received frame count
|
||||
|
||||
int64_t timeDev; ///< last time deviation estimation result
|
||||
uint32_t reconnectCnt; ///< reconnect trial count
|
||||
|
||||
PAYLOAD rxLog;
|
||||
PAYLOAD txLog;
|
||||
} L2_LOG;
|
||||
|
||||
/**
|
||||
* @struct L2_INST
|
||||
* @brief A linklayer instance
|
||||
*/
|
||||
typedef struct
|
||||
{
|
||||
L2_CONFIG config; ///< linklayer configuration parameters
|
||||
L2_NODE_INFO myInfo; ///< current node info
|
||||
NODE_LIST *nodeList; ///< discovered node list
|
||||
L2_SCHD_POOL *schdEventPool; ///< event schedule pool
|
||||
uint32_t netStartTime; ///< network start time
|
||||
bool isEnable; ///< indicates if the node is connected to the network
|
||||
bool dataAvail; ///< indicates if there is a valid received frame data in rx buffer
|
||||
bool cxn2Net; ///< indicates if the node is connected to the network
|
||||
L2_RCD frameNum; ///< stores the last current and next frame number.
|
||||
L2_RCD slotNum; ///< stores the last current and next slot number.
|
||||
L2_RCD frame; ///< stores the last current and next link layer frame type.
|
||||
L2_RCD tor; ///< stores the last current and next link layer tx/rx 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_SVC_FSM *serviceFSM; ///< FSM of service net (data service)
|
||||
uint8_t *sendPktData; ///< send data buffer
|
||||
uint8_t *recvPktData; ///< received data buffer
|
||||
L2_LOG log; ///< Linklayer operating log
|
||||
// for system log
|
||||
SYSINFO sysInfo; ///< system info
|
||||
LINKLOG linkLog; ///< linklayer log for system monitor
|
||||
} L2_INST;
|
||||
|
||||
/************************** Variable Definitions *****************************/
|
||||
extern L2_RTNE_TABLE_ENTRY *initTable[L2_INIT_RTNE_PERIOD];
|
||||
extern L2_RTNE_TABLE_ENTRY *normalTable[L2_NORMAL_RTNE_PERIOD];
|
||||
/************************** Function Prototypes ******************************/
|
||||
/**
|
||||
* @defgroup LinkLayer_Main
|
||||
* @{
|
||||
*/
|
||||
int32_t Linklayer_Init(L2_INST **instPtr, const L2_CONFIG *config);
|
||||
int32_t Linklayer_Reconfig(L2_INST *instPtr, const L2_CONFIG *config);
|
||||
void Linklayer_Destroy(L2_INST *instPtr);
|
||||
int32_t Linklayer_Start(L2_INST *instPtr);
|
||||
int32_t Linklayer_Halt(L2_INST *instPtr);
|
||||
int32_t Linklayer_Proc(L2_INST *instPtr);
|
||||
int32_t Linklayer_EventHandler(L2_INST *instPtr, const L2_EVENT *l2Event);
|
||||
int32_t Linklayer_PktHandler(L2_INST *instPtr, L2_FRAME_TYPE frameType);
|
||||
int32_t L2_SetupNextSlot(L2_INST *instPtr, L2_TOR_TYPE txrxMode, L2_FRAME_TYPE frameType);
|
||||
/** @} LinkLayer_Main*/
|
||||
|
||||
/**
|
||||
* @defgroup LinkLayer_Framing
|
||||
* @{
|
||||
*/
|
||||
void L2_FrameInsertChunk(uint8_t *frame, size_t offset, size_t bytes, const void *chunk);
|
||||
void L2_FrameGetChunk(uint8_t *frame, size_t offset, size_t bytes, void *chunk);
|
||||
int32_t L2_CrcAppend(uint8_t *dataBuff, uint32_t dataLen, uint32_t crcPos, L2_CRC_TYPE crcType);
|
||||
int32_t L2_CrcGen(uint8_t *dataBuff, uint32_t dataLen, L2_CRC_TYPE crcType, uint32_t *crc);
|
||||
bool L2_CrcCheck(uint8_t *dataBuff, uint32_t dataLen, L2_CRC_TYPE crcType);
|
||||
/** @} LinkLayer_Framing*/
|
||||
|
||||
/**
|
||||
* @defgroup LinkLayer_Scheduler
|
||||
* @{
|
||||
*/
|
||||
void L2_EventPoolHandler(L2_INST *instPtr);
|
||||
int32_t L2_ScheduleEventAt(L2_INST *instPtr, L2_SCHD_EVENT *schdEvent, uint32_t frameNo, uint32_t slotNo);
|
||||
int32_t L2_ScheduleEventAfter(L2_INST *instPtr, L2_SCHD_EVENT *schdEvent, uint32_t numSlots);
|
||||
int32_t L2_RescheduleEventAt(L2_INST *instPtr, L2_SCHD_EVENT *schdEvent, uint32_t frameNo, uint32_t slotNo);
|
||||
int32_t L2_RescheduleEventAfter(L2_INST *instPtr, L2_SCHD_EVENT *schdEvent, uint32_t numSlots);
|
||||
/** @} LinkLayer_Scheduler*/
|
||||
|
||||
/**
|
||||
* @defgroup LinkLayer_Scan
|
||||
* @{
|
||||
*/
|
||||
int32_t L2_Scan_PktHandler(L2_INST *instPtr, uint8_t *msg);
|
||||
int32_t L2_Scan_Routine(L2_INST *instPtr, const L2_RTNE_TABLE_ENTRY *schedEntry);
|
||||
/** @} LinkLayer_Scan*/
|
||||
|
||||
/**
|
||||
* @defgroup LinkLayer_Service
|
||||
* @{
|
||||
*/
|
||||
int32_t L2_Service_Init(L2_SVC_FSM **svcFSMPtr);
|
||||
void L2_Service_Reset(L2_SVC_FSM *svcFSMPtr);
|
||||
void L2_Service_Destroy(L2_SVC_FSM *svcFSMPtr);
|
||||
int32_t L2_Service_PktHandler(L2_INST *instPtr, uint8_t *msg);
|
||||
int32_t L2_Service_Proc(L2_INST *instPtr);
|
||||
int32_t L2_Service_Routine(L2_INST *instPtr, const L2_RTNE_TABLE_ENTRY *schedEntry);
|
||||
/** @} LinkLayer_Service*/
|
||||
|
||||
/**
|
||||
* @defgroup LinkLayer_TimeSync
|
||||
* @{
|
||||
*/
|
||||
int32_t L2_GetTimeDeviation(L2_INST *instPtr, uint32_t frameNum, uint32_t slotNum, bool relative, int64_t *timeDev);
|
||||
void L2_TimeAdjust(L2_INST *instPtr, const int64_t *timeDev);
|
||||
/** @} LinkLayer_TimeSync*/
|
||||
|
||||
/**
|
||||
* @defgroup LinkLayer_Utility
|
||||
* @{
|
||||
*/
|
||||
uint8_t L2_GetNodeId(L2_INST *instPtr);
|
||||
uint8_t L2_GetClusterId(L2_INST *instPtr);
|
||||
uint8_t L2_GetHeadId(L2_INST *instPtr);
|
||||
L2_NODE_TYPE L2_GetNodeType(L2_INST *instPtr);
|
||||
L2_NODE_STATE L2_GetMainState(L2_INST *instPtr);
|
||||
bool L2_isStarted(L2_INST *instPtr);
|
||||
bool L2_isDataAvail(L2_INST *instPtr);
|
||||
bool L2_isCxn2Net(L2_INST *instPtr);
|
||||
uint32_t L2_GetFrameLen(L2_FRAME_TYPE l2FrameType, L2_SVC_RATE l2SvcRate);
|
||||
uint32_t L2_GetLastFrameNum(L2_INST *instPtr);
|
||||
uint32_t L2_GetLastSlotNum(L2_INST *instPtr);
|
||||
L2_FRAME_TYPE L2_GetLastFrameType(L2_INST *instPtr);
|
||||
L2_TOR_TYPE L2_GetLastTor(L2_INST *instPtr);
|
||||
uint32_t L2_GetCurrentFrameNum(L2_INST *instPtr);
|
||||
uint32_t L2_GetCurrentSlotNum(L2_INST *instPtr);
|
||||
L2_FRAME_TYPE L2_GetCurrentFrameType(L2_INST *instPtr);
|
||||
L2_TOR_TYPE L2_GetCurrentTor(L2_INST *instPtr);
|
||||
uint32_t L2_GetNextFrameNum(L2_INST *instPtr);
|
||||
uint32_t L2_GetNextSlotNum(L2_INST *instPtr);
|
||||
L2_FRAME_TYPE L2_GetNextFrameType(L2_INST *instPtr);
|
||||
L2_TOR_TYPE L2_GetNextTor(L2_INST *instPtr);
|
||||
L1_FRAME_TYPE L2_L2frame2L1frame(L2_FRAME_TYPE l2Frame, L2_SVC_RATE l2SvcRate);
|
||||
size_t L2_GetSvcPacketLen(L2_SVC_RATE l2SvcRate);
|
||||
size_t L2_GetSvcPayloadLen(L2_SVC_RATE l2SvcRate);
|
||||
PHY_ANT_INDEX L2_AntId2AntIndex(uint8_t antId);
|
||||
/** @} LinkLayer_Utility*/
|
||||
|
||||
#endif /* __LINK_LAYER_H_ */
|
||||
|
||||
/** @} LinkLayer*/
|
||||
@@ -0,0 +1,252 @@
|
||||
/*****************************************************************************/
|
||||
/**
|
||||
* @file link_layer_types.h
|
||||
* @author FanHao ([email protected])
|
||||
* @brief DF Linklayer types definition.
|
||||
* @version 0.1
|
||||
* @date 2023-05-06
|
||||
*
|
||||
* @copyright Copyright(c) 2024 Beijing Institute of Technology.\n
|
||||
* Lab of Communication and Network
|
||||
*
|
||||
******************************************************************************/
|
||||
/**
|
||||
* @defgroup LinkLayer
|
||||
* @{
|
||||
*/
|
||||
|
||||
#ifndef __LINK_LAYER_DEF_H_
|
||||
#define __LINK_LAYER_DEF_H_
|
||||
|
||||
/***************************** Include Files *********************************/
|
||||
#include <stdbool.h>
|
||||
#include <stdint.h>
|
||||
#include "geometry/coord.h"
|
||||
#include "geometry/quaternion.h"
|
||||
#include "../phy_layer/phy_layer.h"
|
||||
#ifndef EMULATION
|
||||
#include "../config/app_config.h"
|
||||
#endif
|
||||
/************************** Constant Definitions *****************************/
|
||||
/**
|
||||
* @name Linklayer_Common_Constants
|
||||
* @{
|
||||
*/
|
||||
// Common node ID definition
|
||||
#define L2_BROADCAST_ID1 0xF1 // broadcast
|
||||
#define L2_BROADCAST_ID2 0xF2
|
||||
#define L2_BROADCAST_ID3 0xF4
|
||||
#define L2_NONE_ID 0
|
||||
|
||||
// Routine configure
|
||||
#define L2_INIT_RTNE_PERIOD 1
|
||||
#define L2_NORMAL_RTNE_PERIOD 1
|
||||
|
||||
// Common slot number definition
|
||||
#define L2_SLOTNO_NONE 0
|
||||
#define L2_FRAMENO_NONE 0
|
||||
|
||||
// Common number of slots definition
|
||||
#define L2_SLOTS_PER_FRAME 250 // 2 ms per slot, 250 slots per time unit
|
||||
|
||||
/** @} Linklayer_Common_Constants*/
|
||||
|
||||
/**************************** Type Definitions *******************************/
|
||||
|
||||
/**
|
||||
* @struct L2_RCD
|
||||
* @brief Linklayer recode buffer
|
||||
*/
|
||||
typedef struct
|
||||
{
|
||||
uint32_t last;
|
||||
uint32_t curr;
|
||||
uint32_t next;
|
||||
} L2_RCD;
|
||||
|
||||
/**
|
||||
* @enum L2_TOR_TYPE
|
||||
*/
|
||||
typedef enum
|
||||
{
|
||||
L2_TOR_SUSPEND = 0,
|
||||
L2_TOR_TX = 1,
|
||||
L2_TOR_RX = 2
|
||||
} L2_TOR_TYPE;
|
||||
|
||||
/**
|
||||
* @enum L2_NODE_TYPE
|
||||
*/
|
||||
typedef enum
|
||||
{
|
||||
L2_NODE_NONE = 0,
|
||||
L2_NODE_HEAD = 1,
|
||||
L2_NODE_MEMBER = 2,
|
||||
L2_NODE_BACKUP = 3
|
||||
} L2_NODE_TYPE;
|
||||
|
||||
/**
|
||||
* @enum L2_NODE_STATE
|
||||
*/
|
||||
typedef enum
|
||||
{
|
||||
L2_NONE_STATE = 0,
|
||||
L2_INIT_STATE = 1,
|
||||
L2_NORMAL_STATE = 2,
|
||||
L2_INTRA_SEEK_STATE = 3,
|
||||
L2_INTER_SEEK_STATE = 4,
|
||||
L2_SEEK_STATE = 5
|
||||
} L2_NODE_STATE;
|
||||
|
||||
/**
|
||||
* @struct L2_NODE_INFO
|
||||
* @brief Node information struct contains the detail node info which needed by the link layer.
|
||||
*/
|
||||
typedef struct
|
||||
{
|
||||
uint8_t nodeId; ///< node ID
|
||||
uint8_t clustId; ///< node cluster ID
|
||||
uint8_t headId; ///< cluster head ID
|
||||
L2_NODE_TYPE nodeType; ///< node type
|
||||
uint8_t antIndex; ///< antenna number to this node
|
||||
COORD pos; ///< node position
|
||||
COORD speed; ///< node speed
|
||||
QUATERNION attitude; ///< node attitude
|
||||
} L2_NODE_INFO;
|
||||
|
||||
/**
|
||||
* @enum L2_EVENT_TYPE
|
||||
*/
|
||||
typedef enum
|
||||
{
|
||||
L2_EV_NONE = 0,
|
||||
L2_EV_SCAN_PROC_DONE = 1,
|
||||
L2_EV_NET_ADV_UPDATE = 2,
|
||||
L2_EV_NET_SETUP_DONE = 3,
|
||||
L2_EV_TIME_SYNC = 4,
|
||||
L2_EV_LOST_CXN = 5,
|
||||
L2_EV_LOST_NODE = 6,
|
||||
L2_EV_SET_FRAME_NUM = 7,
|
||||
L2_EV_SET_SLOT_NUM = 8,
|
||||
L2_EV_MAIN_STATE_UPDATE = 9,
|
||||
L2_ROUTINE_FINISH = 10,
|
||||
L2_ROUTINE_UPDATE = 11
|
||||
} L2_EVENT_TYPE;
|
||||
|
||||
/**
|
||||
* @struct L2_EVENT
|
||||
* @brief Linklayer event instance
|
||||
*/
|
||||
typedef struct
|
||||
{
|
||||
L2_EVENT_TYPE event; ///< event type
|
||||
void *msg; ///< event message (MUST be a heap item)
|
||||
} L2_EVENT;
|
||||
|
||||
/**
|
||||
* @enum L2_SLOT_TYPE
|
||||
*/
|
||||
typedef enum
|
||||
{
|
||||
L2_SLOT_NONE = 0,
|
||||
L2_SLOT_IDLE = 1,
|
||||
L2_SLOT_ADVERTISE = 2,
|
||||
L2_SLOT_ACKNOWLEDGE = 3,
|
||||
L2_SLOT_DISTRIBUTE = 4,
|
||||
L2_SLOT_MANAGE = 5,
|
||||
L2_SLOT_RTT = 6,
|
||||
L2_SLOT_SERVICE = 7
|
||||
} L2_SLOT_TYPE;
|
||||
|
||||
/**
|
||||
* @enum L2_FRAME_TYPE
|
||||
*/
|
||||
typedef enum
|
||||
{
|
||||
L2_FRAME_NONE = 0,
|
||||
L2_FRAME_ADV = 1,
|
||||
L2_FRAME_ACK = 2,
|
||||
L2_FRAME_DIS = 3,
|
||||
L2_FRAME_MNG = 4,
|
||||
L2_FRAME_RTT = 5,
|
||||
L2_FRAME_SVC = 6
|
||||
} L2_FRAME_TYPE;
|
||||
|
||||
/**
|
||||
* @enum L2_SVC_RATE
|
||||
*/
|
||||
typedef enum
|
||||
{
|
||||
L2_SVC_NONE = 0,
|
||||
L2_SVC_128K = 1, // 128 kbps
|
||||
L2_SVC_512K = 2, // 512 kbps
|
||||
L2_SVC_2M = 3 // 2 Mbps
|
||||
} L2_SVC_RATE;
|
||||
|
||||
/**
|
||||
* @enum L2_CRC_TYPE
|
||||
*/
|
||||
typedef enum
|
||||
{
|
||||
L2_CRC_NONE = 0,
|
||||
L2_CRC16_1021 = 1,
|
||||
L2_CRC16_8005 = 2,
|
||||
L2_CRC8_31 = 3
|
||||
} L2_CRC_TYPE;
|
||||
|
||||
/**
|
||||
* @struct L2_RTNE_TABLE_ENTRY
|
||||
* @brief Linklayer routine table entry
|
||||
*/
|
||||
typedef struct
|
||||
{
|
||||
L2_SLOT_TYPE slotType : 8; ///< slot type
|
||||
uint8_t slotSeq; ///< slot sequence number
|
||||
uint8_t src; ///< source node ID
|
||||
uint8_t dst; ///< destination node ID
|
||||
} L2_RTNE_TABLE_ENTRY;
|
||||
|
||||
/**
|
||||
* @enum SCAN_STAGE
|
||||
*/
|
||||
typedef enum
|
||||
{
|
||||
L2_SCAN_STAGE_NONE = 0,
|
||||
L2_SCAN_STAGE_ADV = 1
|
||||
} SCAN_STAGE;
|
||||
|
||||
/**
|
||||
* @struct L2_SVC_FSM
|
||||
* @brief Linklayer service process FSM
|
||||
*/
|
||||
typedef struct
|
||||
{
|
||||
uint32_t txCnt; ///< tx frame count
|
||||
uint32_t rxCnt; ///< rx frame count
|
||||
uint32_t rxdCnt; ///< received frame count
|
||||
#ifdef DATALINK_ENABLE_BERTEST
|
||||
uint32_t bitTotal; ///< received bits count
|
||||
uint32_t bitError; ///< error bits count
|
||||
double lossRate; ///< packet loss rate
|
||||
double errorRate; ///< bit error rate
|
||||
#endif
|
||||
} L2_SVC_FSM;
|
||||
|
||||
/**
|
||||
* @struct L2_RTNE_CONFIG
|
||||
* @brief Linklayer routine table configuration
|
||||
*/
|
||||
typedef struct
|
||||
{
|
||||
L2_RTNE_TABLE_ENTRY **rtneTable; ///< routine table
|
||||
uint32_t rtnePrd; ///< period of routine table
|
||||
L2_RTNE_TABLE_ENTRY *last; ///< last routine entry
|
||||
L2_RTNE_TABLE_ENTRY *curr; ///< curent routine entry
|
||||
L2_RTNE_TABLE_ENTRY *next; ///< next routine entry
|
||||
} L2_RTNE_CONFIG;
|
||||
|
||||
/************************** Variable Definitions *****************************/
|
||||
|
||||
#endif /* __LINK_LAYER_DEF_H_ */
|
||||
|
||||
/** @} LinkLayer*/
|
||||
@@ -0,0 +1,179 @@
|
||||
/*****************************************************************************/
|
||||
/**
|
||||
* @file msg_def.c
|
||||
* @author FanHao ([email protected])
|
||||
* @brief DF Linklayer message definition constants.
|
||||
* @version 0.0
|
||||
* @date 2023-05-06
|
||||
*
|
||||
* @copyright Copyright(c) 2024 Beijing Institute of Technology.\n
|
||||
* Lab of Communication and Network
|
||||
*
|
||||
******************************************************************************/
|
||||
|
||||
/**
|
||||
* @addtogroup LinkLayer
|
||||
* @{
|
||||
*/
|
||||
|
||||
/**
|
||||
* @name LinkLayer_MSG_Constants
|
||||
* @{
|
||||
*/
|
||||
#ifndef __L2_MSG_DEF_H_
|
||||
#define __L2_MSG_DEF_H_
|
||||
|
||||
#define L2_PACK_LEN_MAX 2048 // bytes
|
||||
|
||||
#define L2_PAYLOAD_LEN_ADV 17
|
||||
#define L2_PAYLOAD_LEN_ACK 9
|
||||
#define L2_PAYLOAD_LEN_SVC_128K 42
|
||||
#define L2_PAYLOAD_LEN_SVC_512K 267
|
||||
#define L2_PAYLOAD_LEN_SVC_2M 1222
|
||||
#define L2_MSG_CRC_LEN 1
|
||||
|
||||
|
||||
// [bytes] [name]
|
||||
// 1 udu number
|
||||
// 10 crypto sync word
|
||||
// 8 message authentication code
|
||||
// 4 frame number
|
||||
// 1 slot number
|
||||
// 1 source id
|
||||
// 1 destination id
|
||||
// 1 message type
|
||||
#define L2_MSG_UDU_NUM 0
|
||||
#define L2_MSG_UDU_NUM_LEN 1
|
||||
#define L2_MSG_SYNC_WORD 1
|
||||
#define L2_MSG_SYNC_WORD_LEN 10
|
||||
#define L2_MSG_AUTH_CODE 11
|
||||
#define L2_MSG_AUTH_CODE_LEN 8
|
||||
#define L2_MSG_FRAME_NUM 19
|
||||
#define L2_MSG_FRAME_NUM_LEN 4
|
||||
#define L2_MSG_SLOT_NUM 23
|
||||
#define L2_MSG_SLOT_NUM_LEN 1
|
||||
#define L2_MSG_SRC_ID 24
|
||||
#define L2_MSG_SRC_ID_LEN 1
|
||||
#define L2_MSG_DST_ID 25
|
||||
#define L2_MSG_DST_ID_LEN 1
|
||||
#define L2_MSG_TYPE 26
|
||||
#define L2_MSG_TYPE_LEN 1
|
||||
#define L2_MSG_PAYLOAD 27
|
||||
|
||||
#pragma pack(push, 1) // Force 1-byte alignment
|
||||
|
||||
/**
|
||||
* @struct L2_MSG_COMMON
|
||||
* @brief Link layer common message header, 27 byte / 216 bit in total.
|
||||
*/
|
||||
typedef struct
|
||||
{
|
||||
uint8_t uduNum; ///< udu (user data unit) number per pdu (packet data unit)
|
||||
struct
|
||||
{
|
||||
uint8_t cryptoSyncWord[10];
|
||||
uint8_t msgAuthCode[8];
|
||||
} MSG_KEY;
|
||||
|
||||
uint32_t frameNum; ///< frame number
|
||||
uint8_t slotNum; ///< slot number
|
||||
uint8_t src; ///< source id
|
||||
uint8_t dst; ///< destination id
|
||||
uint8_t msgType; ///< message type
|
||||
} L2_MSG_COMMON;
|
||||
|
||||
/**
|
||||
* @struct L2_MSG_ADV
|
||||
* @brief Link layer advertise message definition, 65 byte / 520 bit in total.
|
||||
*/
|
||||
typedef struct
|
||||
{
|
||||
L2_MSG_COMMON common; ///< common message header
|
||||
double srcPosX; ///< position x
|
||||
double srcPosY; ///< position y
|
||||
double srcPosZ; ///< position z
|
||||
uint32_t srcNetStatTime;///< net start time
|
||||
uint32_t advDataId; ///< advertise payload id
|
||||
uint8_t advData[17]; ///< advertise payload data
|
||||
uint8_t crc; ///< crc
|
||||
} L2_MSG_ADV;
|
||||
|
||||
/**
|
||||
* @struct L2_MSG_ACK
|
||||
* @brief Link layer acknowledge message definition, 65 byte / 520 bit in total.
|
||||
*/
|
||||
typedef struct
|
||||
{
|
||||
L2_MSG_COMMON common; ///< common message header
|
||||
double srcPosX; ///< position x
|
||||
double srcPosY; ///< position y
|
||||
double srcPosZ; ///< position z
|
||||
double srcSpeedX; ///< speed x
|
||||
double srcSpeedY; ///< speed y
|
||||
double srcSpeedZ; ///< speed z
|
||||
uint32_t ackDataId; ///< acknowledge payload id
|
||||
uint8_t ackData[9]; ///< acknowledge payload data
|
||||
uint8_t crc; ///< crc
|
||||
} L2_MSG_ACK;
|
||||
|
||||
/**
|
||||
* @struct L2_MSG_SVC_128K
|
||||
* @brief Link layer 128 kbps service message definition, 98 byte / 784 bit in total.
|
||||
*/
|
||||
typedef struct
|
||||
{
|
||||
L2_MSG_COMMON common; ///< common message header
|
||||
double srcPosX; ///< position x
|
||||
double srcPosY; ///< position y
|
||||
double srcPosZ; ///< position z
|
||||
double srcSpeedX; ///< speed x
|
||||
double srcSpeedY; ///< speed y
|
||||
double srcSpeedZ; ///< speed z
|
||||
uint32_t svcDataId; ///< service payload id
|
||||
uint8_t svcData[42]; ///< service payload data
|
||||
uint8_t crc; ///< crc
|
||||
} L2_MSG_SVC_128K;
|
||||
|
||||
/**
|
||||
* @struct L2_MSG_SVC_512K
|
||||
* @brief Link layer 512 kbps service message definition, 323 byte / 2584 bit in total.
|
||||
*/
|
||||
typedef struct
|
||||
{
|
||||
L2_MSG_COMMON common; ///< common message header
|
||||
double srcPosX; ///< position x
|
||||
double srcPosY; ///< position y
|
||||
double srcPosZ; ///< position z
|
||||
double srcSpeedX; ///< speed x
|
||||
double srcSpeedY; ///< speed y
|
||||
double srcSpeedZ; ///< speed z
|
||||
uint32_t svcDataId; ///< service payload id
|
||||
uint8_t svcData[267]; ///< service payload data
|
||||
uint8_t crc; ///< crc
|
||||
} L2_MSG_SVC_512K;
|
||||
|
||||
/**
|
||||
* @struct L2_MSG_SVC_2M
|
||||
* @brief Link layer 2 Mbps service message definition, 1278 byte / 10224 bit in total.
|
||||
*/
|
||||
typedef struct
|
||||
{
|
||||
L2_MSG_COMMON common; ///< common message header
|
||||
double srcPosX; ///< position x
|
||||
double srcPosY; ///< position y
|
||||
double srcPosZ; ///< position z
|
||||
double srcSpeedX; ///< speed x
|
||||
double srcSpeedY; ///< speed y
|
||||
double srcSpeedZ; ///< speed z
|
||||
uint32_t svcDataId; ///< service payload id
|
||||
uint8_t svcData[1222]; ///< service payload data
|
||||
uint8_t crc; ///< crc
|
||||
} L2_MSG_SVC_2M;
|
||||
|
||||
#pragma pack(pop) // Restore original packing
|
||||
|
||||
#endif //__L2_MSG_DEF_H_
|
||||
|
||||
/** @} LinkLayer_MSG_Constants*/
|
||||
|
||||
/** @} LinkLayer*/
|
||||
@@ -0,0 +1,562 @@
|
||||
/*****************************************************************************/
|
||||
/**
|
||||
* @file node_list.c
|
||||
* @author FanHao ([email protected])
|
||||
* @brief DF Linklayer node list implementation file
|
||||
* @version 0.0
|
||||
* @date 2023-05-06
|
||||
*
|
||||
* @copyright Copyright(c) 2024 Beijing Institute of Technology.\n
|
||||
* Lab of Communication and Network
|
||||
*
|
||||
******************************************************************************/
|
||||
|
||||
/**
|
||||
* @addtogroup LinkLayer
|
||||
* @{
|
||||
*/
|
||||
|
||||
/**
|
||||
* @addtogroup LinkLayer_NodeList
|
||||
* @{
|
||||
*/
|
||||
/***************************** Include Files *********************************/
|
||||
#include <stdlib.h>
|
||||
#include <string.h>
|
||||
#include <stdint.h>
|
||||
#include <stdbool.h>
|
||||
#include "link_layer_types.h"
|
||||
#include "node_list.h"
|
||||
#include "clist.h"
|
||||
#include "common.h"
|
||||
#include "error.h"
|
||||
|
||||
/***************** Macros (Inline Functions) Definitions *********************/
|
||||
/**
|
||||
* @brief Get the node list length
|
||||
* @param list pointer to node list
|
||||
*/
|
||||
inline uint32_t L2_GetNodeListLen(NODE_LIST *list)
|
||||
{
|
||||
ASSERT(list != NULL);
|
||||
return list->len;
|
||||
}
|
||||
/**************************** Type Definitions *******************************/
|
||||
|
||||
/************************** Variable Definitions *****************************/
|
||||
static NODE_ENTRY util_entry = { 0 };
|
||||
/************************** Function Prototypes ******************************/
|
||||
/**
|
||||
* @brief helper function to free node entry.
|
||||
*/
|
||||
static void entry_free(void *val)
|
||||
{
|
||||
free(val);
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief helper function to match node entries by nodeIds
|
||||
*/
|
||||
static int8_t entry_compare(void *a, void *b)
|
||||
{
|
||||
ASSERT(a != NULL);
|
||||
ASSERT(b != NULL);
|
||||
register uint8_t idA, idB;
|
||||
idA = ((NODE_ENTRY *)a)->nodeInfo.nodeId;
|
||||
idB = ((NODE_ENTRY *)b)->nodeInfo.nodeId;
|
||||
if (idA == idB)
|
||||
{
|
||||
return 0;
|
||||
}
|
||||
else if (idA < idB)
|
||||
{
|
||||
return -1;
|
||||
}
|
||||
else
|
||||
{
|
||||
return 1;
|
||||
}
|
||||
}
|
||||
/*****************************************************************************/
|
||||
/**
|
||||
* @brief Initialize node list
|
||||
*
|
||||
* @param list
|
||||
*
|
||||
* @return SUCCESS
|
||||
* ENOMEM - not enough memory.
|
||||
*
|
||||
******************************************************************************/
|
||||
int32_t L2_NodeList_Init(NODE_LIST **list)
|
||||
{
|
||||
(*list) = clist_new();
|
||||
if (*list == NULL)
|
||||
{
|
||||
return ENOMEM;
|
||||
}
|
||||
(*list)->free = entry_free;
|
||||
(*list)->compare = entry_compare;
|
||||
return SUCCESS;
|
||||
}
|
||||
|
||||
/*****************************************************************************/
|
||||
/**
|
||||
* @brief Clear all nodes in the node list, free allocated memory.
|
||||
*
|
||||
* @param list
|
||||
* @return NONE
|
||||
*
|
||||
******************************************************************************/
|
||||
void L2_NodeList_Clear(NODE_LIST *list)
|
||||
{
|
||||
ASSERT(list != NULL);
|
||||
clist_clear(list);
|
||||
}
|
||||
|
||||
/*****************************************************************************/
|
||||
/**
|
||||
* @brief Completely destroy node list.
|
||||
*
|
||||
* @param list
|
||||
*
|
||||
* @return NONE
|
||||
*
|
||||
******************************************************************************/
|
||||
void L2_NodeList_Destroy(NODE_LIST *list)
|
||||
{
|
||||
clist_destroy(list);
|
||||
}
|
||||
/*****************************************************************************/
|
||||
/**
|
||||
* @brief Get node entry from node list base on node id.
|
||||
*
|
||||
* @param list
|
||||
* @param nodeId
|
||||
*
|
||||
* @return Node entry \n
|
||||
* NULL if node dose not exist.
|
||||
*
|
||||
******************************************************************************/
|
||||
NODE_ENTRY *L2_GetNodeById(NODE_LIST *list, uint8_t nodeId)
|
||||
{
|
||||
ASSERT(list != NULL);
|
||||
if (nodeId == L2_BROADCAST_ID1 || nodeId == L2_BROADCAST_ID2 ||
|
||||
nodeId == L2_BROADCAST_ID3 || nodeId == L2_NONE_ID)
|
||||
{
|
||||
return NULL;
|
||||
}
|
||||
util_entry.nodeInfo.nodeId = nodeId;
|
||||
clist_node_t *node = clist_find(list, &util_entry);
|
||||
if (node == NULL)
|
||||
{
|
||||
return NULL;
|
||||
}
|
||||
else
|
||||
{
|
||||
return ((NODE_ENTRY *)node->val);
|
||||
}
|
||||
}
|
||||
|
||||
/*****************************************************************************/
|
||||
/**
|
||||
* @brief Append a new node entry to the node list
|
||||
*
|
||||
* @param list
|
||||
* @param nodeId
|
||||
* @param nodeType
|
||||
*
|
||||
* @return SUCCESS Node was added successfully.\n
|
||||
* Other Error occurred.(Node already exist, memory alloc error ect.)
|
||||
*
|
||||
******************************************************************************/
|
||||
NODE_ENTRY *L2_AppendNode(NODE_LIST *list, uint8_t nodeId, L2_NODE_TYPE nodeType)
|
||||
{
|
||||
ASSERT(list != NULL);
|
||||
if (L2_isNodeInList(list, nodeId))
|
||||
{
|
||||
DBG_WARNING("Node [%" PRIu8 "] already exist.\n", nodeId);
|
||||
return NULL;
|
||||
}
|
||||
DBG_INFO("Link layer appending node [%" PRIu8 "] to node list.\n", nodeId);
|
||||
NODE_ENTRY *nodeEntry = (NODE_ENTRY *)malloc(sizeof(NODE_ENTRY));
|
||||
if (nodeEntry == NULL)
|
||||
{
|
||||
DBG_ERROR("Failed to allocated memory for nodeEntry.\n");
|
||||
return NULL;
|
||||
}
|
||||
clist_node_t *node = clist_node_new(nodeEntry);
|
||||
if (node == NULL)
|
||||
{
|
||||
DBG_ERROR("Failed to allocated memory for list node.\n");
|
||||
free(nodeEntry);
|
||||
return NULL;
|
||||
}
|
||||
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.antIndex = 0;
|
||||
nodeEntry->ttlCnt = 0;
|
||||
clist_rpush(list, node);
|
||||
return nodeEntry;
|
||||
}
|
||||
|
||||
/*****************************************************************************/
|
||||
/**
|
||||
* @brief Delete a node in list based on node id.
|
||||
*
|
||||
* @param list
|
||||
* @param nodeId
|
||||
*
|
||||
* @return SUCCESS Node has been successfully deleted.\n
|
||||
* Other Node not exist.
|
||||
*
|
||||
******************************************************************************/
|
||||
int32_t L2_DeleteNodeById(NODE_LIST *list, uint8_t nodeId)
|
||||
{
|
||||
ASSERT(list != NULL);
|
||||
util_entry.nodeInfo.nodeId = nodeId;
|
||||
clist_node_t *node = clist_find(list, &util_entry);
|
||||
if (node == NULL)
|
||||
{
|
||||
DBG_WARNING("Node [%" PRIu8 "] in not in node list, skip deleting process.\n", nodeId);
|
||||
return SUCCESS;
|
||||
}
|
||||
DBG_INFO("Deleting node [%" PRIu8 "].\n", nodeId);
|
||||
clist_remove(list, node);
|
||||
return SUCCESS;
|
||||
}
|
||||
|
||||
/*****************************************************************************/
|
||||
/**
|
||||
* @brief Check whether the node is already in the node list.
|
||||
*
|
||||
* @param list
|
||||
* @param nodeId
|
||||
*
|
||||
* @return true node in the list\n
|
||||
* false node not in the list
|
||||
*
|
||||
******************************************************************************/
|
||||
bool L2_isNodeInList(NODE_LIST *list, uint8_t nodeId)
|
||||
{
|
||||
ASSERT(list != NULL);
|
||||
return (L2_GetNodeById(list, nodeId) != NULL);
|
||||
}
|
||||
|
||||
/*****************************************************************************/
|
||||
/**
|
||||
* @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\n
|
||||
* FAILURE Node dose not exit.
|
||||
*
|
||||
******************************************************************************/
|
||||
|
||||
int32_t L2_GetDistance(NODE_LIST *list, COORD myCoord, uint8_t nodeId, double *dst)
|
||||
{
|
||||
ASSERT(list != NULL);
|
||||
NODE_ENTRY *nodeEntry = L2_GetNodeById(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.\n
|
||||
* FAILURE node not exist.
|
||||
*
|
||||
******************************************************************************/
|
||||
int32_t L2_UpdateNodePos(NODE_LIST *list, uint8_t nodeId, const COORD *newPos)
|
||||
{
|
||||
ASSERT(list != NULL);
|
||||
NODE_ENTRY *nodeEntry = L2_GetNodeById(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.\n
|
||||
* FAILURE node not exist.
|
||||
*
|
||||
******************************************************************************/
|
||||
int32_t L2_GetNodePos(NODE_LIST *list, uint8_t nodeId, COORD *pos)
|
||||
{
|
||||
ASSERT(list != NULL);
|
||||
NODE_ENTRY *nodeEntry = L2_GetNodeById(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.\n
|
||||
* FAILURE node not exist.
|
||||
*
|
||||
******************************************************************************/
|
||||
int32_t L2_UpdateNodeSpeed(NODE_LIST *list, uint8_t nodeId, const COORD *newSpeed)
|
||||
{
|
||||
ASSERT(list != NULL);
|
||||
NODE_ENTRY *nodeEntry = L2_GetNodeById(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.\n
|
||||
* FAILURE node not exist.
|
||||
*
|
||||
******************************************************************************/
|
||||
int32_t L2_GetNodeSpeed(NODE_LIST *list, uint8_t nodeId, COORD *speed)
|
||||
{
|
||||
ASSERT(list != NULL);
|
||||
NODE_ENTRY *nodeEntry = L2_GetNodeById(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 devi
|
||||
*
|
||||
* @return SUCCESS update successfully.\n
|
||||
* FAILURE Node not exist.
|
||||
*
|
||||
******************************************************************************/
|
||||
int32_t L2_UpdateNodeTimeDeviation(NODE_LIST *list, uint8_t nodeId, int32_t devi)
|
||||
{
|
||||
ASSERT(list != NULL);
|
||||
NODE_ENTRY *nodeEntry = L2_GetNodeById(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 L2_IncNodeTTL(NODE_LIST *list, uint8_t nodeId, uint32_t inc)
|
||||
{
|
||||
ASSERT(list != NULL);
|
||||
NODE_ENTRY *nodeEntry = L2_GetNodeById(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.\n
|
||||
* FAILURE Node not exist.
|
||||
*
|
||||
******************************************************************************/
|
||||
int32_t L2_RstNodeTTL(NODE_LIST *list, uint8_t nodeId)
|
||||
{
|
||||
ASSERT(list != NULL);
|
||||
NODE_ENTRY *nodeEntry = L2_GetNodeById(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 L2_GetNodeTTL(NODE_LIST *list, uint8_t nodeId, uint32_t *ttl)
|
||||
{
|
||||
ASSERT(list != NULL);
|
||||
NODE_ENTRY *nodeEntry = L2_GetNodeById(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.\n
|
||||
* Other Error occurred.
|
||||
*
|
||||
******************************************************************************/
|
||||
int32_t L2_IterEditNode(NODE_LIST *list, const void *param, int32_t (*operation)(NODE_ENTRY *, const void *))
|
||||
{
|
||||
ASSERT(list != NULL);
|
||||
int32_t Status;
|
||||
clist_node_t *node;
|
||||
clist_iterator_t *iter = clist_iterator_new(list, LIST_HEAD);
|
||||
if (iter == NULL)
|
||||
{
|
||||
DBG_ERROR("Failed to initialize list iteration process.\n");
|
||||
return ENOMEM;
|
||||
}
|
||||
while ((node = clist_iterator_next(iter)))
|
||||
{
|
||||
Status = operation((NODE_ENTRY *)node->val, param);
|
||||
if (Status != SUCCESS)
|
||||
{
|
||||
DBG_ERROR("Error occurred when iteratively edit node entry.\n");
|
||||
break;
|
||||
}
|
||||
}
|
||||
clist_iterator_destroy(iter);
|
||||
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 L2_IterDeleteNode(NODE_LIST *list, bool (*condition)(NODE_ENTRY *))
|
||||
{
|
||||
ASSERT(list != NULL);
|
||||
clist_node_t *node;
|
||||
clist_iterator_t *iter = clist_iterator_new(list, LIST_HEAD);
|
||||
if (iter == NULL)
|
||||
{
|
||||
DBG_ERROR("Failed to initialize list iteration process.\n");
|
||||
return ENOMEM;
|
||||
}
|
||||
while ((node = clist_iterator_next(iter)))
|
||||
{
|
||||
if (condition((NODE_ENTRY *)node->val) == false)
|
||||
{
|
||||
clist_remove(list, node);
|
||||
}
|
||||
}
|
||||
clist_iterator_destroy(iter);
|
||||
return SUCCESS;
|
||||
}
|
||||
|
||||
/** @} LinkLayer_NodeList*/
|
||||
|
||||
/** @} LinkLayer*/
|
||||
@@ -0,0 +1,79 @@
|
||||
/*****************************************************************************/
|
||||
/**
|
||||
* @file node_list.h
|
||||
* @author FanHao ([email protected])
|
||||
* @brief DF Linklayer node list header file
|
||||
* @version 0.0
|
||||
* @date 2023-05-06
|
||||
*
|
||||
* @copyright Copyright(c) 2024 Beijing Institute of Technology.\n
|
||||
* Lab of Communication and Network
|
||||
*
|
||||
******************************************************************************/
|
||||
|
||||
/**
|
||||
* @addtogroup LinkLayer
|
||||
* @{
|
||||
*/
|
||||
|
||||
/**
|
||||
* @defgroup LinkLayer_NodeList
|
||||
* @{
|
||||
*/
|
||||
|
||||
|
||||
/***************************** Include Files *********************************/
|
||||
#ifndef NODE_LIST_H_
|
||||
#define NODE_LIST_H_
|
||||
#include <stdlib.h>
|
||||
#include <stdio.h>
|
||||
#include <math.h>
|
||||
#include <stdbool.h>
|
||||
#include "link_layer_types.h"
|
||||
#include "geometry/coord.h"
|
||||
#include "util.h"
|
||||
#include "clist.h"
|
||||
|
||||
/************************** Constant Definitions *****************************/
|
||||
/***************** Macros (Inline Functions) Definitions *********************/
|
||||
/**************************** Type Definitions *******************************/
|
||||
/**
|
||||
* @struct NODE_ENTRY
|
||||
* @brief Linklayer discovered node list node entry
|
||||
*/
|
||||
typedef struct
|
||||
{
|
||||
L2_NODE_INFO nodeInfo;
|
||||
uint32_t ttlCnt;
|
||||
int32_t timeDevi;
|
||||
} NODE_ENTRY;
|
||||
|
||||
typedef clist_t NODE_LIST;
|
||||
|
||||
/************************** Variable Definitions *****************************/
|
||||
|
||||
/************************** Function Prototypes ******************************/
|
||||
int32_t L2_NodeList_Init(NODE_LIST **list);
|
||||
void L2_NodeList_Clear(NODE_LIST *list);
|
||||
void L2_NodeList_Destroy(NODE_LIST *list);
|
||||
uint32_t L2_GetNodeListLen(NODE_LIST *list);
|
||||
NODE_ENTRY *L2_GetNodeById(NODE_LIST *list, uint8_t nodeId);
|
||||
NODE_ENTRY *L2_AppendNode(NODE_LIST *list, uint8_t nodeId, L2_NODE_TYPE nodeType);
|
||||
int32_t L2_DeleteNodeById(NODE_LIST *list, uint8_t nodeId);
|
||||
int32_t L2_UpdateNodePos(NODE_LIST *list, uint8_t nodeId, const COORD *newPos);
|
||||
bool L2_isNodeInList(NODE_LIST *list, uint8_t nodeId);
|
||||
int32_t L2_GetNodePos(NODE_LIST *list, uint8_t nodeId, COORD *newPos);
|
||||
int32_t L2_UpdateNodeSpeed(NODE_LIST *list, uint8_t nodeId, const COORD *newSpeed);
|
||||
int32_t L2_GetNodeSpeed(NODE_LIST *list, uint8_t nodeId, COORD *speed);
|
||||
int32_t L2_UpdateNodeTimeDeviation(NODE_LIST *list, uint8_t nodeId, int32_t devi);
|
||||
int32_t L2_GetDistance(NODE_LIST *list, COORD myCoord, uint8_t nodeId, double *dst);
|
||||
int32_t L2_RstNodeTTL(NODE_LIST *list, uint8_t nodeId);
|
||||
int32_t L2_IncNodeTTL(NODE_LIST *list, uint8_t nodeId, uint32_t inc);
|
||||
int32_t L2_GetNodeTTL(NODE_LIST *list, uint8_t nodeId, uint32_t *ttl);
|
||||
int32_t L2_IterEditNode(NODE_LIST *list, const void *param, int32_t (*operation)(NODE_ENTRY *, const void *));
|
||||
int32_t L2_IterDeleteNode(NODE_LIST *list, bool (*condition)(NODE_ENTRY *));
|
||||
|
||||
#endif /* NODE_LIST_H_ */
|
||||
|
||||
/** @} LinkLayer_NodeList*/
|
||||
/** @} LinkLayer*/
|
||||
@@ -0,0 +1,314 @@
|
||||
/*****************************************************************************/
|
||||
/**
|
||||
* @file routine_table.c
|
||||
* @author FanHao ([email protected])
|
||||
* @brief DF Linklayer node list implementation file
|
||||
* @version 0.0
|
||||
* @date 2023-05-06
|
||||
*
|
||||
* @copyright Copyright(c) 2024 Beijing Institute of Technology.\n
|
||||
* Lab of Communication and Network
|
||||
*
|
||||
******************************************************************************/
|
||||
|
||||
/**
|
||||
* @addtogroup LinkLayer
|
||||
* @{
|
||||
*/
|
||||
|
||||
/**
|
||||
* @name RoutineTable
|
||||
* @{
|
||||
*/
|
||||
|
||||
#include <inttypes.h>
|
||||
#include "link_layer.h"
|
||||
#include "link_layer_types.h"
|
||||
|
||||
L2_RTNE_TABLE_ENTRY innerScanSubT[L2_SLOTS_PER_FRAME] =
|
||||
{
|
||||
// 36 slots: advertise of antenna-1 ~ antenna-3
|
||||
// 12 slots: idle for no antenna-4
|
||||
// 2 slots: service slot, set to idle
|
||||
{L2_SLOT_ADVERTISE, 0x00, 0x01, L2_BROADCAST_ID1},
|
||||
{L2_SLOT_ADVERTISE, 0x01, 0x01, L2_BROADCAST_ID1},
|
||||
{L2_SLOT_ADVERTISE, 0x02, 0x01, L2_BROADCAST_ID1},
|
||||
{L2_SLOT_ADVERTISE, 0x03, 0x01, L2_BROADCAST_ID1},
|
||||
{L2_SLOT_ADVERTISE, 0x04, 0x01, L2_BROADCAST_ID1},
|
||||
{L2_SLOT_ADVERTISE, 0x05, 0x01, L2_BROADCAST_ID1},
|
||||
{L2_SLOT_ADVERTISE, 0x06, 0x01, L2_BROADCAST_ID1},
|
||||
{L2_SLOT_ADVERTISE, 0x07, 0x01, L2_BROADCAST_ID1},
|
||||
{L2_SLOT_ADVERTISE, 0x08, 0x01, L2_BROADCAST_ID1},
|
||||
{L2_SLOT_ADVERTISE, 0x09, 0x01, L2_BROADCAST_ID1},
|
||||
{L2_SLOT_ADVERTISE, 0x0A, 0x01, L2_BROADCAST_ID1},
|
||||
{L2_SLOT_ADVERTISE, 0x0B, 0x01, L2_BROADCAST_ID1},
|
||||
{L2_SLOT_ADVERTISE, 0x00, 0x01, L2_BROADCAST_ID2},
|
||||
{L2_SLOT_ADVERTISE, 0x01, 0x01, L2_BROADCAST_ID2},
|
||||
{L2_SLOT_ADVERTISE, 0x02, 0x01, L2_BROADCAST_ID2},
|
||||
{L2_SLOT_ADVERTISE, 0x03, 0x01, L2_BROADCAST_ID2},
|
||||
{L2_SLOT_ADVERTISE, 0x04, 0x01, L2_BROADCAST_ID2},
|
||||
{L2_SLOT_ADVERTISE, 0x05, 0x01, L2_BROADCAST_ID2},
|
||||
{L2_SLOT_ADVERTISE, 0x06, 0x01, L2_BROADCAST_ID2},
|
||||
{L2_SLOT_ADVERTISE, 0x07, 0x01, L2_BROADCAST_ID2},
|
||||
{L2_SLOT_ADVERTISE, 0x08, 0x01, L2_BROADCAST_ID2},
|
||||
{L2_SLOT_ADVERTISE, 0x09, 0x01, L2_BROADCAST_ID2},
|
||||
{L2_SLOT_ADVERTISE, 0x0A, 0x01, L2_BROADCAST_ID2},
|
||||
{L2_SLOT_ADVERTISE, 0x0B, 0x01, L2_BROADCAST_ID2},
|
||||
{L2_SLOT_ADVERTISE, 0x00, 0x01, L2_BROADCAST_ID3},
|
||||
{L2_SLOT_ADVERTISE, 0x01, 0x01, L2_BROADCAST_ID3},
|
||||
{L2_SLOT_ADVERTISE, 0x02, 0x01, L2_BROADCAST_ID3},
|
||||
{L2_SLOT_ADVERTISE, 0x03, 0x01, L2_BROADCAST_ID3},
|
||||
{L2_SLOT_ADVERTISE, 0x04, 0x01, L2_BROADCAST_ID3},
|
||||
{L2_SLOT_ADVERTISE, 0x05, 0x01, L2_BROADCAST_ID3},
|
||||
{L2_SLOT_ADVERTISE, 0x06, 0x01, L2_BROADCAST_ID3},
|
||||
{L2_SLOT_ADVERTISE, 0x07, 0x01, L2_BROADCAST_ID3},
|
||||
{L2_SLOT_ADVERTISE, 0x08, 0x01, L2_BROADCAST_ID3},
|
||||
{L2_SLOT_ADVERTISE, 0x09, 0x01, L2_BROADCAST_ID3},
|
||||
{L2_SLOT_ADVERTISE, 0x0A, 0x01, L2_BROADCAST_ID3},
|
||||
{L2_SLOT_ADVERTISE, 0x0B, 0x01, L2_BROADCAST_ID3},
|
||||
{L2_SLOT_IDLE, 0x00, 0x00, 0x00},
|
||||
{L2_SLOT_IDLE, 0x00, 0x00, 0x00},
|
||||
{L2_SLOT_IDLE, 0x00, 0x00, 0x00},
|
||||
{L2_SLOT_IDLE, 0x00, 0x00, 0x00},
|
||||
{L2_SLOT_IDLE, 0x00, 0x00, 0x00},
|
||||
{L2_SLOT_IDLE, 0x00, 0x00, 0x00},
|
||||
{L2_SLOT_IDLE, 0x00, 0x00, 0x00},
|
||||
{L2_SLOT_IDLE, 0x00, 0x00, 0x00},
|
||||
{L2_SLOT_IDLE, 0x00, 0x00, 0x00},
|
||||
{L2_SLOT_IDLE, 0x00, 0x00, 0x00},
|
||||
{L2_SLOT_IDLE, 0x00, 0x00, 0x00},
|
||||
{L2_SLOT_IDLE, 0x00, 0x00, 0x00},
|
||||
{L2_SLOT_IDLE, 0x00, 0x00, 0x00},
|
||||
{L2_SLOT_IDLE, 0x00, 0x00, 0x00},
|
||||
|
||||
// 36 slots: acknowledge of member nodes
|
||||
// 14 slots: service
|
||||
{L2_SLOT_ACKNOWLEDGE, 0x00, 0x02, 0x01},
|
||||
{L2_SLOT_ACKNOWLEDGE, 0x01, 0x02, 0x01},
|
||||
{L2_SLOT_ACKNOWLEDGE, 0x02, 0x02, 0x01},
|
||||
{L2_SLOT_ACKNOWLEDGE, 0x03, 0x02, 0x01},
|
||||
{L2_SLOT_ACKNOWLEDGE, 0x00, 0x03, 0x01},
|
||||
{L2_SLOT_ACKNOWLEDGE, 0x01, 0x03, 0x01},
|
||||
{L2_SLOT_ACKNOWLEDGE, 0x02, 0x03, 0x01},
|
||||
{L2_SLOT_ACKNOWLEDGE, 0x03, 0x03, 0x01},
|
||||
{L2_SLOT_ACKNOWLEDGE, 0x00, 0x04, 0x01},
|
||||
{L2_SLOT_ACKNOWLEDGE, 0x01, 0x04, 0x01},
|
||||
{L2_SLOT_ACKNOWLEDGE, 0x02, 0x04, 0x01},
|
||||
{L2_SLOT_ACKNOWLEDGE, 0x03, 0x04, 0x01},
|
||||
{L2_SLOT_ACKNOWLEDGE, 0x00, 0x05, 0x01},
|
||||
{L2_SLOT_ACKNOWLEDGE, 0x01, 0x05, 0x01},
|
||||
{L2_SLOT_ACKNOWLEDGE, 0x02, 0x05, 0x01},
|
||||
{L2_SLOT_ACKNOWLEDGE, 0x03, 0x05, 0x01},
|
||||
{L2_SLOT_ACKNOWLEDGE, 0x00, 0x06, 0x01},
|
||||
{L2_SLOT_ACKNOWLEDGE, 0x01, 0x06, 0x01},
|
||||
{L2_SLOT_ACKNOWLEDGE, 0x02, 0x06, 0x01},
|
||||
{L2_SLOT_ACKNOWLEDGE, 0x03, 0x06, 0x01},
|
||||
{L2_SLOT_ACKNOWLEDGE, 0x00, 0x07, 0x01},
|
||||
{L2_SLOT_ACKNOWLEDGE, 0x01, 0x07, 0x01},
|
||||
{L2_SLOT_ACKNOWLEDGE, 0x02, 0x07, 0x01},
|
||||
{L2_SLOT_ACKNOWLEDGE, 0x03, 0x07, 0x01},
|
||||
{L2_SLOT_ACKNOWLEDGE, 0x00, 0x08, 0x01},
|
||||
{L2_SLOT_ACKNOWLEDGE, 0x01, 0x08, 0x01},
|
||||
{L2_SLOT_ACKNOWLEDGE, 0x02, 0x08, 0x01},
|
||||
{L2_SLOT_ACKNOWLEDGE, 0x03, 0x08, 0x01},
|
||||
{L2_SLOT_ACKNOWLEDGE, 0x00, 0x09, 0x01},
|
||||
{L2_SLOT_ACKNOWLEDGE, 0x01, 0x09, 0x01},
|
||||
{L2_SLOT_ACKNOWLEDGE, 0x02, 0x09, 0x01},
|
||||
{L2_SLOT_ACKNOWLEDGE, 0x03, 0x09, 0x01},
|
||||
{L2_SLOT_ACKNOWLEDGE, 0x00, 0x0A, 0x01},
|
||||
{L2_SLOT_ACKNOWLEDGE, 0x01, 0x0A, 0x01},
|
||||
{L2_SLOT_ACKNOWLEDGE, 0x02, 0x0A, 0x01},
|
||||
{L2_SLOT_ACKNOWLEDGE, 0x03, 0x0A, 0x01},
|
||||
{L2_SLOT_SERVICE, 0x00, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x01, 0x01, 0x02},
|
||||
{L2_SLOT_SERVICE, 0x00, 0x01, 0x03},
|
||||
{L2_SLOT_SERVICE, 0x01, 0x01, 0x03},
|
||||
{L2_SLOT_SERVICE, 0x00, 0x01, 0x04},
|
||||
{L2_SLOT_SERVICE, 0x01, 0x01, 0x04},
|
||||
{L2_SLOT_SERVICE, 0x00, 0x01, 0x05},
|
||||
{L2_SLOT_SERVICE, 0x01, 0x01, 0x05},
|
||||
{L2_SLOT_SERVICE, 0x00, 0x01, 0x06},
|
||||
{L2_SLOT_SERVICE, 0x01, 0x01, 0x06},
|
||||
{L2_SLOT_SERVICE, 0x00, 0x01, 0x07},
|
||||
{L2_SLOT_SERVICE, 0x01, 0x01, 0x07},
|
||||
{L2_SLOT_SERVICE, 0x00, 0x01, 0x08},
|
||||
{L2_SLOT_SERVICE, 0x01, 0x01, 0x08},
|
||||
|
||||
// 12 slots: network management, head <--> node
|
||||
// 18 slots: message distribution, head ---> node
|
||||
// 12 slots: rtt, head <--> node
|
||||
// 8 slots: service
|
||||
{L2_SLOT_MANAGE, 0x00, 0x01, 0x02},
|
||||
{L2_SLOT_MANAGE, 0x01, 0x01, 0x02},
|
||||
{L2_SLOT_MANAGE, 0x00, 0x01, 0x03},
|
||||
{L2_SLOT_MANAGE, 0x01, 0x01, 0x03},
|
||||
{L2_SLOT_MANAGE, 0x00, 0x01, 0x04},
|
||||
{L2_SLOT_MANAGE, 0x01, 0x01, 0x04},
|
||||
{L2_SLOT_MANAGE, 0x00, 0x02, 0x01},
|
||||
{L2_SLOT_MANAGE, 0x01, 0x02, 0x01},
|
||||
{L2_SLOT_MANAGE, 0x00, 0x03, 0x01},
|
||||
{L2_SLOT_MANAGE, 0x01, 0x03, 0x01},
|
||||
{L2_SLOT_MANAGE, 0x00, 0x04, 0x01},
|
||||
{L2_SLOT_MANAGE, 0x01, 0x04, 0x01},
|
||||
{L2_SLOT_DISTRIBUTE, 0x00, 0x01, 0x02},
|
||||
{L2_SLOT_DISTRIBUTE, 0x01, 0x01, 0x02},
|
||||
{L2_SLOT_DISTRIBUTE, 0x00, 0x01, 0x03},
|
||||
{L2_SLOT_DISTRIBUTE, 0x01, 0x01, 0x03},
|
||||
{L2_SLOT_DISTRIBUTE, 0x00, 0x01, 0x04},
|
||||
{L2_SLOT_DISTRIBUTE, 0x01, 0x01, 0x04},
|
||||
{L2_SLOT_DISTRIBUTE, 0x00, 0x01, 0x05},
|
||||
{L2_SLOT_DISTRIBUTE, 0x01, 0x01, 0x05},
|
||||
{L2_SLOT_DISTRIBUTE, 0x00, 0x01, 0x06},
|
||||
{L2_SLOT_DISTRIBUTE, 0x01, 0x01, 0x06},
|
||||
{L2_SLOT_DISTRIBUTE, 0x00, 0x01, 0x07},
|
||||
{L2_SLOT_DISTRIBUTE, 0x01, 0x01, 0x07},
|
||||
{L2_SLOT_DISTRIBUTE, 0x00, 0x01, 0x08},
|
||||
{L2_SLOT_DISTRIBUTE, 0x01, 0x01, 0x08},
|
||||
{L2_SLOT_DISTRIBUTE, 0x00, 0x01, 0x09},
|
||||
{L2_SLOT_DISTRIBUTE, 0x01, 0x01, 0x09},
|
||||
{L2_SLOT_DISTRIBUTE, 0x00, 0x01, 0x0A},
|
||||
{L2_SLOT_DISTRIBUTE, 0x01, 0x01, 0x0A},
|
||||
{L2_SLOT_RTT, 0x00, 0x01, 0x02},
|
||||
{L2_SLOT_RTT, 0x01, 0x01, 0x02},
|
||||
{L2_SLOT_RTT, 0x00, 0x01, 0x03},
|
||||
{L2_SLOT_RTT, 0x01, 0x01, 0x03},
|
||||
{L2_SLOT_RTT, 0x00, 0x01, 0x04},
|
||||
{L2_SLOT_RTT, 0x01, 0x01, 0x04},
|
||||
{L2_SLOT_RTT, 0x00, 0x02, 0x01},
|
||||
{L2_SLOT_RTT, 0x01, 0x02, 0x01},
|
||||
{L2_SLOT_RTT, 0x00, 0x03, 0x01},
|
||||
{L2_SLOT_RTT, 0x01, 0x03, 0x01},
|
||||
{L2_SLOT_RTT, 0x00, 0x04, 0x01},
|
||||
{L2_SLOT_RTT, 0x01, 0x04, 0x01},
|
||||
{L2_SLOT_SERVICE, 0x00, 0x01, 0x09},
|
||||
{L2_SLOT_SERVICE, 0x01, 0x01, 0x09},
|
||||
{L2_SLOT_SERVICE, 0x00, 0x01, 0x0A},
|
||||
{L2_SLOT_SERVICE, 0x01, 0x01, 0x0A},
|
||||
{L2_SLOT_SERVICE, 0x00, 0x01, 0x0B},
|
||||
{L2_SLOT_SERVICE, 0x01, 0x01, 0x0B},
|
||||
{L2_SLOT_SERVICE, 0x00, 0x02, 0x01},
|
||||
{L2_SLOT_SERVICE, 0x01, 0x02, 0x01},
|
||||
|
||||
// 12 slots: network management, head <--> node
|
||||
// 18 slots: message distribution, head ---> node
|
||||
// 12 slots: rtt, head <--> node
|
||||
// 8 slots: service
|
||||
{L2_SLOT_MANAGE, 0x00, 0x01, 0x05},
|
||||
{L2_SLOT_MANAGE, 0x01, 0x01, 0x05},
|
||||
{L2_SLOT_MANAGE, 0x00, 0x01, 0x06},
|
||||
{L2_SLOT_MANAGE, 0x01, 0x01, 0x06},
|
||||
{L2_SLOT_MANAGE, 0x00, 0x01, 0x07},
|
||||
{L2_SLOT_MANAGE, 0x01, 0x01, 0x07},
|
||||
{L2_SLOT_MANAGE, 0x00, 0x05, 0x01},
|
||||
{L2_SLOT_MANAGE, 0x01, 0x05, 0x01},
|
||||
{L2_SLOT_MANAGE, 0x00, 0x06, 0x01},
|
||||
{L2_SLOT_MANAGE, 0x01, 0x06, 0x01},
|
||||
{L2_SLOT_MANAGE, 0x00, 0x07, 0x01},
|
||||
{L2_SLOT_MANAGE, 0x01, 0x07, 0x01},
|
||||
{L2_SLOT_DISTRIBUTE, 0x00, 0x01, 0x02},
|
||||
{L2_SLOT_DISTRIBUTE, 0x01, 0x01, 0x02},
|
||||
{L2_SLOT_DISTRIBUTE, 0x00, 0x01, 0x03},
|
||||
{L2_SLOT_DISTRIBUTE, 0x01, 0x01, 0x03},
|
||||
{L2_SLOT_DISTRIBUTE, 0x00, 0x01, 0x04},
|
||||
{L2_SLOT_DISTRIBUTE, 0x01, 0x01, 0x04},
|
||||
{L2_SLOT_DISTRIBUTE, 0x00, 0x01, 0x05},
|
||||
{L2_SLOT_DISTRIBUTE, 0x01, 0x01, 0x05},
|
||||
{L2_SLOT_DISTRIBUTE, 0x00, 0x01, 0x06},
|
||||
{L2_SLOT_DISTRIBUTE, 0x01, 0x01, 0x06},
|
||||
{L2_SLOT_DISTRIBUTE, 0x00, 0x01, 0x07},
|
||||
{L2_SLOT_DISTRIBUTE, 0x01, 0x01, 0x07},
|
||||
{L2_SLOT_DISTRIBUTE, 0x00, 0x01, 0x08},
|
||||
{L2_SLOT_DISTRIBUTE, 0x01, 0x01, 0x08},
|
||||
{L2_SLOT_DISTRIBUTE, 0x00, 0x01, 0x09},
|
||||
{L2_SLOT_DISTRIBUTE, 0x01, 0x01, 0x09},
|
||||
{L2_SLOT_DISTRIBUTE, 0x00, 0x01, 0x0A},
|
||||
{L2_SLOT_DISTRIBUTE, 0x01, 0x01, 0x0A},
|
||||
{L2_SLOT_RTT, 0x00, 0x01, 0x05},
|
||||
{L2_SLOT_RTT, 0x01, 0x01, 0x05},
|
||||
{L2_SLOT_RTT, 0x00, 0x01, 0x06},
|
||||
{L2_SLOT_RTT, 0x01, 0x01, 0x06},
|
||||
{L2_SLOT_RTT, 0x00, 0x01, 0x07},
|
||||
{L2_SLOT_RTT, 0x01, 0x01, 0x07},
|
||||
{L2_SLOT_RTT, 0x00, 0x05, 0x01},
|
||||
{L2_SLOT_RTT, 0x01, 0x05, 0x01},
|
||||
{L2_SLOT_RTT, 0x00, 0x06, 0x01},
|
||||
{L2_SLOT_RTT, 0x01, 0x06, 0x01},
|
||||
{L2_SLOT_RTT, 0x00, 0x07, 0x01},
|
||||
{L2_SLOT_RTT, 0x01, 0x07, 0x01},
|
||||
{L2_SLOT_SERVICE, 0x00, 0x03, 0x01},
|
||||
{L2_SLOT_SERVICE, 0x01, 0x03, 0x01},
|
||||
{L2_SLOT_SERVICE, 0x00, 0x04, 0x01},
|
||||
{L2_SLOT_SERVICE, 0x01, 0x04, 0x01},
|
||||
{L2_SLOT_SERVICE, 0x00, 0x05, 0x01},
|
||||
{L2_SLOT_SERVICE, 0x01, 0x05, 0x01},
|
||||
{L2_SLOT_SERVICE, 0x00, 0x06, 0x01},
|
||||
{L2_SLOT_SERVICE, 0x01, 0x06, 0x01},
|
||||
|
||||
// 12 slots: network management, head <--> node
|
||||
// 18 slots: message distribution, head ---> node
|
||||
// 12 slots: rtt, head <--> node
|
||||
// 8 slots: service, with one slot idle
|
||||
{L2_SLOT_MANAGE, 0x00, 0x01, 0x08},
|
||||
{L2_SLOT_MANAGE, 0x01, 0x01, 0x08},
|
||||
{L2_SLOT_MANAGE, 0x00, 0x01, 0x09},
|
||||
{L2_SLOT_MANAGE, 0x01, 0x01, 0x09},
|
||||
{L2_SLOT_MANAGE, 0x00, 0x01, 0x0A},
|
||||
{L2_SLOT_MANAGE, 0x01, 0x01, 0x0A},
|
||||
{L2_SLOT_MANAGE, 0x00, 0x08, 0x01},
|
||||
{L2_SLOT_MANAGE, 0x01, 0x08, 0x01},
|
||||
{L2_SLOT_MANAGE, 0x00, 0x09, 0x01},
|
||||
{L2_SLOT_MANAGE, 0x01, 0x09, 0x01},
|
||||
{L2_SLOT_MANAGE, 0x00, 0x0A, 0x01},
|
||||
{L2_SLOT_MANAGE, 0x01, 0x0A, 0x01},
|
||||
{L2_SLOT_DISTRIBUTE, 0x00, 0x01, 0x02},
|
||||
{L2_SLOT_DISTRIBUTE, 0x01, 0x01, 0x02},
|
||||
{L2_SLOT_DISTRIBUTE, 0x00, 0x01, 0x03},
|
||||
{L2_SLOT_DISTRIBUTE, 0x01, 0x01, 0x03},
|
||||
{L2_SLOT_DISTRIBUTE, 0x00, 0x01, 0x04},
|
||||
{L2_SLOT_DISTRIBUTE, 0x01, 0x01, 0x04},
|
||||
{L2_SLOT_DISTRIBUTE, 0x00, 0x01, 0x05},
|
||||
{L2_SLOT_DISTRIBUTE, 0x01, 0x01, 0x05},
|
||||
{L2_SLOT_DISTRIBUTE, 0x00, 0x01, 0x06},
|
||||
{L2_SLOT_DISTRIBUTE, 0x01, 0x01, 0x06},
|
||||
{L2_SLOT_DISTRIBUTE, 0x00, 0x01, 0x07},
|
||||
{L2_SLOT_DISTRIBUTE, 0x01, 0x01, 0x07},
|
||||
{L2_SLOT_DISTRIBUTE, 0x00, 0x01, 0x08},
|
||||
{L2_SLOT_DISTRIBUTE, 0x01, 0x01, 0x08},
|
||||
{L2_SLOT_DISTRIBUTE, 0x00, 0x01, 0x09},
|
||||
{L2_SLOT_DISTRIBUTE, 0x01, 0x01, 0x09},
|
||||
{L2_SLOT_DISTRIBUTE, 0x00, 0x01, 0x0A},
|
||||
{L2_SLOT_DISTRIBUTE, 0x01, 0x01, 0x0A},
|
||||
{L2_SLOT_RTT, 0x00, 0x01, 0x08},
|
||||
{L2_SLOT_RTT, 0x01, 0x01, 0x08},
|
||||
{L2_SLOT_RTT, 0x00, 0x01, 0x09},
|
||||
{L2_SLOT_RTT, 0x01, 0x01, 0x09},
|
||||
{L2_SLOT_RTT, 0x00, 0x01, 0x0A},
|
||||
{L2_SLOT_RTT, 0x01, 0x01, 0x0A},
|
||||
{L2_SLOT_RTT, 0x00, 0x08, 0x01},
|
||||
{L2_SLOT_RTT, 0x01, 0x08, 0x01},
|
||||
{L2_SLOT_RTT, 0x00, 0x09, 0x01},
|
||||
{L2_SLOT_RTT, 0x01, 0x09, 0x01},
|
||||
{L2_SLOT_RTT, 0x00, 0x0A, 0x01},
|
||||
{L2_SLOT_RTT, 0x01, 0x0A, 0x01},
|
||||
{L2_SLOT_SERVICE, 0x00, 0x08, 0x01},
|
||||
{L2_SLOT_SERVICE, 0x01, 0x08, 0x01},
|
||||
{L2_SLOT_SERVICE, 0x00, 0x09, 0x01},
|
||||
{L2_SLOT_SERVICE, 0x01, 0x09, 0x01},
|
||||
{L2_SLOT_SERVICE, 0x00, 0x0A, 0x01},
|
||||
{L2_SLOT_SERVICE, 0x01, 0x0A, 0x01},
|
||||
{L2_SLOT_IDLE, 0x00, 0x00, 0x00},
|
||||
{L2_SLOT_IDLE, 0x00, 0x00, 0x00}
|
||||
};
|
||||
|
||||
L2_RTNE_TABLE_ENTRY *initTable[L2_INIT_RTNE_PERIOD] =
|
||||
{
|
||||
innerScanSubT
|
||||
};
|
||||
|
||||
L2_RTNE_TABLE_ENTRY *normalTable[L2_NORMAL_RTNE_PERIOD] =
|
||||
{
|
||||
innerScanSubT
|
||||
};
|
||||
|
||||
/** @} RoutineTable*/
|
||||
/** @} LinkLayer*/
|
||||
@@ -0,0 +1,431 @@
|
||||
/*****************************************************************************/
|
||||
/**
|
||||
* @file scan.c
|
||||
* @author FanHao ([email protected])
|
||||
* @brief DF Linklayer scan implementation file.
|
||||
* @version 0.1
|
||||
* @date 2023-05-06
|
||||
*
|
||||
* @copyright Copyright(c) 2024 Beijing Institute of Technology.\n
|
||||
* Lab of Communication and Network
|
||||
*
|
||||
******************************************************************************/
|
||||
/**
|
||||
* @addtogroup LinkLayer
|
||||
* @{
|
||||
*/
|
||||
|
||||
/**
|
||||
* @addtogroup LinkLayer_Scan
|
||||
* @{
|
||||
*/
|
||||
|
||||
/***************************** Include Files *********************************/
|
||||
#include <string.h>
|
||||
#include "link_layer.h"
|
||||
#include "link_layer_types.h"
|
||||
#include "msg_def.h"
|
||||
#include "node_list.h"
|
||||
#include "error.h"
|
||||
#include "common.h"
|
||||
#include "../phy_layer/phy_layer.h"
|
||||
|
||||
/************************** Constant Definitions *****************************/
|
||||
#define ACK_FRAME_SLOTS 4
|
||||
/***************** Macros (In-line Functions) Definitions *********************/
|
||||
|
||||
/**************************** Type Definitions *******************************/
|
||||
|
||||
/************************** Variable Definitions *****************************/
|
||||
|
||||
/************************** Function Prototypes ******************************/
|
||||
static int32_t ScanMsg_Packing(L2_INST *instPtr, L2_FRAME_TYPE type, uint8_t dstId);
|
||||
|
||||
/*****************************************************************************/
|
||||
/**
|
||||
* @brief Packing Scan message to a bit array.
|
||||
*
|
||||
* @param bitarr pointer to bit array buffer.
|
||||
* @param msgType message type.
|
||||
*
|
||||
* @return EPARAM
|
||||
* SUCCESS
|
||||
*
|
||||
******************************************************************************/
|
||||
static int32_t ScanMsg_Packing(L2_INST *instPtr, L2_FRAME_TYPE type, uint8_t dstId)
|
||||
{
|
||||
ASSERT(instPtr != NULL);
|
||||
int32_t Status;
|
||||
|
||||
switch (type)
|
||||
{
|
||||
case L2_FRAME_ADV:
|
||||
{
|
||||
L2_MSG_ADV *message = (L2_MSG_ADV *)(instPtr->sendPktData);
|
||||
|
||||
// Common section of link layer package
|
||||
message->common.frameNum = L2_GetNextFrameNum(instPtr);
|
||||
message->common.slotNum = L2_GetNextSlotNum(instPtr);
|
||||
message->common.src = L2_GetNodeId(instPtr);
|
||||
message->common.dst = dstId;
|
||||
message->common.msgType = L2_FRAME_ADV;
|
||||
|
||||
// insert position and speed info
|
||||
SysMonitor_GetSysinfo(&(instPtr->sysInfo));
|
||||
message->srcPosX = instPtr->sysInfo.posX;
|
||||
message->srcPosY = instPtr->sysInfo.posY;
|
||||
message->srcPosZ = instPtr->sysInfo.posZ;
|
||||
|
||||
// Insert net start time
|
||||
message->srcNetStatTime = instPtr->netStartTime;
|
||||
|
||||
// Packing extra payload
|
||||
SysMonitor_GetSendPayload(message->advData, &(message->advDataId), L2_PAYLOAD_LEN_ADV);
|
||||
if (message->advDataId == DATA_ID_NONE)
|
||||
{
|
||||
memset(message->advData, 0xAA, L2_PAYLOAD_LEN_ADV);
|
||||
}
|
||||
|
||||
// Append CRC
|
||||
uint32_t crcVal;
|
||||
Status = L2_CrcGen((uint8_t *)message, sizeof(L2_MSG_ADV) - L2_MSG_CRC_LEN, L2_CRC8_31, &crcVal);
|
||||
if (Status != SUCCESS)
|
||||
{
|
||||
DBG_ERROR("Fail to generate CRC, error code: %" PRId32 ".\n", Status);
|
||||
return Status;
|
||||
}
|
||||
message->crc = crcVal;
|
||||
|
||||
// Logging...
|
||||
memcpy(instPtr->log.txLog.data, message, sizeof(L2_MSG_ADV));
|
||||
instPtr->log.txLog.dataId = message->advDataId;
|
||||
|
||||
break;
|
||||
}
|
||||
case L2_FRAME_ACK:
|
||||
{
|
||||
L2_MSG_ACK *message = (L2_MSG_ACK *)(instPtr->sendPktData);
|
||||
|
||||
// Common section of link layer package
|
||||
message->common.frameNum = L2_GetNextFrameNum(instPtr);
|
||||
message->common.slotNum = L2_GetNextSlotNum(instPtr);
|
||||
message->common.src = L2_GetNodeId(instPtr);
|
||||
message->common.dst = dstId;
|
||||
message->common.msgType = L2_FRAME_ADV;
|
||||
|
||||
// insert position and speed info
|
||||
SysMonitor_GetSysinfo(&(instPtr->sysInfo));
|
||||
message->srcPosX = instPtr->sysInfo.posX;
|
||||
message->srcPosY = instPtr->sysInfo.posY;
|
||||
message->srcPosZ = instPtr->sysInfo.posZ;
|
||||
message->srcSpeedX = instPtr->sysInfo.vX;
|
||||
message->srcSpeedY = instPtr->sysInfo.vY;
|
||||
message->srcSpeedZ = instPtr->sysInfo.vZ;
|
||||
|
||||
// Packing extra payload
|
||||
SysMonitor_GetSendPayload(message->ackData, &(message->ackDataId), L2_PAYLOAD_LEN_ACK);
|
||||
if (message->ackDataId == DATA_ID_NONE)
|
||||
{
|
||||
memset(message->ackData, 0xAA, L2_PAYLOAD_LEN_ACK);
|
||||
}
|
||||
|
||||
// Append CRC
|
||||
uint32_t crcVal;
|
||||
Status = L2_CrcGen((uint8_t *)message, sizeof(L2_MSG_ACK) - L2_MSG_CRC_LEN, L2_CRC8_31, &crcVal);
|
||||
if (Status != SUCCESS)
|
||||
{
|
||||
DBG_ERROR("Fail to generate CRC, error code: %" PRId32 ".\n", Status);
|
||||
return Status;
|
||||
}
|
||||
message->crc = crcVal;
|
||||
|
||||
// Logging...
|
||||
memcpy(instPtr->log.txLog.data, message, sizeof(L2_MSG_ACK));
|
||||
instPtr->log.txLog.dataId = message->ackDataId;
|
||||
|
||||
break;
|
||||
}
|
||||
default:
|
||||
DBG_ERROR("Unexpected frame type: %" PRId8 ".\n", type);
|
||||
return EPARAM;
|
||||
}
|
||||
return SUCCESS;
|
||||
}
|
||||
|
||||
/*****************************************************************************/
|
||||
/**
|
||||
* @brief Scan net packet handler.
|
||||
*
|
||||
* @param instPtr a pointer to linklayer layer instance
|
||||
* @param msg A pointer to frame data buffer.
|
||||
*
|
||||
* @return
|
||||
*
|
||||
******************************************************************************/
|
||||
int32_t L2_Scan_PktHandler(L2_INST *instPtr, uint8_t *msg)
|
||||
{
|
||||
ASSERT(instPtr != NULL);
|
||||
ASSERT(msg != NULL);
|
||||
int32_t Status;
|
||||
L2_FRAME_TYPE frameType = L2_FRAME_NONE;
|
||||
L2_FrameGetChunk(msg, L2_MSG_TYPE, L2_MSG_TYPE_LEN, &frameType);
|
||||
|
||||
switch (frameType)
|
||||
{
|
||||
case L2_FRAME_ADV:
|
||||
{
|
||||
if (L2_GetNodeType(instPtr) == L2_NODE_HEAD)
|
||||
{
|
||||
DBG_ERROR("Unexpected call from head node, only member Node can handle L2_FRAME_ADV.\n");
|
||||
Status = EUNEXP;
|
||||
break;
|
||||
}
|
||||
|
||||
L2_MSG_ADV *message = (L2_MSG_ADV *)msg;
|
||||
|
||||
// Update node status.
|
||||
uint8_t nodeId = message->common.src;
|
||||
NODE_ENTRY *node;
|
||||
node = L2_GetNodeById(instPtr->nodeList, nodeId);
|
||||
if (node == NULL)
|
||||
{
|
||||
node = L2_AppendNode(instPtr->nodeList, nodeId, L2_NODE_HEAD);
|
||||
if (node == NULL)
|
||||
{
|
||||
DBG_ERROR("Failed to append new node [id: %" PRId8 "]", nodeId);
|
||||
Status = ENOMEM;
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
// Update node info.
|
||||
node->nodeInfo.pos.x = message->srcPosX;
|
||||
node->nodeInfo.pos.y = message->srcPosY;
|
||||
node->nodeInfo.pos.z = message->srcPosZ;
|
||||
node->nodeInfo.antIndex = Phy_ReceiveAntIndex(instPtr->config.phyInstPtr);
|
||||
|
||||
// Reset node ttl
|
||||
node->ttlCnt = 0;
|
||||
|
||||
// Update net start time.
|
||||
uint32_t netstartTime = message->srcNetStatTime;
|
||||
L2_EVENT netSetupDoneEvent =
|
||||
{
|
||||
.event = L2_EV_NET_ADV_UPDATE,
|
||||
.msg = &netstartTime
|
||||
};
|
||||
AssertL2Event(instPtr, &netSetupDoneEvent);
|
||||
|
||||
// Calculate time deviation
|
||||
uint32_t frameNum = message->common.frameNum;
|
||||
uint32_t slotNum = (uint32_t)message->common.slotNum;
|
||||
Status = L2_GetTimeDeviation(instPtr, frameNum, slotNum, true, &(instPtr->log.timeDev));
|
||||
if (Status != SUCCESS)
|
||||
{
|
||||
DBG_ERROR("Failed to compute time deviation.\n")
|
||||
break;
|
||||
}
|
||||
|
||||
// check if net setup finished.
|
||||
if (L2_isCxn2Net(instPtr) == false)
|
||||
{
|
||||
// Assert Net-Setup Done.
|
||||
L2_EVENT l2Event = {
|
||||
.event = L2_EV_NET_SETUP_DONE,
|
||||
.msg = NULL
|
||||
};
|
||||
(void)AssertL2Event(instPtr, &l2Event);
|
||||
}
|
||||
// Time sync.
|
||||
L2_TimeAdjust(instPtr, &(instPtr->log.timeDev));
|
||||
|
||||
// Get data payload
|
||||
uint32_t dataId = message->advDataId;
|
||||
SysMonitor_UpdateRecvPayload(message->advData, dataId, L2_PAYLOAD_LEN_ADV);
|
||||
|
||||
// Logging...
|
||||
memcpy(instPtr->log.rxLog.data, msg, sizeof(L2_MSG_ADV));
|
||||
instPtr->log.rxLog.dataId = dataId;
|
||||
|
||||
Status = SUCCESS;
|
||||
break;
|
||||
}
|
||||
case L2_FRAME_ACK:
|
||||
{
|
||||
if (L2_GetNodeType(instPtr) == L2_NODE_MEMBER)
|
||||
{
|
||||
DBG_ERROR("Unexpected call from member node, only head Node can handle L2_FRAME_ACK.\n");
|
||||
Status = EUNEXP;
|
||||
break;
|
||||
}
|
||||
|
||||
L2_MSG_ACK *message = (L2_MSG_ACK *)msg;
|
||||
|
||||
// Update node status.
|
||||
uint8_t nodeId = message->common.src;
|
||||
NODE_ENTRY *node;
|
||||
node = L2_GetNodeById(instPtr->nodeList, nodeId);
|
||||
if (node == NULL)
|
||||
{
|
||||
node = L2_AppendNode(instPtr->nodeList, nodeId, L2_NODE_MEMBER);
|
||||
if (node == NULL)
|
||||
{
|
||||
DBG_ERROR("Failed to append new node [id: %" PRId8 "]", nodeId);
|
||||
Status = ENOMEM;
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
// Update node info.
|
||||
node->nodeInfo.pos.x = message->srcPosX;
|
||||
node->nodeInfo.pos.y = message->srcPosY;
|
||||
node->nodeInfo.pos.z = message->srcPosZ;
|
||||
node->nodeInfo.speed.x = message->srcSpeedX;
|
||||
node->nodeInfo.speed.y = message->srcSpeedY;
|
||||
node->nodeInfo.speed.z = message->srcSpeedZ;
|
||||
node->nodeInfo.antIndex = Phy_ReceiveAntIndex(instPtr->config.phyInstPtr);
|
||||
|
||||
// Reset node ttl
|
||||
node->ttlCnt = 0;
|
||||
|
||||
// Calculate time deviation
|
||||
uint32_t frameNum = message->common.frameNum;
|
||||
uint32_t slotNum = (uint32_t)message->common.slotNum;
|
||||
Status = L2_GetTimeDeviation(instPtr, frameNum, slotNum, true, &(instPtr->log.timeDev));
|
||||
if (Status != SUCCESS)
|
||||
{
|
||||
DBG_ERROR("Failed to compute time deviation.\n")
|
||||
break;
|
||||
}
|
||||
|
||||
if (L2_GetMainState(instPtr) == L2_INIT_STATE) // FIXME: state switch should follow routing table
|
||||
{
|
||||
// Assert Net-Setup Done.
|
||||
L2_EVENT l2Event;
|
||||
L2_NODE_STATE newState = L2_NORMAL_STATE;
|
||||
l2Event.event = L2_EV_MAIN_STATE_UPDATE;
|
||||
l2Event.msg = &newState;
|
||||
(void)AssertL2Event(instPtr, &l2Event);
|
||||
}
|
||||
|
||||
// Get data payload
|
||||
uint32_t dataId = message->ackDataId;
|
||||
SysMonitor_UpdateRecvPayload(message->ackData, dataId, L2_PAYLOAD_LEN_ACK);
|
||||
|
||||
// Logging...
|
||||
memcpy(instPtr->log.rxLog.data, msg, sizeof(L2_MSG_ACK));
|
||||
instPtr->log.rxLog.dataId = dataId;
|
||||
|
||||
Status = SUCCESS;
|
||||
break;
|
||||
}
|
||||
default:
|
||||
DBG_ERROR("Unsupported frame type: %" PRId8 ".\n", frameType);
|
||||
Status = EUNEXP;
|
||||
break;
|
||||
}
|
||||
return Status;
|
||||
}
|
||||
|
||||
/*****************************************************************************/
|
||||
|
||||
/**
|
||||
* @brief Execute a scan net Routine procedure.
|
||||
*
|
||||
* @param rtneEntry Routine configuration
|
||||
*
|
||||
* @return used slot (2 ms slot).
|
||||
*
|
||||
******************************************************************************/
|
||||
int32_t L2_Scan_Routine(L2_INST *instPtr, const L2_RTNE_TABLE_ENTRY *rtneEntry)
|
||||
{
|
||||
ASSERT(instPtr != NULL);
|
||||
DBG_INFO("-------- Scan Routine --------\n");
|
||||
|
||||
if (rtneEntry->slotSeq != 0)
|
||||
{
|
||||
return SUCCESS; // skip other slots, only process in the 1st slot
|
||||
}
|
||||
|
||||
PHY_INST *phyRef = instPtr->config.phyInstPtr;
|
||||
PHY_ANT_INDEX antIndex;
|
||||
L2_TOR_TYPE torState;
|
||||
const NODE_ENTRY *commNode;
|
||||
|
||||
switch (rtneEntry->slotType)
|
||||
{
|
||||
case L2_SLOT_ADVERTISE:
|
||||
if (L2_GetNodeType(instPtr) == L2_NODE_HEAD && rtneEntry->src == L2_GetNodeId(instPtr))
|
||||
{
|
||||
// antenna selection based on broadcast id in routing table
|
||||
antIndex = L2_AntId2AntIndex(rtneEntry->dst);
|
||||
// head node transmits messages in advertise slot
|
||||
torState = L2_TOR_TX;
|
||||
|
||||
// reset memory
|
||||
memset(instPtr->sendPktData, 0xAA, L2_PACK_LEN_MAX);
|
||||
// pack message
|
||||
if (ScanMsg_Packing(instPtr, L2_FRAME_ADV, rtneEntry->dst) != SUCCESS)
|
||||
{
|
||||
return FAILURE;
|
||||
}
|
||||
}
|
||||
else if (L2_GetNodeType(instPtr) == L2_NODE_MEMBER)
|
||||
{
|
||||
commNode = L2_GetNodeById(instPtr->nodeList, rtneEntry->src);
|
||||
// antenna selection based on node list
|
||||
antIndex = commNode->nodeInfo.antIndex;
|
||||
// member node receives message in advertise slot
|
||||
torState = L2_TOR_RX;
|
||||
}
|
||||
else
|
||||
{
|
||||
// otherwise suspend
|
||||
antIndex = PHY_ANT_INDEX_NONE;
|
||||
torState = L2_TOR_SUSPEND;
|
||||
}
|
||||
(void)Phy_SetAntIndex(instPtr->config.phyInstPtr, antIndex);
|
||||
(void)L2_SetupNextSlot(instPtr, torState, L2_FRAME_ADV);
|
||||
break;
|
||||
case L2_SLOT_ACKNOWLEDGE:
|
||||
if (L2_GetNodeType(instPtr) == L2_NODE_HEAD && rtneEntry->dst == L2_GetNodeId(instPtr))
|
||||
{
|
||||
// FIXME: is antenna index necessary here?
|
||||
antIndex = PHY_ANT_INDEX_NONE;
|
||||
// head node receives messages in acknowledge slot
|
||||
torState = L2_TOR_RX;
|
||||
}
|
||||
else if (L2_GetNodeType(instPtr) == L2_NODE_MEMBER && rtneEntry->src == L2_GetNodeId(instPtr))
|
||||
{
|
||||
commNode = L2_GetNodeById(instPtr->nodeList, rtneEntry->src);
|
||||
// antenna selection based on node list
|
||||
antIndex = commNode->nodeInfo.antIndex;
|
||||
// member node transmits message in acknowledge slot
|
||||
torState = L2_TOR_TX;
|
||||
|
||||
// reset memory
|
||||
memset(instPtr->sendPktData, 0xAA, L2_PACK_LEN_MAX);
|
||||
// pack message
|
||||
if (ScanMsg_Packing(instPtr, L2_FRAME_ACK, rtneEntry->dst) != SUCCESS)
|
||||
{
|
||||
return FAILURE;
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
// otherwise suspend
|
||||
antIndex = PHY_ANT_INDEX_NONE;
|
||||
torState = L2_TOR_SUSPEND;
|
||||
}
|
||||
(void)Phy_SetAntIndex(instPtr->config.phyInstPtr, antIndex);
|
||||
(void)L2_SetupNextSlot(instPtr, torState, L2_FRAME_ACK);
|
||||
break;
|
||||
default:
|
||||
DBG_ERROR("Cannot start a routine for slot type [%" PRId8 "] properly.\n", rtneEntry->slotType);
|
||||
return FAILURE;
|
||||
}
|
||||
return SUCCESS;
|
||||
}
|
||||
|
||||
/** @} LinkLayer_Scan*/
|
||||
/** @} LinkLayer*/
|
||||
@@ -0,0 +1,438 @@
|
||||
/*****************************************************************************/
|
||||
/**
|
||||
* @file schdevent.c
|
||||
* @author FanHao ([email protected])
|
||||
* @brief DF Linklayer Event scheduling implementation file.
|
||||
* @version 0.1
|
||||
* @date 2023-05-06
|
||||
*
|
||||
* @copyright Copyright(c) 2024 Beijing Institute of Technology.\n
|
||||
* Lab of Communication and Network
|
||||
*
|
||||
******************************************************************************/
|
||||
/**
|
||||
* @addtogroup LinkLayer
|
||||
* @{
|
||||
*/
|
||||
|
||||
/**
|
||||
* @addtogroup LinkLayer_Scheduler
|
||||
* @{
|
||||
*/
|
||||
/***************************** Include Files *********************************/
|
||||
#include <stdlib.h>
|
||||
#include "schdevent.h"
|
||||
#include "link_layer.h"
|
||||
#include "link_layer_types.h"
|
||||
#include "error.h"
|
||||
#include "clist.h"
|
||||
/***************** Macros (Inline Functions) Definitions *********************/
|
||||
/**************************** Type Definitions *******************************/
|
||||
/************************** Variable Definitions *****************************/
|
||||
static L2_SCHD_EVENT util_schdevent;
|
||||
/************************** Function Prototypes ******************************/
|
||||
/**
|
||||
* @brief helper function to free schedule events.
|
||||
*/
|
||||
static void schdevent_free(void *val)
|
||||
{
|
||||
if (val == NULL)
|
||||
{
|
||||
return;
|
||||
}
|
||||
free(((L2_SCHD_EVENT *)val)->l2Event.msg);
|
||||
free(val);
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief helper function to match events by ids.
|
||||
*/
|
||||
static int8_t schdevent_compare(void *a, void *b)
|
||||
{
|
||||
ASSERT(a == NULL);
|
||||
ASSERT(b != NULL);
|
||||
register uint32_t idA, idB;
|
||||
idA = ((L2_SCHD_EVENT *)a)->eventId;
|
||||
idB = ((L2_SCHD_EVENT *)b)->eventId;
|
||||
if (idA == idB)
|
||||
{
|
||||
return 0;
|
||||
}
|
||||
else if (idA < idB)
|
||||
{
|
||||
return -1;
|
||||
}
|
||||
else
|
||||
{
|
||||
return 1;
|
||||
}
|
||||
}
|
||||
|
||||
/*****************************************************************************/
|
||||
/**
|
||||
* @brief Creat a schedule event pool.
|
||||
*
|
||||
* @param schdPool a pointer to schedule pool instance.
|
||||
*
|
||||
* @return SUCCESS
|
||||
*
|
||||
******************************************************************************/
|
||||
int32_t L2_CreateSchedEventPool(L2_SCHD_POOL **schdPool)
|
||||
{
|
||||
(*schdPool) = malloc(sizeof(L2_SCHD_POOL));
|
||||
if (*schdPool == NULL)
|
||||
{
|
||||
DBG_ERROR("Failed to allocated memory for schdPool.\n");
|
||||
return ENOMEM;
|
||||
}
|
||||
(*schdPool)->schdList = clist_new();
|
||||
if ((*schdPool)->schdList == NULL)
|
||||
{
|
||||
DBG_ERROR("Failed to allocated memory for schdPool->schdList.\n");
|
||||
free(*schdPool);
|
||||
return ENOMEM;
|
||||
}
|
||||
(*schdPool)->schdList->free = schdevent_free;
|
||||
(*schdPool)->schdList->compare = schdevent_compare;
|
||||
(*schdPool)->envCnt = 0;
|
||||
return SUCCESS;
|
||||
}
|
||||
|
||||
/*****************************************************************************/
|
||||
/**
|
||||
* @brief Clear schedule event pool, delete all events in pool and
|
||||
* free allocated memory.
|
||||
*
|
||||
* @param schdPool a pointer to schedule pool instance.
|
||||
*
|
||||
* @return SUCCESS
|
||||
*
|
||||
******************************************************************************/
|
||||
void L2_ClearSchedEventPool(L2_SCHD_POOL *schdPool)
|
||||
{
|
||||
ASSERT(schdPool != NULL);
|
||||
clist_clear(schdPool->schdList);
|
||||
schdPool->envCnt = 0;
|
||||
}
|
||||
|
||||
/*****************************************************************************/
|
||||
/**
|
||||
* @brief Completely destroy schedule event pool.
|
||||
*
|
||||
* @param schdPool a pointer to schedule pool instance.
|
||||
*
|
||||
* @return SUCCESS
|
||||
*
|
||||
******************************************************************************/
|
||||
void L2_DestroySchedEventPool(L2_SCHD_POOL *schdPool)
|
||||
{
|
||||
if (schdPool == NULL)
|
||||
{
|
||||
return;
|
||||
}
|
||||
clist_destroy(schdPool->schdList);
|
||||
free(schdPool);
|
||||
}
|
||||
|
||||
/*****************************************************************************/
|
||||
/**
|
||||
* @brief Creat a new schedule event, register event in schedule event pool.
|
||||
*
|
||||
* @param schdPool a pointer to schedule pool instance.
|
||||
* @param event event type of the new schedule event.
|
||||
* @param isVolatile event type of the new schedule event.
|
||||
*
|
||||
* @return NULL error occurred
|
||||
* Pointer to schedule event.
|
||||
*
|
||||
******************************************************************************/
|
||||
L2_SCHD_EVENT *L2_CreatNewSchdEvent(L2_SCHD_POOL *schdPool, L2_EVENT_TYPE event, bool isVolatile)
|
||||
{
|
||||
ASSERT(schdPool != NULL);
|
||||
if (event < L2_EV_SCAN_PROC_DONE || event > L2_ROUTINE_UPDATE)
|
||||
{
|
||||
DBG_ERROR("Unknown event type: %" PRId8 ".\n", event);
|
||||
return NULL;
|
||||
}
|
||||
L2_SCHD_EVENT *schdEvent = (L2_SCHD_EVENT *)malloc(sizeof(L2_SCHD_EVENT));
|
||||
if (schdEvent == NULL)
|
||||
{
|
||||
DBG_ERROR("Failed to allocate schedule event.\n");
|
||||
return NULL;
|
||||
}
|
||||
clist_node_t *node = clist_node_new(schdEvent);
|
||||
if (node == NULL)
|
||||
{
|
||||
DBG_ERROR("Failed to allocate schedule event list node.\n");
|
||||
free(schdEvent);
|
||||
return NULL;
|
||||
}
|
||||
schdEvent->eventId = schdPool->envCnt++;
|
||||
schdEvent->occurCnt = 0;
|
||||
schdEvent->l2Event.event = event;
|
||||
schdEvent->l2Event.msg = NULL;
|
||||
schdEvent->isSchd = false;
|
||||
schdEvent->isVolatile = isVolatile;
|
||||
schdEvent->schdSlot = L2_SLOTNO_NONE;
|
||||
schdEvent->schdFrame = L2_FRAMENO_NONE;
|
||||
clist_rpush(schdPool->schdList, node);
|
||||
return schdEvent;
|
||||
}
|
||||
|
||||
/*****************************************************************************/
|
||||
/**
|
||||
* @brief Get a schedule event by its id.
|
||||
*
|
||||
* @param schdPool a pointer to schedule pool instance.
|
||||
* @param id ID of the event
|
||||
*
|
||||
* @return NULL
|
||||
* Pointer to the schedule event.
|
||||
*
|
||||
******************************************************************************/
|
||||
L2_SCHD_EVENT *L2_GetEventById(L2_SCHD_POOL *schdPool, uint32_t id)
|
||||
{
|
||||
ASSERT(schdPool != NULL);
|
||||
util_schdevent.eventId = id;
|
||||
clist_node_t *node = clist_find(schdPool->schdList, &util_schdevent);
|
||||
if (node != NULL)
|
||||
{
|
||||
DBG_WARNING("Event with id=%" PRIu32 ", dose not exist.\n", id);
|
||||
return NULL;
|
||||
}
|
||||
return ((L2_SCHD_EVENT *)node->val);
|
||||
}
|
||||
|
||||
/*****************************************************************************/
|
||||
/**
|
||||
* @brief Delete a schedule event, remove the event from event pool,
|
||||
* release memory.
|
||||
*
|
||||
* @param schdPool a pointer to schedule pool instance.
|
||||
* @param id ID of the event
|
||||
*
|
||||
* @return SUCCESS
|
||||
* error code
|
||||
*
|
||||
******************************************************************************/
|
||||
int32_t L2_DeleteSchdEventById(L2_SCHD_POOL *schdPool, uint32_t id)
|
||||
{
|
||||
ASSERT(schdPool != NULL);
|
||||
util_schdevent.eventId = id;
|
||||
clist_node_t *node = clist_find(schdPool->schdList, &util_schdevent);
|
||||
if (node != NULL)
|
||||
{
|
||||
DBG_ERROR("Event with id=%" PRIu32 ", dose not exist.\n", id);
|
||||
return EIGNORE;
|
||||
}
|
||||
clist_remove(schdPool->schdList, node);
|
||||
return SUCCESS;
|
||||
}
|
||||
|
||||
/*****************************************************************************/
|
||||
/**
|
||||
* @brief Schedule a event at a specific time(indexed by frame and slot number)
|
||||
*
|
||||
* @param instPtr a pointer to linklayer instance
|
||||
* @param schdEvent pointer to the schedule event.
|
||||
* @param frameNum schedule frame number
|
||||
* @param slotNum schedule slot number
|
||||
*
|
||||
* @return FAILURE, event has already been scheduled/schedule time not allowed.
|
||||
* SUCCESS
|
||||
*
|
||||
******************************************************************************/
|
||||
int32_t L2_ScheduleEventAt(L2_INST *instPtr, L2_SCHD_EVENT *schdEvent, uint32_t frameNum, uint32_t slotNum)
|
||||
{
|
||||
ASSERT(schdEvent != NULL);
|
||||
if (L2_isEventScheduled(schdEvent) == true)
|
||||
{
|
||||
DBG_ERROR("Event %" PRIu32 " has already been scheduled.\n", schdEvent->eventId);
|
||||
return FAILURE;
|
||||
}
|
||||
if (frameNum <= L2_GetCurrentFrameNum(instPtr) && slotNum < L2_GetCurrentSlotNum(instPtr))
|
||||
{
|
||||
DBG_ERROR("Cannot schedule event %" PRIu32 " to the past(current: [%" PRIu32 "-%" PRIu32 "], schedule: [%" PRIu32 "-%" PRIu32 "]).\n",
|
||||
schdEvent->eventId, L2_GetCurrentFrameNum(instPtr), L2_GetCurrentSlotNum(instPtr), frameNum, slotNum);
|
||||
return FAILURE;
|
||||
}
|
||||
DBG_INFO("Schedule event (type:%" PRIu8 ") [%" PRIu32 "] at %" PRIu32 "-%" PRIu32 ".\n",
|
||||
schdEvent->l2Event.event, schdEvent->eventId,
|
||||
frameNum, slotNum);
|
||||
schdEvent->isSchd = true;
|
||||
schdEvent->schdFrame = frameNum;
|
||||
schdEvent->schdSlot = slotNum;
|
||||
return SUCCESS;
|
||||
}
|
||||
|
||||
/*****************************************************************************/
|
||||
/**
|
||||
* @brief Schedule a event after a specific time period.
|
||||
*
|
||||
* @param instPtr a pointer to linklayer instance
|
||||
* @param schdEvent pointer to the schedule event.
|
||||
* @param numSlots how many slot later the event should be scheduled
|
||||
*
|
||||
* @return FAILURE, event has already been scheduled/schedule time not allowed.
|
||||
* SUCCESS
|
||||
*
|
||||
******************************************************************************/
|
||||
int32_t L2_ScheduleEventAfter(L2_INST *instPtr, L2_SCHD_EVENT *schdEvent, uint32_t numSlots)
|
||||
{
|
||||
ASSERT(schdEvent != NULL);
|
||||
if (L2_isEventScheduled(schdEvent) == true)
|
||||
{
|
||||
DBG_ERROR("Event %" PRIu32 " has already been scheduled.\n", schdEvent->eventId);
|
||||
return FAILURE;
|
||||
}
|
||||
uint32_t frameNum = L2_GetCurrentFrameNum(instPtr) + (numSlots / L2_SLOTS_PER_FRAME);
|
||||
uint32_t slotNum = L2_GetCurrentSlotNum(instPtr) + (numSlots % L2_SLOTS_PER_FRAME);
|
||||
return L2_ScheduleEventAt(instPtr, schdEvent, frameNum, slotNum);
|
||||
}
|
||||
|
||||
/*****************************************************************************/
|
||||
/**
|
||||
* @brief Reschedule a event at a specific time(indexed by frame and slot number)
|
||||
*
|
||||
* @param instPtr a pointer to linklayer instance
|
||||
* @param schdEvent pointer to the schedule event.
|
||||
* @param frameNum schedule frame number
|
||||
* @param slotNum schedule slot number
|
||||
*
|
||||
* @return FAILURE, event has already been scheduled/schedule time not allowed.
|
||||
* SUCCESS
|
||||
*
|
||||
******************************************************************************/
|
||||
int32_t L2_RescheduleEventAt(L2_INST *instPtr, L2_SCHD_EVENT *schdEvent, uint32_t frameNum, uint32_t slotNum)
|
||||
{
|
||||
ASSERT(schdEvent != NULL);
|
||||
if (schdEvent->isSchd == false)
|
||||
{
|
||||
DBG_WARNING("Event %" PRIu32 " has not been scheduled yet.\n", schdEvent->eventId);
|
||||
}
|
||||
schdEvent->isSchd = false;
|
||||
return L2_ScheduleEventAt(instPtr, schdEvent, frameNum, slotNum);
|
||||
}
|
||||
|
||||
/*****************************************************************************/
|
||||
/**
|
||||
* @brief Reschedule a event after a specific time period.
|
||||
*
|
||||
* @param instPtr a pointer to linklayer instance
|
||||
* @param schdEvent pointer to the schedule event.
|
||||
* @param numSlots how many slot later the event should be scheduled
|
||||
*
|
||||
* @return FAILURE, event has already been scheduled/schedule time not allowed.
|
||||
* SUCCESS
|
||||
*
|
||||
******************************************************************************/
|
||||
int32_t L2_RescheduleEventAfter(L2_INST *instPtr, L2_SCHD_EVENT *schdEvent, uint32_t numSlots)
|
||||
{
|
||||
ASSERT(schdEvent != NULL);
|
||||
if (schdEvent->isSchd == false)
|
||||
{
|
||||
DBG_WARNING("Event %" PRIu32 " has not been scheduled yet.\n", schdEvent->eventId);
|
||||
}
|
||||
return L2_ScheduleEventAfter(instPtr, schdEvent, numSlots);
|
||||
}
|
||||
|
||||
/*****************************************************************************/
|
||||
/**
|
||||
* @brief Check and excute schedule event in event pool illiterate.
|
||||
*
|
||||
* @param instPtr a pointer to linklayer instance
|
||||
*
|
||||
* @return NONE.
|
||||
*
|
||||
******************************************************************************/
|
||||
void L2_EventPoolHandler(L2_INST *instPtr)
|
||||
{
|
||||
ASSERT(instPtr != NULL);
|
||||
clist_node_t *node;
|
||||
L2_SCHD_EVENT *event;
|
||||
L2_SCHD_POOL *pool = instPtr->schdEventPool;
|
||||
clist_iterator_t *iter = clist_iterator_new(pool->schdList, LIST_HEAD);
|
||||
if (iter == NULL)
|
||||
{
|
||||
DBG_ERROR("Failed to initialize schedule event pool iteration.\n");
|
||||
return;
|
||||
}
|
||||
while ((node = clist_iterator_next(iter)))
|
||||
{
|
||||
event = (L2_SCHD_EVENT *)node->val;
|
||||
if (L2_isEventScheduled(event))
|
||||
{
|
||||
if (event->schdFrame == L2_GetCurrentFrameNum(instPtr) && event->schdSlot == L2_GetCurrentSlotNum(instPtr))
|
||||
{
|
||||
DBG_INFO("Asserting scheduled event [%" PRIu32 "].\n", event->eventId);
|
||||
(void)AssertL2Event(instPtr, &(event->l2Event));
|
||||
if (event->isVolatile == true)
|
||||
{
|
||||
DBG_INFO("Deleting scheduled event [%" PRIu32 "], since it is volatile.\n", event->eventId);
|
||||
clist_remove(pool->schdList, node);
|
||||
}
|
||||
else
|
||||
{
|
||||
event->occurCnt++;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
clist_iterator_destroy(iter);
|
||||
}
|
||||
|
||||
/*****************************************************************************/
|
||||
/**
|
||||
* @brief Append extra message content to specific schedule event.
|
||||
*
|
||||
* @param schdEvent pointer to the schedule event.
|
||||
* msg pointer to the append content.
|
||||
*
|
||||
* @return SUCCESS.
|
||||
* error code.
|
||||
*
|
||||
******************************************************************************/
|
||||
inline void L2_SchdEventAppendMsg(L2_SCHD_EVENT *schdEvent, void *msg)
|
||||
{
|
||||
ASSERT(schdEvent != NULL);
|
||||
schdEvent->l2Event.msg = msg;
|
||||
}
|
||||
|
||||
/*****************************************************************************/
|
||||
/**
|
||||
* @brief Cancel a scheduled event.
|
||||
*
|
||||
* @param schdEvent pointer to the schedule event.
|
||||
*
|
||||
* @return SUCCESS.
|
||||
*
|
||||
******************************************************************************/
|
||||
inline int32_t L2_CancelEvent(L2_SCHD_EVENT *schdEvent)
|
||||
{
|
||||
ASSERT(schdEvent != NULL);
|
||||
if (schdEvent->isSchd == false)
|
||||
{
|
||||
DBG_WARNING("Event %" PRIu32 " has not been scheduled yet.\n", schdEvent->eventId);
|
||||
}
|
||||
schdEvent->isSchd = false;
|
||||
return SUCCESS;
|
||||
}
|
||||
|
||||
/*****************************************************************************/
|
||||
/**
|
||||
* @brief Check if the event was scheduled.
|
||||
*
|
||||
* @param schdEvent pointer to the schedule event.
|
||||
*
|
||||
* @return true.
|
||||
* false.
|
||||
*
|
||||
******************************************************************************/
|
||||
inline bool L2_isEventScheduled(const L2_SCHD_EVENT *schdEvent)
|
||||
{
|
||||
ASSERT(schdEvent != NULL);
|
||||
return schdEvent->isSchd;
|
||||
}
|
||||
|
||||
/** @} LinkLayer_Scheduler*/
|
||||
/** @} LinkLayer*/
|
||||
@@ -0,0 +1,76 @@
|
||||
/*****************************************************************************/
|
||||
/**
|
||||
* @file schdevent.c
|
||||
* @author FanHao ([email protected])
|
||||
* @brief DF Linklayer Event scheduling header file.
|
||||
* @version 0.1
|
||||
* @date 2023-05-06
|
||||
*
|
||||
* @copyright Copyright(c) 2024 Beijing Institute of Technology.\n
|
||||
* Lab of Communication and Network
|
||||
*
|
||||
******************************************************************************/
|
||||
/**
|
||||
* @addtogroup LinkLayer
|
||||
* @{
|
||||
*/
|
||||
|
||||
/**
|
||||
* @addtogroup LinkLayer_Scheduler
|
||||
* @{
|
||||
*/
|
||||
#ifndef SCHDEVENT_H_
|
||||
#define SCHDEVENT_H_
|
||||
|
||||
#define SCHDEVENT_ID_NONE UINT32_MAX
|
||||
/***************************** Include Files *********************************/
|
||||
#include <stdlib.h>
|
||||
#include "link_layer_types.h"
|
||||
#include "error.h"
|
||||
#include "clist.h"
|
||||
/***************** Macros (Inline Functions) Definitions *********************/
|
||||
/**************************** Type Definitions *******************************/
|
||||
/**
|
||||
* @struct L2_SCHD_EVENT
|
||||
* @brief Linklayer scheduled event descriptor
|
||||
*/
|
||||
typedef struct
|
||||
{
|
||||
bool isSchd;
|
||||
bool isVolatile;
|
||||
L2_EVENT l2Event;
|
||||
uint32_t eventId;
|
||||
uint32_t occurCnt;
|
||||
uint32_t schdFrame;
|
||||
uint32_t schdSlot;
|
||||
} L2_SCHD_EVENT;
|
||||
|
||||
/**
|
||||
* @struct L2_SCHD_POOL
|
||||
* @brief Linklayer schedule event pool instance
|
||||
*/
|
||||
typedef struct
|
||||
{
|
||||
clist_t *schdList;
|
||||
uint32_t envCnt;
|
||||
} L2_SCHD_POOL;
|
||||
|
||||
/************************** Variable Definitions *****************************/
|
||||
/************************** Function Prototypes ******************************/
|
||||
// Scheduling Event
|
||||
int32_t L2_CreateSchedEventPool(L2_SCHD_POOL **schdPool);
|
||||
void L2_ClearSchedEventPool(L2_SCHD_POOL *schdPool);
|
||||
void L2_DestroySchedEventPool(L2_SCHD_POOL *schdPool);
|
||||
|
||||
L2_SCHD_EVENT *L2_CreatNewSchdEvent(L2_SCHD_POOL *schdPool, L2_EVENT_TYPE event, bool isVolatile);
|
||||
L2_SCHD_EVENT *L2_GetEventById(L2_SCHD_POOL *schdPool, uint32_t id);
|
||||
int32_t L2_DeleteSchdEventById(L2_SCHD_POOL *schdPool, uint32_t id);
|
||||
|
||||
void L2_SchdEventAppendMsg(L2_SCHD_EVENT *schdEvent, void *msg);
|
||||
bool L2_isEventScheduled(const L2_SCHD_EVENT *schdEvent);
|
||||
int32_t L2_CancelEvent(L2_SCHD_EVENT *schdEvent);
|
||||
|
||||
#endif // SCHDEVENT_H_
|
||||
|
||||
/** @} LinkLayer_Scheduler*/
|
||||
/** @} LinkLayer*/
|
||||
@@ -0,0 +1,896 @@
|
||||
/*****************************************************************************/
|
||||
/**
|
||||
* @file service.c
|
||||
* @author FanHao ([email protected])
|
||||
* @brief DF Linklayer service implementation file.
|
||||
* @version 0.0
|
||||
* @date 2023-05-06
|
||||
*
|
||||
* @copyright Copyright(c) 2024 Beijing Institute of Technology.\n
|
||||
* Lab of Communication and Network
|
||||
*
|
||||
******************************************************************************/
|
||||
/**
|
||||
* @addtogroup LinkLayer
|
||||
* @{
|
||||
*/
|
||||
|
||||
/**
|
||||
* @addtogroup LinkLayer_Service
|
||||
* @{
|
||||
*/
|
||||
/***************************** Include Files *********************************/
|
||||
#include <string.h>
|
||||
#include <stdint.h>
|
||||
#include "link_layer.h"
|
||||
#include "link_layer_types.h"
|
||||
#include "msg_def.h"
|
||||
#include "error.h"
|
||||
#include "common.h"
|
||||
#include "circ_queue.h"
|
||||
#ifndef EMULATION
|
||||
#include "../config/app_config.h"
|
||||
#endif
|
||||
/************************** Constant Definitions *****************************/
|
||||
/***************** Macros (Inline Functions) Definitions *********************/
|
||||
/**************************** Type Definitions *******************************/
|
||||
/************************** Function Prototypes ******************************/
|
||||
static int32_t Service_Packing(L2_INST *instPtr, L2_FRAME_TYPE type, uint8_t dstId);
|
||||
static void Service_PackingCommon(L2_INST *instPtr, L2_FRAME_TYPE type, uint8_t dstId);
|
||||
static int32_t Service_Packing128K(L2_INST *instPtr, L2_FRAME_TYPE type, uint8_t dstId);
|
||||
static int32_t Service_Packing512K(L2_INST *instPtr, L2_FRAME_TYPE type, uint8_t dstId);
|
||||
static int32_t Service_Packing2M(L2_INST *instPtr, L2_FRAME_TYPE type, uint8_t dstId);
|
||||
static int32_t Service_PktHandler128K(L2_INST *instPtr, uint8_t *msg);
|
||||
static int32_t Service_PktHandler512K(L2_INST *instPtr, uint8_t *msg);
|
||||
static int32_t Service_PktHandler2M(L2_INST *instPtr, uint8_t *msg);
|
||||
/************************** Variable Definitions *****************************/
|
||||
|
||||
static uint8_t testData[L2_PACK_LEN_MAX] =
|
||||
{
|
||||
0x39, 0x85, 0x02, 0xF0, 0xC7, 0x6A, 0xE8, 0xB4, 0xE7, 0xAB, 0xC9, 0xDC, 0x0B, 0x41, 0x42, 0xE0,
|
||||
0xED, 0x21, 0x07, 0x62, 0x4E, 0x80, 0xA0, 0xE1, 0xAC, 0x77, 0x9B, 0x1F, 0x48, 0x0B, 0x58, 0xB9,
|
||||
0x45, 0xA5, 0x24, 0xEF, 0xF3, 0x71, 0x2E, 0x84, 0x6C, 0x22, 0xB3, 0x05, 0xEB, 0x75, 0xF2, 0x0B,
|
||||
0x45, 0x73, 0x01, 0xA8, 0x1E, 0x00, 0x21, 0x3E, 0x03, 0x86, 0x55, 0x59, 0x98, 0x5C, 0x61, 0xA2,
|
||||
0xE8, 0x20, 0x0E, 0x7A, 0xD5, 0x72, 0xAF, 0xF4, 0xBC, 0xB3, 0x6F, 0xFE, 0xB3, 0x78, 0x6A, 0x59,
|
||||
0x73, 0xBD, 0x97, 0xBE, 0x44, 0xA4, 0x9B, 0x71, 0x11, 0x45, 0x37, 0xFE, 0xB3, 0x71, 0xEF, 0x1E,
|
||||
0x3A, 0x2B, 0x58, 0x07, 0xF6, 0x01, 0x1C, 0x73, 0x28, 0x11, 0xA8, 0x15, 0xA2, 0x58, 0x32, 0x2E,
|
||||
0x7D, 0x35, 0x9C, 0xE4, 0x98, 0x7F, 0x9C, 0x81, 0x32, 0xAA, 0xC4, 0x87, 0x16, 0x26, 0x23, 0x95,
|
||||
0x57, 0xC9, 0xD4, 0x34, 0xCE, 0x63, 0x78, 0x7E, 0xD4, 0x3C, 0x16, 0x1D, 0x80, 0xC0, 0x55, 0xBF,
|
||||
0xB5, 0xD6, 0x0C, 0xA5, 0xB9, 0x0A, 0x14, 0x5B, 0x4D, 0x52, 0xF2, 0xA9, 0x7A, 0x08, 0x67, 0x8E,
|
||||
0xEB, 0x67, 0x34, 0x7F, 0x7A, 0x35, 0xAF, 0xD9, 0x07, 0x52, 0x30, 0x38, 0x70, 0xC9, 0x41, 0xF8,
|
||||
0x36, 0x61, 0x32, 0x72, 0xA4, 0x7A, 0xEA, 0x21, 0xB4, 0xBF, 0xAA, 0xCF, 0x93, 0x4F, 0xE1, 0x5C,
|
||||
0xA0, 0xFB, 0xD0, 0x2B, 0xA3, 0x82, 0x2E, 0x69, 0x7D, 0xB7, 0x76, 0x88, 0xF7, 0x5C, 0xF3, 0x82,
|
||||
0xC6, 0x6D, 0x49, 0x92, 0x64, 0x88, 0x9A, 0x43, 0xEE, 0xE9, 0x93, 0xC3, 0x70, 0x50, 0x22, 0xF1,
|
||||
0xD3, 0x2B, 0xCF, 0xA9, 0xE5, 0x69, 0x22, 0xF9, 0xBF, 0x78, 0x9A, 0xA4, 0x5A, 0x3D, 0x2E, 0x2A,
|
||||
0x23, 0xE3, 0x51, 0x8F, 0x1A, 0xC1, 0xAF, 0x47, 0x9C, 0x14, 0xCE, 0xB9, 0xCD, 0xD0, 0x02, 0xB7,
|
||||
0xB8, 0x81, 0xCC, 0x44, 0xE7, 0xA2, 0x6F, 0x32, 0x8C, 0x78, 0x79, 0x10, 0xF3, 0xE1, 0xB0, 0x7C,
|
||||
0xAB, 0x40, 0xDB, 0xF9, 0x99, 0xE7, 0x27, 0x30, 0x53, 0x67, 0x00, 0xE8, 0x33, 0x6D, 0xFC, 0x4D,
|
||||
0xDC, 0xB0, 0xFC, 0x3D, 0xEF, 0x8D, 0x43, 0xAF, 0x7E, 0x41, 0x69, 0xDA, 0xFD, 0xDF, 0x2F, 0xF8,
|
||||
0xEC, 0xFB, 0x6A, 0x65, 0xE7, 0xEC, 0xC7, 0x53, 0x1B, 0x14, 0xDF, 0xBD, 0x0C, 0x54, 0x57, 0xD2,
|
||||
0x56, 0x15, 0xF4, 0x34, 0xE2, 0xFB, 0x12, 0xA0, 0xA2, 0xDE, 0x82, 0xF5, 0x04, 0x81, 0x9B, 0x08,
|
||||
0xB6, 0x8C, 0x7B, 0xE0, 0xAA, 0xDF, 0xC9, 0x92, 0x9D, 0x5F, 0x2D, 0xF7, 0x4A, 0x0B, 0xFF, 0xA5,
|
||||
0xAA, 0xDC, 0xD6, 0x3F, 0x56, 0xB2, 0x6A, 0x3C, 0x6F, 0xAA, 0xAC, 0x84, 0xAC, 0x3F, 0x3D, 0x05,
|
||||
0x96, 0x12, 0x44, 0x39, 0x1C, 0xD1, 0xFA, 0xBE, 0x6B, 0x1A, 0xCD, 0x31, 0x29, 0x41, 0x31, 0xA3,
|
||||
0xA8, 0x0D, 0x87, 0xEB, 0x96, 0x21, 0x39, 0xD6, 0x3B, 0x11, 0x50, 0xB5, 0xFE, 0x44, 0x0E, 0x74,
|
||||
0x31, 0x12, 0xA1, 0x66, 0x49, 0xFD, 0x2D, 0x75, 0x72, 0xF9, 0xC8, 0xD9, 0x6C, 0xC9, 0x11, 0x74,
|
||||
0x2E, 0xEB, 0x20, 0x33, 0x3D, 0x2C, 0x5E, 0xEA, 0x39, 0x31, 0x88, 0xC7, 0xD1, 0x93, 0x95, 0xD8,
|
||||
0x9E, 0x87, 0x93, 0xBC, 0x09, 0x17, 0xC9, 0xA2, 0x59, 0x18, 0x51, 0x8F, 0xBF, 0x55, 0x45, 0x81,
|
||||
0x8D, 0xFD, 0xAF, 0xB2, 0x21, 0xB7, 0xD8, 0xA8, 0x00, 0x2E, 0x70, 0x73, 0x2F, 0x74, 0xBC, 0x61,
|
||||
0x0A, 0x8E, 0xE3, 0x0A, 0x92, 0x82, 0xD2, 0x82, 0xBA, 0xE4, 0x3E, 0x8E, 0xBB, 0xC7, 0xA2, 0x8F,
|
||||
0x68, 0xE1, 0x29, 0x34, 0x7C, 0xB4, 0x22, 0xCF, 0x36, 0x8F, 0x9D, 0x63, 0x9D, 0x04, 0x8F, 0x4A,
|
||||
0xFB, 0x84, 0xBB, 0x6B, 0x97, 0x2C, 0x15, 0xC9, 0x37, 0x4F, 0x40, 0xD7, 0x47, 0x50, 0x39, 0x97
|
||||
};
|
||||
|
||||
/*****************************************************************************/
|
||||
/**
|
||||
* @brief Packing Service message
|
||||
*
|
||||
* @param instPtr pointer to linklayer instance
|
||||
* type message type.
|
||||
* dstId destination node address.
|
||||
*
|
||||
* @return EPARAM
|
||||
* SUCCESS
|
||||
*
|
||||
******************************************************************************/
|
||||
static int32_t Service_Packing(L2_INST *instPtr, L2_FRAME_TYPE type, uint8_t dstId)
|
||||
{
|
||||
#ifndef DATALINK_ENABLE_BERTEST
|
||||
ASSERT(instPtr != NULL);
|
||||
int32_t Status;
|
||||
|
||||
switch (type)
|
||||
{
|
||||
case L2_FRAME_DIS:
|
||||
case L2_FRAME_MNG:
|
||||
case L2_FRAME_RTT:
|
||||
{
|
||||
Status = Service_Packing128K(instPtr, type, dstId);
|
||||
break;
|
||||
}
|
||||
case L2_FRAME_SVC:
|
||||
{
|
||||
switch (instPtr->config.serviceRate)
|
||||
{
|
||||
case L2_SVC_128K:
|
||||
Status = Service_Packing128K(instPtr, type, dstId);
|
||||
break;
|
||||
case L2_SVC_512K:
|
||||
Status = Service_Packing512K(instPtr, type, dstId);
|
||||
break;
|
||||
case L2_SVC_2M:
|
||||
Status = Service_Packing2M(instPtr, type, dstId);
|
||||
break;
|
||||
default:
|
||||
DBG_ERROR("Unexpected service rate: %" PRIu8 ".\n", instPtr->config.serviceRate);
|
||||
return EPARAM;
|
||||
}
|
||||
break;
|
||||
}
|
||||
default:
|
||||
DBG_ERROR("Unexpected frame type: %" PRIu8 ".\n", type);
|
||||
return EPARAM;
|
||||
}
|
||||
return Status;
|
||||
#else
|
||||
DBG_ERROR("Unexpected call of Service_Packing in BER test mode.\n");
|
||||
return EUNEXP;
|
||||
#endif
|
||||
}
|
||||
|
||||
/*****************************************************************************/
|
||||
/**
|
||||
* @brief Packing Service message (common part)
|
||||
*
|
||||
* @param instPtr pointer to linklayer instance
|
||||
* type message type.
|
||||
* dstId destination node address.
|
||||
*
|
||||
* @return EPARAM
|
||||
* SUCCESS
|
||||
*
|
||||
******************************************************************************/
|
||||
static void Service_PackingCommon(L2_INST *instPtr, L2_FRAME_TYPE type, uint8_t dstId)
|
||||
{
|
||||
ASSERT(instPtr != NULL);
|
||||
|
||||
L2_MSG_COMMON *message = (L2_MSG_COMMON *)(instPtr->sendPktData);
|
||||
|
||||
// Common section of link layer package
|
||||
message->frameNum = L2_GetNextFrameNum(instPtr);
|
||||
message->slotNum = L2_GetNextSlotNum(instPtr);
|
||||
message->src = L2_GetNodeId(instPtr);
|
||||
message->dst = dstId;
|
||||
message->msgType = type;
|
||||
}
|
||||
|
||||
/*****************************************************************************/
|
||||
/**
|
||||
* @brief Packing Service message (128 kbps)
|
||||
*
|
||||
* @param instPtr pointer to linklayer instance
|
||||
* type message type.
|
||||
* dstId destination node address.
|
||||
*
|
||||
* @return EPARAM
|
||||
* SUCCESS
|
||||
*
|
||||
******************************************************************************/
|
||||
static int32_t Service_Packing128K(L2_INST *instPtr, L2_FRAME_TYPE type, uint8_t dstId)
|
||||
{
|
||||
ASSERT(instPtr != NULL);
|
||||
int32_t Status;
|
||||
|
||||
Service_PackingCommon(instPtr, type, dstId);
|
||||
|
||||
L2_MSG_SVC_128K *message = (L2_MSG_SVC_128K *)(instPtr->sendPktData);
|
||||
size_t msgLen = sizeof(L2_MSG_SVC_128K);
|
||||
size_t dataLen = L2_PAYLOAD_LEN_SVC_128K;
|
||||
|
||||
// insert position and speed info
|
||||
SysMonitor_GetSysinfo(&(instPtr->sysInfo));
|
||||
message->srcPosX = instPtr->sysInfo.posX;
|
||||
message->srcPosY = instPtr->sysInfo.posY;
|
||||
message->srcPosZ = instPtr->sysInfo.posZ;
|
||||
message->srcSpeedX = instPtr->sysInfo.vX;
|
||||
message->srcSpeedY = instPtr->sysInfo.vY;
|
||||
message->srcSpeedZ = instPtr->sysInfo.vZ;
|
||||
|
||||
// Packing extra payload
|
||||
SysMonitor_GetSendPayload(message->svcData, &(message->svcDataId), dataLen);
|
||||
if (message->svcDataId == DATA_ID_NONE)
|
||||
{
|
||||
memset(message->svcData, 0xAA, dataLen);
|
||||
}
|
||||
|
||||
// Append CRC
|
||||
uint32_t crcVal;
|
||||
Status = L2_CrcGen((uint8_t *)message, msgLen - L2_MSG_CRC_LEN, L2_CRC8_31, &crcVal);
|
||||
if (Status != SUCCESS)
|
||||
{
|
||||
DBG_ERROR("Fail to generate CRC, error code: %" PRId32 ".\n", Status);
|
||||
return Status;
|
||||
}
|
||||
message->crc = crcVal;
|
||||
|
||||
// Logging...
|
||||
memcpy(instPtr->log.txLog.data, message, msgLen);
|
||||
instPtr->log.txLog.dataId = message->svcDataId;
|
||||
|
||||
return SUCCESS;
|
||||
}
|
||||
|
||||
/*****************************************************************************/
|
||||
/**
|
||||
* @brief Packing Service message (512 kbps)
|
||||
*
|
||||
* @param instPtr pointer to linklayer instance
|
||||
* type message type.
|
||||
* dstId destination node address.
|
||||
*
|
||||
* @return EPARAM
|
||||
* SUCCESS
|
||||
*
|
||||
******************************************************************************/
|
||||
static int32_t Service_Packing512K(L2_INST *instPtr, L2_FRAME_TYPE type, uint8_t dstId)
|
||||
{
|
||||
ASSERT(instPtr != NULL);
|
||||
int32_t Status;
|
||||
|
||||
Service_PackingCommon(instPtr, type, dstId);
|
||||
|
||||
L2_MSG_SVC_512K *message = (L2_MSG_SVC_512K *)(instPtr->sendPktData);
|
||||
size_t msgLen = sizeof(L2_MSG_SVC_512K);
|
||||
size_t dataLen = L2_PAYLOAD_LEN_SVC_512K;
|
||||
|
||||
// insert position and speed info
|
||||
SysMonitor_GetSysinfo(&(instPtr->sysInfo));
|
||||
message->srcPosX = instPtr->sysInfo.posX;
|
||||
message->srcPosY = instPtr->sysInfo.posY;
|
||||
message->srcPosZ = instPtr->sysInfo.posZ;
|
||||
message->srcSpeedX = instPtr->sysInfo.vX;
|
||||
message->srcSpeedY = instPtr->sysInfo.vY;
|
||||
message->srcSpeedZ = instPtr->sysInfo.vZ;
|
||||
|
||||
// Packing extra payload
|
||||
SysMonitor_GetSendPayload(message->svcData, &(message->svcDataId), dataLen);
|
||||
if (message->svcDataId == DATA_ID_NONE)
|
||||
{
|
||||
memset(message->svcData, 0xAA, dataLen);
|
||||
}
|
||||
|
||||
// Append CRC
|
||||
uint32_t crcVal;
|
||||
Status = L2_CrcGen((uint8_t *)message, msgLen - L2_MSG_CRC_LEN, L2_CRC8_31, &crcVal);
|
||||
if (Status != SUCCESS)
|
||||
{
|
||||
DBG_ERROR("Fail to generate CRC, error code: %" PRId32 ".\n", Status);
|
||||
return Status;
|
||||
}
|
||||
message->crc = crcVal;
|
||||
|
||||
// Logging...
|
||||
memcpy(instPtr->log.txLog.data, message, msgLen);
|
||||
instPtr->log.txLog.dataId = message->svcDataId;
|
||||
|
||||
|
||||
return SUCCESS;
|
||||
}
|
||||
|
||||
/*****************************************************************************/
|
||||
/**
|
||||
* @brief Packing Service message (2 Mbps)
|
||||
*
|
||||
* @param instPtr pointer to linklayer instance
|
||||
* type message type.
|
||||
* dstId destination node address.
|
||||
*
|
||||
* @return EPARAM
|
||||
* SUCCESS
|
||||
*
|
||||
******************************************************************************/
|
||||
static int32_t Service_Packing2M(L2_INST *instPtr, L2_FRAME_TYPE type, uint8_t dstId)
|
||||
{
|
||||
ASSERT(instPtr != NULL);
|
||||
int32_t Status;
|
||||
|
||||
Service_PackingCommon(instPtr, type, dstId);
|
||||
|
||||
L2_MSG_SVC_2M *message = (L2_MSG_SVC_2M *)(instPtr->sendPktData);
|
||||
size_t msgLen = sizeof(L2_MSG_SVC_2M);
|
||||
size_t dataLen = L2_PAYLOAD_LEN_SVC_2M;
|
||||
|
||||
// insert position and speed info
|
||||
SysMonitor_GetSysinfo(&(instPtr->sysInfo));
|
||||
message->srcPosX = instPtr->sysInfo.posX;
|
||||
message->srcPosY = instPtr->sysInfo.posY;
|
||||
message->srcPosZ = instPtr->sysInfo.posZ;
|
||||
message->srcSpeedX = instPtr->sysInfo.vX;
|
||||
message->srcSpeedY = instPtr->sysInfo.vY;
|
||||
message->srcSpeedZ = instPtr->sysInfo.vZ;
|
||||
|
||||
// Packing extra payload
|
||||
SysMonitor_GetSendPayload(message->svcData, &(message->svcDataId), dataLen);
|
||||
if (message->svcDataId == DATA_ID_NONE)
|
||||
{
|
||||
memset(message->svcData, 0xAA, dataLen);
|
||||
}
|
||||
|
||||
// Append CRC
|
||||
uint32_t crcVal;
|
||||
Status = L2_CrcGen((uint8_t *)message, msgLen - L2_MSG_CRC_LEN, L2_CRC8_31, &crcVal);
|
||||
if (Status != SUCCESS)
|
||||
{
|
||||
DBG_ERROR("Fail to generate CRC, error code: %" PRId32 ".\n", Status);
|
||||
return Status;
|
||||
}
|
||||
message->crc = crcVal;
|
||||
|
||||
// Logging...
|
||||
memcpy(instPtr->log.txLog.data, message, msgLen);
|
||||
instPtr->log.txLog.dataId = message->svcDataId;
|
||||
|
||||
|
||||
return SUCCESS;
|
||||
}
|
||||
|
||||
/*****************************************************************************/
|
||||
/**
|
||||
* @brief Handle service received packet. Send data up to MPC
|
||||
*
|
||||
* @param msg A point32_ter the the message frame.
|
||||
*
|
||||
* @return SUCCESS message has been successfully handled.
|
||||
* other error occurred.
|
||||
*
|
||||
******************************************************************************/
|
||||
int32_t L2_Service_PktHandler(L2_INST *instPtr, uint8_t *msg)
|
||||
{
|
||||
ASSERT(instPtr != NULL);
|
||||
ASSERT(msg != NULL);
|
||||
int32_t Status;
|
||||
#ifndef DATALINK_ENABLE_BERTEST
|
||||
L2_FRAME_TYPE frameType = L2_FRAME_NONE;
|
||||
L2_FrameGetChunk(msg, L2_MSG_TYPE, L2_MSG_TYPE_LEN, &frameType);
|
||||
|
||||
// retrieve basic info
|
||||
switch (frameType)
|
||||
{
|
||||
case L2_FRAME_DIS:
|
||||
if (L2_GetNodeType(instPtr) == L2_NODE_HEAD)
|
||||
{
|
||||
DBG_ERROR("Unexpected call from head node, only member Node can handle L2_FRAME_DIS.\n");
|
||||
return EUNEXP;
|
||||
}
|
||||
case L2_FRAME_MNG:
|
||||
case L2_FRAME_RTT:
|
||||
Status = Service_PktHandler128K(instPtr, msg);
|
||||
break;
|
||||
case L2_FRAME_SVC:
|
||||
switch (instPtr->config.serviceRate)
|
||||
{
|
||||
case L2_SVC_128K:
|
||||
Status = Service_PktHandler128K(instPtr, msg);
|
||||
break;
|
||||
case L2_SVC_512K:
|
||||
Status = Service_PktHandler512K(instPtr, msg);
|
||||
break;
|
||||
case L2_SVC_2M:
|
||||
Status = Service_PktHandler2M(instPtr, msg);
|
||||
break;
|
||||
default:
|
||||
DBG_ERROR("Unexpected service rate: %" PRIu8 ".\n", instPtr->config.serviceRate);
|
||||
return EPARAM;
|
||||
}
|
||||
break;
|
||||
default:
|
||||
DBG_ERROR("Unsupported frame type: %" PRIu8 ".\n", frameType);
|
||||
return EPARAM;
|
||||
}
|
||||
return Status;
|
||||
|
||||
#else
|
||||
uint32_t errorBits = 0;
|
||||
uint32_t pktLen = L2_GetSvcPacketLen(instPtr->config.serviceRate);
|
||||
|
||||
for (int i = 0; i < pktLen; i++)
|
||||
{
|
||||
errorBits += count_ones(msg[i] ^ testData[i]);
|
||||
}
|
||||
|
||||
instPtr->serviceFSM->bitError += errorBits;
|
||||
instPtr->serviceFSM->bitTotal += pktLen * 8;
|
||||
|
||||
// Calculate time deviation
|
||||
uint32_t frameNum = message->common.frameNum;
|
||||
uint32_t slotNum = (uint32_t)message->common.slotNum;
|
||||
Status = L2_GetTimeDeviation(instPtr, frameNum, slotNum, true, &(instPtr->log.timeDev));
|
||||
if (Status != SUCCESS)
|
||||
{
|
||||
DBG_ERROR("Failed to compute time deviation.\n")
|
||||
return Status;
|
||||
}
|
||||
// Implement a auto time sync.
|
||||
if (L2_GetNodeType(instPtr) != L2_NODE_HEAD)
|
||||
{
|
||||
// Time sync.
|
||||
L2_TimeAdjust(instPtr, &(instPtr->log.timeDev));
|
||||
}
|
||||
return SUCCESS;
|
||||
#endif
|
||||
}
|
||||
|
||||
/*****************************************************************************/
|
||||
/**
|
||||
* @brief Handle service received packet. Send data up to MPC. (128 kbps)
|
||||
*
|
||||
* @param msg A point32_ter the the message frame.
|
||||
*
|
||||
* @return SUCCESS message has been successfully handled.
|
||||
* other error occurred.
|
||||
*
|
||||
******************************************************************************/
|
||||
static int32_t Service_PktHandler128K(L2_INST *instPtr, uint8_t *msg)
|
||||
{
|
||||
ASSERT(instPtr != NULL);
|
||||
ASSERT(msg != NULL);
|
||||
int32_t Status;
|
||||
|
||||
L2_MSG_SVC_128K *message = (L2_MSG_SVC_128K *)msg;
|
||||
size_t msgLen = sizeof(L2_MSG_SVC_128K);
|
||||
size_t dataLen = L2_PAYLOAD_LEN_SVC_128K;
|
||||
|
||||
// Update node status.
|
||||
uint8_t nodeId = message->common.src;
|
||||
NODE_ENTRY *node;
|
||||
node = L2_GetNodeById(instPtr->nodeList, nodeId);
|
||||
if (node == NULL)
|
||||
{
|
||||
node = L2_AppendNode(instPtr->nodeList, nodeId, L2_NODE_MEMBER);
|
||||
if (node == NULL)
|
||||
{
|
||||
DBG_ERROR("Failed to append new node [id: %" PRId8 "]", nodeId);
|
||||
return ENOMEM;
|
||||
}
|
||||
}
|
||||
|
||||
// Update node info.
|
||||
node->nodeInfo.pos.x = message->srcPosX;
|
||||
node->nodeInfo.pos.y = message->srcPosY;
|
||||
node->nodeInfo.pos.z = message->srcPosZ;
|
||||
node->nodeInfo.speed.x = message->srcSpeedX;
|
||||
node->nodeInfo.speed.y = message->srcSpeedY;
|
||||
node->nodeInfo.speed.z = message->srcSpeedZ;
|
||||
node->nodeInfo.antIndex = Phy_ReceiveAntIndex(instPtr->config.phyInstPtr);
|
||||
|
||||
// Reset node ttl
|
||||
node->ttlCnt = 0;
|
||||
|
||||
// Calculate time deviation
|
||||
uint32_t frameNum = message->common.frameNum;
|
||||
uint32_t slotNum = (uint32_t)message->common.slotNum;
|
||||
Status = L2_GetTimeDeviation(instPtr, frameNum, slotNum, true, &(instPtr->log.timeDev));
|
||||
if (Status != SUCCESS)
|
||||
{
|
||||
DBG_ERROR("Failed to compute time deviation.\n")
|
||||
return Status;
|
||||
}
|
||||
|
||||
// Get data payload
|
||||
SysMonitor_UpdateRecvPayload(message->svcData, message->svcDataId, dataLen);
|
||||
|
||||
// Logging...
|
||||
memcpy(instPtr->log.rxLog.data, msg, msgLen);
|
||||
instPtr->log.rxLog.dataId = message->svcDataId;
|
||||
|
||||
#ifdef DATALINK_ENABLE_AUTOTIMESYNC
|
||||
// Implement a auto time sync.
|
||||
if (L2_GetNodeType(instPtr) != L2_NODE_HEAD)
|
||||
{
|
||||
// Time sync.
|
||||
L2_TimeAdjust(instPtr, &(instPtr->log.timeDev));
|
||||
}
|
||||
#endif
|
||||
|
||||
return Status;
|
||||
}
|
||||
|
||||
/*****************************************************************************/
|
||||
/**
|
||||
* @brief Handle service received packet. Send data up to MPC. (512 kbps)
|
||||
*
|
||||
* @param msg A point32_ter the the message frame.
|
||||
*
|
||||
* @return SUCCESS message has been successfully handled.
|
||||
* other error occurred.
|
||||
*
|
||||
******************************************************************************/
|
||||
static int32_t Service_PktHandler512K(L2_INST *instPtr, uint8_t *msg)
|
||||
{
|
||||
ASSERT(instPtr != NULL);
|
||||
ASSERT(msg != NULL);
|
||||
int32_t Status;
|
||||
|
||||
L2_MSG_SVC_512K *message = (L2_MSG_SVC_512K *)msg;
|
||||
size_t msgLen = sizeof(L2_MSG_SVC_512K);
|
||||
size_t dataLen = L2_PAYLOAD_LEN_SVC_512K;
|
||||
|
||||
// Update node status.
|
||||
uint8_t nodeId = message->common.src;
|
||||
NODE_ENTRY *node;
|
||||
node = L2_GetNodeById(instPtr->nodeList, nodeId);
|
||||
if (node == NULL)
|
||||
{
|
||||
node = L2_AppendNode(instPtr->nodeList, nodeId, L2_NODE_MEMBER);
|
||||
if (node == NULL)
|
||||
{
|
||||
DBG_ERROR("Failed to append new node [id: %" PRId8 "]", nodeId);
|
||||
return ENOMEM;
|
||||
}
|
||||
}
|
||||
|
||||
// Update node info.
|
||||
node->nodeInfo.pos.x = message->srcPosX;
|
||||
node->nodeInfo.pos.y = message->srcPosY;
|
||||
node->nodeInfo.pos.z = message->srcPosZ;
|
||||
node->nodeInfo.speed.x = message->srcSpeedX;
|
||||
node->nodeInfo.speed.y = message->srcSpeedY;
|
||||
node->nodeInfo.speed.z = message->srcSpeedZ;
|
||||
node->nodeInfo.antIndex = Phy_ReceiveAntIndex(instPtr->config.phyInstPtr);
|
||||
|
||||
// Reset node ttl
|
||||
node->ttlCnt = 0;
|
||||
|
||||
// Calculate time deviation
|
||||
uint32_t frameNum = message->common.frameNum;
|
||||
uint32_t slotNum = (uint32_t)message->common.slotNum;
|
||||
Status = L2_GetTimeDeviation(instPtr, frameNum, slotNum, true, &(instPtr->log.timeDev));
|
||||
if (Status != SUCCESS)
|
||||
{
|
||||
DBG_ERROR("Failed to compute time deviation.\n")
|
||||
return Status;
|
||||
}
|
||||
|
||||
// Get data payload
|
||||
SysMonitor_UpdateRecvPayload(message->svcData, message->svcDataId, dataLen);
|
||||
|
||||
// Logging...
|
||||
memcpy(instPtr->log.rxLog.data, msg, msgLen);
|
||||
instPtr->log.rxLog.dataId = message->svcDataId;
|
||||
|
||||
#ifdef DATALINK_ENABLE_AUTOTIMESYNC
|
||||
// Implement a auto time sync.
|
||||
if (L2_GetNodeType(instPtr) != L2_NODE_HEAD)
|
||||
{
|
||||
// Time sync.
|
||||
L2_TimeAdjust(instPtr, &(instPtr->log.timeDev));
|
||||
}
|
||||
#endif
|
||||
|
||||
return Status;
|
||||
}
|
||||
|
||||
/*****************************************************************************/
|
||||
/**
|
||||
* @brief Handle service received packet. Send data up to MPC. (2 Mbps)
|
||||
*
|
||||
* @param msg A point32_ter the the message frame.
|
||||
*
|
||||
* @return SUCCESS message has been successfully handled.
|
||||
* other error occurred.
|
||||
*
|
||||
******************************************************************************/
|
||||
static int32_t Service_PktHandler2M(L2_INST *instPtr, uint8_t *msg)
|
||||
{
|
||||
ASSERT(instPtr != NULL);
|
||||
ASSERT(msg != NULL);
|
||||
int32_t Status;
|
||||
|
||||
L2_MSG_SVC_2M *message = (L2_MSG_SVC_2M *)msg;
|
||||
size_t msgLen = sizeof(L2_MSG_SVC_2M);
|
||||
size_t dataLen = L2_PAYLOAD_LEN_SVC_2M;
|
||||
|
||||
// Update node status.
|
||||
uint8_t nodeId = message->common.src;
|
||||
NODE_ENTRY *node;
|
||||
node = L2_GetNodeById(instPtr->nodeList, nodeId);
|
||||
if (node == NULL)
|
||||
{
|
||||
node = L2_AppendNode(instPtr->nodeList, nodeId, L2_NODE_MEMBER);
|
||||
if (node == NULL)
|
||||
{
|
||||
DBG_ERROR("Failed to append new node [id: %" PRId8 "]", nodeId);
|
||||
return ENOMEM;
|
||||
}
|
||||
}
|
||||
|
||||
// Update node info.
|
||||
node->nodeInfo.pos.x = message->srcPosX;
|
||||
node->nodeInfo.pos.y = message->srcPosY;
|
||||
node->nodeInfo.pos.z = message->srcPosZ;
|
||||
node->nodeInfo.speed.x = message->srcSpeedX;
|
||||
node->nodeInfo.speed.y = message->srcSpeedY;
|
||||
node->nodeInfo.speed.z = message->srcSpeedZ;
|
||||
node->nodeInfo.antIndex = Phy_ReceiveAntIndex(instPtr->config.phyInstPtr);
|
||||
|
||||
// Reset node ttl
|
||||
node->ttlCnt = 0;
|
||||
|
||||
// Calculate time deviation
|
||||
uint32_t frameNum = message->common.frameNum;
|
||||
uint32_t slotNum = (uint32_t)message->common.slotNum;
|
||||
Status = L2_GetTimeDeviation(instPtr, frameNum, slotNum, true, &(instPtr->log.timeDev));
|
||||
if (Status != SUCCESS)
|
||||
{
|
||||
DBG_ERROR("Failed to compute time deviation.\n")
|
||||
return Status;
|
||||
}
|
||||
|
||||
// Get data payload
|
||||
SysMonitor_UpdateRecvPayload(message->svcData, message->svcDataId, dataLen);
|
||||
|
||||
// Logging...
|
||||
memcpy(instPtr->log.rxLog.data, msg, msgLen);
|
||||
instPtr->log.rxLog.dataId = message->svcDataId;
|
||||
|
||||
#ifdef DATALINK_ENABLE_AUTOTIMESYNC
|
||||
// Implement a auto time sync.
|
||||
if (L2_GetNodeType(instPtr) != L2_NODE_HEAD)
|
||||
{
|
||||
// Time sync.
|
||||
L2_TimeAdjust(instPtr, &(instPtr->log.timeDev));
|
||||
}
|
||||
#endif
|
||||
|
||||
return Status;
|
||||
}
|
||||
|
||||
/*****************************************************************************/
|
||||
/**
|
||||
* @brief Initialize service net FSM
|
||||
*
|
||||
* @param serviceFSM A pointer to service FSM instance.
|
||||
*
|
||||
* @return SUCCESS Initialize successfully.
|
||||
* Other Initialize failed.
|
||||
*
|
||||
******************************************************************************/
|
||||
int32_t L2_Service_Init(L2_SVC_FSM **svcFSMPtr)
|
||||
{
|
||||
ASSERT(svcFSMPtr != NULL);
|
||||
*svcFSMPtr = (L2_SVC_FSM *)malloc(sizeof(L2_SVC_FSM));
|
||||
if (*svcFSMPtr == NULL)
|
||||
{
|
||||
return ENOMEM;
|
||||
}
|
||||
L2_Service_Reset(*svcFSMPtr);
|
||||
return SUCCESS;
|
||||
}
|
||||
|
||||
/*****************************************************************************/
|
||||
/**
|
||||
* @brief Reset ServiceNet FSM
|
||||
*
|
||||
* @param scanFsmPtr a pointer to service net finite state machine
|
||||
*
|
||||
* @return NONE
|
||||
*
|
||||
******************************************************************************/
|
||||
void L2_Service_Reset(L2_SVC_FSM *svcFSMPtr)
|
||||
{
|
||||
ASSERT(svcFSMPtr != NULL);
|
||||
svcFSMPtr->txCnt = 0;
|
||||
svcFSMPtr->rxCnt = 0;
|
||||
svcFSMPtr->rxdCnt = 0;
|
||||
#ifdef DATALINK_ENABLE_BERTEST
|
||||
svcFSMPtr->bitTotal = 0;
|
||||
svcFSMPtr->bitError = 0;
|
||||
svcFSMPtr->lossRate = 0;
|
||||
svcFSMPtr->errorRate = 0;
|
||||
#endif
|
||||
}
|
||||
|
||||
/*****************************************************************************/
|
||||
/**
|
||||
* @brief Destroy ServiceNet FSM, free memory
|
||||
*
|
||||
* @param serviceFSM a pointer to service net finite state machine
|
||||
*
|
||||
* @return NONE
|
||||
*
|
||||
******************************************************************************/
|
||||
void L2_Service_Destroy(L2_SVC_FSM *serviceFSM)
|
||||
{
|
||||
free(serviceFSM);
|
||||
}
|
||||
|
||||
/*****************************************************************************/
|
||||
/**
|
||||
* @brief Service FSM Routine.
|
||||
*
|
||||
* @param instPtr
|
||||
* @param schedEntry
|
||||
*
|
||||
* @return Slot has been used.
|
||||
*
|
||||
******************************************************************************/
|
||||
int32_t L2_Service_Routine(L2_INST *instPtr, const L2_RTNE_TABLE_ENTRY *rtneEntry)
|
||||
{
|
||||
ASSERT(instPtr != NULL);
|
||||
DBG_INFO("-------- ServiceNet Routine --------\n");
|
||||
|
||||
if (rtneEntry->slotSeq != 0)
|
||||
{
|
||||
return SUCCESS; // skip
|
||||
}
|
||||
|
||||
PHY_INST *phyRef = instPtr->config.phyInstPtr;
|
||||
PHY_ANT_INDEX antIndex;
|
||||
L2_SVC_FSM *svcFSMPtr = instPtr->serviceFSM;
|
||||
L2_TOR_TYPE torState;
|
||||
const NODE_ENTRY *commNode;
|
||||
|
||||
switch (rtneEntry->slotType)
|
||||
{
|
||||
case L2_SLOT_DISTRIBUTE:
|
||||
if (L2_GetNodeType(instPtr) == L2_NODE_HEAD && rtneEntry->src == L2_GetNodeId(instPtr))
|
||||
{
|
||||
commNode = L2_GetNodeById(instPtr->nodeList, rtneEntry->dst);
|
||||
// antenna selection based on destination id in routing table
|
||||
antIndex = commNode->nodeInfo.antIndex;
|
||||
// head node transmits messages in distribute slot
|
||||
torState = L2_TOR_TX;
|
||||
|
||||
// reset memory
|
||||
memset(instPtr->sendPktData, 0xAA, L2_PACK_LEN_MAX);
|
||||
// pack message
|
||||
if (Service_Packing(instPtr, L2_FRAME_DIS, rtneEntry->dst) != SUCCESS)
|
||||
{
|
||||
return FAILURE;
|
||||
}
|
||||
}
|
||||
else if (L2_GetNodeType(instPtr) == L2_NODE_MEMBER && rtneEntry->dst == L2_GetNodeId(instPtr))
|
||||
{
|
||||
commNode = L2_GetNodeById(instPtr->nodeList, rtneEntry->src);
|
||||
// antenna selection based on node list
|
||||
antIndex = commNode->nodeInfo.antIndex;
|
||||
// member node receives message in advertise slot
|
||||
torState = L2_TOR_RX;
|
||||
}
|
||||
else
|
||||
{
|
||||
// otherwise suspend
|
||||
antIndex = PHY_ANT_INDEX_NONE;
|
||||
torState = L2_TOR_SUSPEND;
|
||||
}
|
||||
(void)Phy_SetAntIndex(instPtr->config.phyInstPtr, antIndex);
|
||||
(void)L2_SetupNextSlot(instPtr, torState, L2_FRAME_DIS);
|
||||
break;
|
||||
case L2_SLOT_MANAGE:
|
||||
if (rtneEntry->src == L2_GetNodeId(instPtr))
|
||||
{
|
||||
commNode = L2_GetNodeById(instPtr->nodeList, rtneEntry->dst);
|
||||
// antenna selection based on destination id in routing table
|
||||
antIndex = commNode->nodeInfo.antIndex;
|
||||
// head node transmits messages in distribute slot
|
||||
torState = L2_TOR_TX;
|
||||
|
||||
// reset memory
|
||||
memset(instPtr->sendPktData, 0xAA, L2_PACK_LEN_MAX);
|
||||
// pack message
|
||||
if (Service_Packing(instPtr, L2_FRAME_MNG, rtneEntry->dst) != SUCCESS)
|
||||
{
|
||||
return FAILURE;
|
||||
}
|
||||
}
|
||||
else if (rtneEntry->dst == L2_GetNodeId(instPtr))
|
||||
{
|
||||
commNode = L2_GetNodeById(instPtr->nodeList, rtneEntry->src);
|
||||
// antenna selection based on node list
|
||||
antIndex = commNode->nodeInfo.antIndex;
|
||||
// member node receives message in advertise slot
|
||||
torState = L2_TOR_RX;
|
||||
}
|
||||
else
|
||||
{
|
||||
// otherwise suspend
|
||||
antIndex = PHY_ANT_INDEX_NONE;
|
||||
torState = L2_TOR_SUSPEND;
|
||||
}
|
||||
(void)Phy_SetAntIndex(instPtr->config.phyInstPtr, antIndex);
|
||||
(void)L2_SetupNextSlot(instPtr, torState, L2_FRAME_MNG);
|
||||
break;
|
||||
case L2_SLOT_RTT:
|
||||
if (rtneEntry->src == L2_GetNodeId(instPtr))
|
||||
{
|
||||
commNode = L2_GetNodeById(instPtr->nodeList, rtneEntry->dst);
|
||||
// antenna selection based on destination id in routing table
|
||||
antIndex = commNode->nodeInfo.antIndex;
|
||||
// head node transmits messages in distribute slot
|
||||
torState = L2_TOR_TX;
|
||||
|
||||
// reset memory
|
||||
memset(instPtr->sendPktData, 0xAA, L2_PACK_LEN_MAX);
|
||||
// pack message
|
||||
if (Service_Packing(instPtr, L2_FRAME_RTT, rtneEntry->dst) != SUCCESS)
|
||||
{
|
||||
return FAILURE;
|
||||
}
|
||||
}
|
||||
else if (rtneEntry->dst == L2_GetNodeId(instPtr))
|
||||
{
|
||||
commNode = L2_GetNodeById(instPtr->nodeList, rtneEntry->src);
|
||||
// antenna selection based on node list
|
||||
antIndex = commNode->nodeInfo.antIndex;
|
||||
// member node receives message in advertise slot
|
||||
torState = L2_TOR_RX;
|
||||
}
|
||||
else
|
||||
{
|
||||
// otherwise suspend
|
||||
antIndex = PHY_ANT_INDEX_NONE;
|
||||
torState = L2_TOR_SUSPEND;
|
||||
}
|
||||
(void)Phy_SetAntIndex(instPtr->config.phyInstPtr, antIndex);
|
||||
(void)L2_SetupNextSlot(instPtr, torState, L2_FRAME_RTT);
|
||||
break;
|
||||
case L2_SLOT_SERVICE:
|
||||
if (rtneEntry->src == L2_GetNodeId(instPtr))
|
||||
{
|
||||
commNode = L2_GetNodeById(instPtr->nodeList, rtneEntry->dst);
|
||||
// antenna selection based on destination id in routing table
|
||||
antIndex = commNode->nodeInfo.antIndex;
|
||||
// head node transmits messages in distribute slot
|
||||
torState = L2_TOR_TX;
|
||||
|
||||
svcFSMPtr->txCnt++;
|
||||
|
||||
// reset memory
|
||||
memset(instPtr->sendPktData, 0xAA, L2_PACK_LEN_MAX);
|
||||
#ifndef DATALINK_ENABLE_BERTEST
|
||||
// pack message
|
||||
if (Service_Packing(instPtr, L2_FRAME_SVC, rtneEntry->dst) != SUCCESS)
|
||||
{
|
||||
return FAILURE;
|
||||
}
|
||||
#else
|
||||
size_t msgLen = L2_GetSvcPacketLen(instPtr->config.serviceRate);
|
||||
memcpy(instPtr->sendPktData, testData, msgLen);
|
||||
#endif
|
||||
}
|
||||
else if (rtneEntry->dst == L2_GetNodeId(instPtr))
|
||||
{
|
||||
commNode = L2_GetNodeById(instPtr->nodeList, rtneEntry->src);
|
||||
// antenna selection based on node list
|
||||
antIndex = commNode->nodeInfo.antIndex;
|
||||
// member node receives message in advertise slot
|
||||
torState = L2_TOR_RX;
|
||||
|
||||
svcFSMPtr->rxCnt++;
|
||||
}
|
||||
else
|
||||
{
|
||||
// otherwise suspend
|
||||
antIndex = PHY_ANT_INDEX_NONE;
|
||||
torState = L2_TOR_SUSPEND;
|
||||
}
|
||||
(void)Phy_SetAntIndex(instPtr->config.phyInstPtr, antIndex);
|
||||
(void)L2_SetupNextSlot(instPtr, torState, L2_FRAME_SVC);
|
||||
break;
|
||||
default:
|
||||
DBG_ERROR("UNKNOWN slot type [%" PRIu8 "].\n", rtneEntry->slotType);
|
||||
return FAILURE;
|
||||
}
|
||||
#ifdef DATALINK_ENABLE_BERTEST
|
||||
uint32_t pktLoss;
|
||||
pktLoss = svcFSMPtr->rxCnt - svcFSMPtr->rxdCnt;
|
||||
svcFSMPtr->lossRate = (svcFSMPtr->rxCnt == 0) ? 0 : (double)pktLoss / (double)svcFSMPtr->rxCnt;
|
||||
svcFSMPtr->errorRate = (svcFSMPtr->bitTotal == 0) ? 0 : (double)svcFSMPtr->bitError / (double)svcFSMPtr->bitTotal;
|
||||
DBG_PRINT("Packet Loss Rate -- Total: %lu, Loss: %lu.\n", instPtr->log.svcRxCnt, pktLoss);
|
||||
DBG_PRINT("Bit Error Rate -- Total:%lu, Error:%lu.\n", svcFSMPtr->bitTotal, svcFSMPtr->bitError);
|
||||
#endif
|
||||
return SUCCESS;
|
||||
}
|
||||
|
||||
/** @} LinkLayer_Service*/
|
||||
/** @} LinkLayer*/
|
||||
@@ -0,0 +1,105 @@
|
||||
/*****************************************************************************/
|
||||
/**
|
||||
* @file time_sync.c
|
||||
* @author FanHao ([email protected])
|
||||
* @brief DF Linklayer time sync implementation file.
|
||||
* @version 0.0
|
||||
* @date 2023-05-06
|
||||
*
|
||||
* @copyright Copyright(c) 2024 Beijing Institute of Technology.\n
|
||||
* Lab of Communication and Network
|
||||
*
|
||||
******************************************************************************/
|
||||
/**
|
||||
* @addtogroup LinkLayer
|
||||
* @{
|
||||
*/
|
||||
|
||||
/**
|
||||
* @addtogroup LinkLayer_TimeSync
|
||||
* @{
|
||||
*/
|
||||
/***************************** Include Files *********************************/
|
||||
#include <string.h>
|
||||
#include <stdlib.h>
|
||||
#include "link_layer.h"
|
||||
#include "../phy_layer/phy_layer.h"
|
||||
#include "error.h"
|
||||
#include "common.h"
|
||||
|
||||
/************************** Constant Definitions *****************************/
|
||||
/***************** Macros (Inline Functions) Definitions *********************/
|
||||
/**************************** Type Definitions *******************************/
|
||||
/************************** Variable Definitions *****************************/
|
||||
/************************** Function Prototypes ******************************/
|
||||
|
||||
/*****************************************************************************/
|
||||
/**
|
||||
* @brief Get time deviation to the specified frame and slot number.
|
||||
*
|
||||
* @param instPtr a pointer to linklayer layer instance
|
||||
* @param frameNum
|
||||
* @param slotNum
|
||||
* @param relative
|
||||
* @param timeDev
|
||||
*
|
||||
* @return SUCCESS
|
||||
* EIGNORE there is no received frame
|
||||
*
|
||||
******************************************************************************/
|
||||
int32_t L2_GetTimeDeviation(L2_INST *instPtr, uint32_t frameNum, uint32_t slotNum, bool relative, int64_t *timeDev)
|
||||
{
|
||||
ASSERT(instPtr != NULL);
|
||||
ASSERT(timeDev != NULL);
|
||||
L1_FRAME_TYPE lastRecvdFrame;
|
||||
PHY_INST *phyRef = instPtr->config.phyInstPtr;
|
||||
lastRecvdFrame = Phy_GetLastRecvFrameType(phyRef);
|
||||
if (lastRecvdFrame == L1_FRAME_NONE)
|
||||
{
|
||||
DBG_ERROR("Invalid TimeAlign operation, no frame was received.");
|
||||
return EIGNORE;
|
||||
}
|
||||
|
||||
DBG_INFO("Starting TimeAlign operation...\n");
|
||||
// read nextSendTime from physical layer
|
||||
uint64_t currTime, expectTime;
|
||||
TIME_WORD timeStamp;
|
||||
expectTime = ((uint64_t)(frameNum * L2_SLOTS_PER_FRAME + slotNum)) * TIME_FRAC_MAX;
|
||||
expectTime += Phy_GetTimeComp(lastRecvdFrame);
|
||||
|
||||
if (relative)
|
||||
{
|
||||
expectTime += (uint64_t)instPtr->netStartTime * L2_SLOTS_PER_FRAME * TIME_FRAC_MAX;
|
||||
}
|
||||
|
||||
Phy_GetLastReceivedFrameSyncTimeStamp(phyRef, &timeStamp);
|
||||
currTime = (uint64_t)timeStamp.frac_part;
|
||||
currTime += ((uint64_t)timeStamp.int_part) * TIME_FRAC_MAX;
|
||||
|
||||
*timeDev = (int64_t)(expectTime - currTime);
|
||||
return SUCCESS;
|
||||
}
|
||||
|
||||
/*****************************************************************************/
|
||||
/**
|
||||
* @brief Implement time adjustment by applying time deviation.
|
||||
*
|
||||
* @param instPtr a pointer to linklayer layer instance
|
||||
* @param timeDev
|
||||
*
|
||||
* @return NONE
|
||||
*
|
||||
******************************************************************************/
|
||||
void L2_TimeAdjust(L2_INST *instPtr, const int64_t *timeDev)
|
||||
{
|
||||
ASSERT(instPtr != NULL);
|
||||
ASSERT(timeDev != NULL);
|
||||
TIME_WORD slotTimerAdjmet;
|
||||
slotTimerAdjmet.int_part = (uint32_t)(*timeDev / TIME_FRAC_MAX);
|
||||
slotTimerAdjmet.frac_part = (uint32_t)(*timeDev % TIME_FRAC_MAX);
|
||||
TimeCtrl_TimeAdjust(instPtr->config.tmcInstPtr, &slotTimerAdjmet, TM_ADJ_ADD);
|
||||
}
|
||||
|
||||
/** @} LinkLayer_TimeSync*/
|
||||
|
||||
/** @} LinkLayer*/
|
||||
Reference in New Issue
Block a user