josh/hj_datalink
1
0
Fork 0
forked from 0xha0/df_datalink
Code Issues Pull Requests Releases Activity

unified indentation

Browse Source
This commit is contained in:
bit-cyber-fh
2024-03-28 15:25:58 +08:00
parent 8779c9855e
commit 16d1ad30a7
14 changed files with 1019 additions and 1030 deletions
Download Patch File Download Diff File Expand all files Collapse all files
df_datalink_sp/src/include/circ_queue.h
+21 -22
View File
@@ -29,71 +29,70 @@
#include <stddef.h> #include <stddef.h>
/** /**
* opaque structure * @struct CircularQueueInst
*/ */
typedef struct typedef struct
{ {
void **data; void **data;
volatile uint32_t nelts; /**< # elements */ volatile uint32_t nelts; /**< # elements */
uint32_t in; /**< next empty location */ uint32_t in; /**< next empty location */
uint32_t out; /**< next filled location */ uint32_t out; /**< next filled location */
uint32_t bounds; /**< max size of queue */ uint32_t bounds; /**< max size of queue */
} CIRC_QUEUE; } CIRC_QUEUE;
/** /**
* create a FIFO queue * @brief create a FIFO queue
* @param queue The new queue * @param queue The new queue
* @param queue_capacity maximum size of the queue * @param queue_capacity maximum size of the queue
* @param a pool to allocate queue from * @param a pool to allocate queue from
*/ */
bool CircQueueCreate(CIRC_QUEUE **queue, uint32_t queue_capacity); bool CircQueueCreate(CIRC_QUEUE **queue, uint32_t queue_capacity);
/** free queue memory -- call after destroy /**
* @brief free queue memory -- call after destroy
* @note we do not free in destroy() to follow pthreads convention * @note we do not free in destroy() to follow pthreads convention
*/ */
void CircQueueFree(CIRC_QUEUE *queue); void CircQueueFree(CIRC_QUEUE *queue);
/** /**
* Detects when the CIRC_QUEUE is full. This utility function is expected * @brief Detects when the CIRC_QUEUE is full. This utility function is expected
* to be called from within critical sections, and is not threadsafe. * to be called from within critical sections, and is not threadsafe.
*/ */
bool isCircQueueFull(CIRC_QUEUE *queue); bool isCircQueueFull(CIRC_QUEUE *queue);
/** /**
* Detects when the CIRC_QUEUE is empty. This utility function is expected * @brief Detects when the CIRC_QUEUE is empty. This utility function is expected
* to be called from within critical sections, and is not threadsafe. * to be called from within critical sections, and is not threadsafe.
*/ */
bool isCircQueueEmpty(CIRC_QUEUE *queue); bool isCircQueueEmpty(CIRC_QUEUE *queue);
/** /**
* push/add an object to the queue, blocking if the queue is already full * @brief push/add an object to the queue, blocking if the queue is already full
* *
* @param queue the queue * @param queue the queue
* @param data the data * @param data the data
* @returns RPA_EINTR the blocking was interrupted (try again) * @return RPA_EINTR the blocking was interrupted (try again)
* @returns RPA_EOF the queue has been terminated * @return RPA_EOF the queue has been terminated
* @returns RPA_SUCCESS on a successful push * @return RPA_SUCCESS on a successful push
*/ */
bool CircQueuePush(CIRC_QUEUE *queue, void *data); bool CircQueuePush(CIRC_QUEUE *queue, void *data);
/** /**
* pop/get an object from the queue, blocking if the queue is already empty * @brief pop/get an object from the queue, blocking if the queue is already empty
* *
* @param queue the queue * @param queue the queue
* @param data the data * @param data the data
* @returns RPA_EINTR the blocking was interrupted (try again) * @return RPA_EINTR the blocking was interrupted (try again)
* @returns RPA_EOF if the queue has been terminated * @return RPA_EOF if the queue has been terminated
* @returns RPA_SUCCESS on a successful pop * @return RPA_SUCCESS on a successful pop
*/ */
bool CircQueuePop(CIRC_QUEUE *queue, void **data); bool CircQueuePop(CIRC_QUEUE *queue, void **data);
/** /**
* returns the size of the queue. * @brief returns the size of the queue.
* *
* @warning this is not threadsafe, and is intended for reporting/monitoring
* of the queue.
* @param queue the queue * @param queue the queue
* @returns the size of the queue * @return the size of the queue
*/ */
uint32_t CircQueueSize(CIRC_QUEUE *queue); uint32_t CircQueueSize(CIRC_QUEUE *queue);
df_datalink_sp/src/include/clist.h
+73 -69
View File
@@ -32,100 +32,104 @@ extern "C"
#include <stdlib.h> #include <stdlib.h>
#include <stdint.h> #include <stdint.h>
/* /**
* clist_t iterator direction. * @enum cListDirection
*/ * @brief clist_t iterator direction.
*/
typedef enum typedef enum
{ {
LIST_HEAD, LIST_HEAD,
LIST_TAIL LIST_TAIL
} clist_direction_t; } clist_direction_t;
/* /**
* clist_t node struct. * @struct cListNode
*/ * @brief clist_t node struct.
*/
typedef struct clist_node typedef struct clist_node
{ {
struct clist_node *prev; struct clist_node *prev;
struct clist_node *next; struct clist_node *next;
void *val; void *val;
} clist_node_t; } clist_node_t;
/* /**
* clist_t struct. * @struct cListInst
*/ * @brief clist_t struct.
*/
typedef struct typedef struct
{ {
clist_node_t *head; clist_node_t *head;
clist_node_t *tail; clist_node_t *tail;
uint32_t len; uint32_t len;
void (*free)(void *val); void (*free)(void *val);
int8_t (*compare)(void *a, void *b); int8_t (*compare)(void *a, void *b);
} clist_t; } clist_t;
/* /**
* clist_t iterator struct. * @struct cListIter
*/ * @brief clist_t iterator struct.
*/
typedef struct typedef struct
{ {
clist_node_t *next; clist_node_t *next;
clist_direction_t direction; clist_direction_t direction;
} clist_iterator_t; } clist_iterator_t;
// Node prototypes. // Node prototypes.
clist_node_t * clist_node_t *
clist_node_new(void *val); clist_node_new(void *val);
// clist_t prototypes. // clist_t prototypes.
clist_t * clist_t *
clist_new(void); clist_new(void);
clist_node_t * clist_node_t *
clist_rpush(clist_t *self, clist_node_t *node); clist_rpush(clist_t *self, clist_node_t *node);
clist_node_t * clist_node_t *
clist_lpush(clist_t *self, clist_node_t *node); clist_lpush(clist_t *self, clist_node_t *node);
clist_node_t * clist_node_t *
clist_find(clist_t *self, void *val); clist_find(clist_t *self, void *val);
clist_node_t * clist_node_t *
clist_at(clist_t *self, int index); clist_at(clist_t *self, int index);
clist_node_t * clist_node_t *
clist_rpop(clist_t *self); clist_rpop(clist_t *self);
clist_node_t * clist_node_t *
clist_lpop(clist_t *self); clist_lpop(clist_t *self);
void void
clist_remove(clist_t *self, clist_node_t *node); clist_remove(clist_t *self, clist_node_t *node);
void void
clist_clear(clist_t *self); clist_clear(clist_t *self);
void void
clist_destroy(clist_t *self); clist_destroy(clist_t *self);
// clist_t iterator prototypes. // clist_t iterator prototypes.
clist_iterator_t * clist_iterator_t *
clist_iterator_new(clist_t *list, clist_direction_t direction); clist_iterator_new(clist_t *list, clist_direction_t direction);
clist_iterator_t * clist_iterator_t *
clist_iterator_new_from_node(clist_node_t *node, clist_direction_t direction); clist_iterator_new_from_node(clist_node_t *node, clist_direction_t direction);
clist_node_t * clist_node_t *
clist_iterator_next(clist_iterator_t *self); clist_iterator_next(clist_iterator_t *self);
void void
clist_iterator_destroy(clist_iterator_t *self); clist_iterator_destroy(clist_iterator_t *self);
#ifdef __cplusplus #ifdef __cplusplus
} }
df_datalink_sp/src/networking/link_layer/geometry/coord.c
+94 -94
View File
@@ -37,132 +37,132 @@ const COORD COORD_Z_AXIS = {0.0, 0.0, 1.0};
/** @brief Subtracts two coordinate vectors. */ /** @brief Subtracts two coordinate vectors. */
COORD Coord_Sub(COORD a, COORD b) COORD Coord_Sub(COORD a, COORD b)
{ {
COORD ret; COORD ret;
ret.x = a.x - b.x; ret.x = a.x - b.x;
ret.y = a.y - b.y; ret.y = a.y - b.y;
ret.z = a.z - b.z; ret.z = a.z - b.z;
return ret; return ret;
} }
/** @brief Adds two coordinate vectors. */ /** @brief Adds two coordinate vectors. */
COORD Coord_Add(COORD a, COORD b) COORD Coord_Add(COORD a, COORD b)
{ {
COORD ret; COORD ret;
ret.x = a.x + b.x; ret.x = a.x + b.x;
ret.y = a.y + b.y; ret.y = a.y + b.y;
ret.z = a.z + b.z; ret.z = a.z + b.z;
return ret; return ret;
} }
/** @brief Multiplies a coordinate vector by a real number. */ /** @brief Multiplies a coordinate vector by a real number. */
void Coord_ScalarMult(COORD *a, double t) void Coord_ScalarMult(COORD *a, double t)
{ {
a->x *= t; a->x *= t;
a->y *= t; a->y *= t;
a->z *= t; a->z *= t;
} }
/** @brief Multiplies a coordinate vector by a real number. */ /** @brief Multiplies a coordinate vector by a real number. */
COORD Coord_GetScalarMult(COORD a, double t) COORD Coord_GetScalarMult(COORD a, double t)
{ {
COORD ret; COORD ret;
ret.x = a.x * t; ret.x = a.x * t;
ret.y = a.y * t; ret.y = a.y * t;
ret.z = a.z * t; ret.z = a.z * t;
return ret; return ret;
} }
/** @brief Divides a coordinate vector by a real number. */ /** @brief Divides a coordinate vector by a real number. */
void Coord_ScalarDiv(COORD *a, double t) void Coord_ScalarDiv(COORD *a, double t)
{ {
a->x /= t; a->x /= t;
a->y /= t; a->y /= t;
a->z /= t; a->z /= t;
} }
/** @brief Divides a coordinate vector by a real number. */ /** @brief Divides a coordinate vector by a real number. */
COORD Coord_GetScalarDiv(COORD a, double t) COORD Coord_GetScalarDiv(COORD a, double t)
{ {
COORD ret; COORD ret;
ret.x = a.x / t; ret.x = a.x / t;
ret.y = a.y / t; ret.y = a.y / t;
ret.z = a.z / t; ret.z = a.z / t;
return ret; return ret;
} }
/** @brief Dot product. */ /** @brief Dot product. */
inline double Coord_Dot(COORD a, COORD b) inline double Coord_Dot(COORD a, COORD b)
{ {
return (a.x * b.x + a.y * b.y + a.z * b.z); return (a.x * b.x + a.y * b.y + a.z * b.z);
} }
/** @brief Cross product. */ /** @brief Cross product. */
COORD Coord_Cross(COORD a, COORD b) COORD Coord_Cross(COORD a, COORD b)
{ {
COORD ret; COORD ret;
ret.x = a.y * b.z - a.z * b.y; ret.x = a.y * b.z - a.z * b.y;
ret.y = a.z * b.x - a.x * b.z; ret.y = a.z * b.x - a.x * b.z;
ret.z = a.x * b.y - a.y * b.x; ret.z = a.x * b.y - a.y * b.x;
return ret; return ret;
} }
/** @brief Multiplies two coordinate vector in elementwise. */ /** @brief Multiplies two coordinate vector in elementwise. */
COORD Coord_ElementwiseMult(COORD a, COORD b) COORD Coord_ElementwiseMult(COORD a, COORD b)
{ {
COORD ret; COORD ret;
ret.x = a.x * b.x; ret.x = a.x * b.x;
ret.y = a.y * b.y; ret.y = a.y * b.y;
ret.z = a.z * b.z; ret.z = a.z * b.z;
return ret; return ret;
} }
/** @brief Divides two coordinate vector in elementwise. */ /** @brief Divides two coordinate vector in elementwise. */
COORD Coord_ElementwiseDiv(COORD a, COORD b) COORD Coord_ElementwiseDiv(COORD a, COORD b)
{ {
COORD ret; COORD ret;
ret.x = a.x / b.x; ret.x = a.x / b.x;
ret.y = a.y / b.y; ret.y = a.y / b.y;
ret.z = a.z / b.z; ret.z = a.z / b.z;
return ret; return ret;
} }
/** @brief Calculate a coordinate vector length^2 (omits calling square root). */ /** @brief Calculate a coordinate vector length^2 (omits calling square root). */
double Coord_SquareLength(COORD a) double Coord_SquareLength(COORD a)
{ {
return (a.x * a.x + a.y * a.y + a.z * a.z); return (a.x * a.x + a.y * a.y + a.z * a.z);
} }
/** @brief Calculate a coordinate vector length. */ /** @brief Calculate a coordinate vector length. */
double Coord_Length(COORD a) double Coord_Length(COORD a)
{ {
return sqrt(Coord_SquareLength(a)); return sqrt(Coord_SquareLength(a));
} }
/** @brief Calculate two coordinates' distance. */ /** @brief Calculate two coordinates' distance. */
double Coord_Distance(COORD a, COORD b) double Coord_Distance(COORD a, COORD b)
{ {
return Coord_Length(Coord_Sub(a, b)); return Coord_Length(Coord_Sub(a, b));
} }
/** @brief Normalize coordinate vector. */ /** @brief Normalize coordinate vector. */
void Coord_Normalize(COORD *a) void Coord_Normalize(COORD *a)
{ {
Coord_ScalarDiv(a, Coord_Length(*a)); Coord_ScalarDiv(a, Coord_Length(*a));
} }
/** @brief Normalize coordinate vector. */ /** @brief Normalize coordinate vector. */
COORD Coord_GetNormalized(COORD a) COORD Coord_GetNormalized(COORD a)
{ {
Coord_Normalize(&a); Coord_Normalize(&a);
return a; return a;
} }
/** @brief Returns the angle between the two vectors. */ /** @brief Returns the angle between the two vectors. */
double Coord_Angle(COORD a, COORD b) double Coord_Angle(COORD a, COORD b)
{ {
Coord_Normalize(&a); Coord_Normalize(&a);
Coord_Normalize(&b); Coord_Normalize(&b);
return acos(Coord_Dot(a, b)); return acos(Coord_Dot(a, b));
} }
// Additional Coordinates convert // Additional Coordinates convert
@@ -170,64 +170,64 @@ double Coord_Angle(COORD a, COORD b)
/** @brief Convert coordinate from Launch coordinate system to ECEF. */ /** @brief Convert coordinate from Launch coordinate system to ECEF. */
COORD Coord_Launch2Ecef(COORD o, COORD p, double A, double B, double lambda) COORD Coord_Launch2Ecef(COORD o, COORD p, double A, double B, double lambda)
{ {
COORD ret; COORD ret;
double sinA, cosA, sinB, cosB, sinL, cosL; double sinA, cosA, sinB, cosB, sinL, cosL;
double S11, S12, S13, S21, S22, S23, S31, S32, S33; double S11, S12, S13, S21, S22, S23, S31, S32, S33;
sinA = sin(A); sinA = sin(A);
cosA = cos(A); cosA = cos(A);
sinB = sin(B); sinB = sin(B);
cosB = cos(B); cosB = cos(B);
sinL = sin(lambda); sinL = sin(lambda);
cosL = cos(lambda); cosL = cos(lambda);
S11 = -sinA * sinL - cosA * sinB * cosL; S11 = -sinA * sinL - cosA * sinB * cosL;
S12 = cosB * cosL; S12 = cosB * cosL;
S13 = -cosA * sinL + sinA * sinB * cosL; S13 = -cosA * sinL + sinA * sinB * cosL;
S21 = sinA * cosL - cosA * sinB * sinL; S21 = sinA * cosL - cosA * sinB * sinL;
S22 = cosB * sinL; S22 = cosB * sinL;
S23 = cosA * cosL + sinA * sinB * sinL; S23 = cosA * cosL + sinA * sinB * sinL;
S31 = cosA * cosB; S31 = cosA * cosB;
S32 = sinB; S32 = sinB;
S33 = -sinA * cosB; S33 = -sinA * cosB;
ret.x = p.x * (S11 + S21 + S31) + o.x; ret.x = p.x * (S11 + S21 + S31) + o.x;
ret.y = p.y * (S12 + S22 + S32) + o.y; ret.y = p.y * (S12 + S22 + S32) + o.y;
ret.z = p.z * (S13 + S23 + S33) + o.z; ret.z = p.z * (S13 + S23 + S33) + o.z;
return ret; return ret;
} }
/** @brief Convert coordinate from launch coordinate system to Inertia coordinate system. */ /** @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 Coord_Launch2Inertia(COORD o, COORD p, double A, double B, double we, double t)
{ {
COORD ret; COORD ret;
double OmgX, OmgY, OmgZ, sinwet, coswet; double OmgX, OmgY, OmgZ, sinwet, coswet;
double M11, M12, M13, M21, M22, M23, M31, M32, M33; double M11, M12, M13, M21, M22, M23, M31, M32, M33;
OmgX = cos(A) * cos(B); OmgX = cos(A) * cos(B);
OmgY = sin(B); OmgY = sin(B);
OmgZ = -sin(A) * cos(B); OmgZ = -sin(A) * cos(B);
sinwet = sin(we * t); sinwet = sin(we * t);
coswet = cos(we * t); coswet = cos(we * t);
M11 = 1 - (1 - OmgX * OmgX) * (1 - coswet); M11 = 1 - (1 - OmgX * OmgX) * (1 - coswet);
M12 = OmgX * OmgY * (1 - coswet) + OmgZ * sinwet; M12 = OmgX * OmgY * (1 - coswet) + OmgZ * sinwet;
M13 = OmgX * OmgZ * (1 - coswet) - OmgY * sinwet; M13 = OmgX * OmgZ * (1 - coswet) - OmgY * sinwet;
M21 = OmgY * OmgX * (1 - coswet) - OmgZ * sinwet; M21 = OmgY * OmgX * (1 - coswet) - OmgZ * sinwet;
M22 = 1 - (1 - OmgY * OmgY) * (1 - coswet); M22 = 1 - (1 - OmgY * OmgY) * (1 - coswet);
M23 = OmgY * OmgZ * (1 - coswet) + OmgX * sinwet; M23 = OmgY * OmgZ * (1 - coswet) + OmgX * sinwet;
M31 = OmgZ * OmgX * (1 - coswet) + OmgY * sinwet; M31 = OmgZ * OmgX * (1 - coswet) + OmgY * sinwet;
M32 = OmgZ * OmgY * (1 - coswet) - OmgX * sinwet; M32 = OmgZ * OmgY * (1 - coswet) - OmgX * sinwet;
M33 = 1 - (1 - OmgZ * OmgZ) * (1 - coswet); M33 = 1 - (1 - OmgZ * OmgZ) * (1 - coswet);
ret.x = p.x * (M11 + M21 + M31) + o.x; ret.x = p.x * (M11 + M21 + M31) + o.x;
ret.y = p.y * (M12 + M22 + M32) + o.y; ret.y = p.y * (M12 + M22 + M32) + o.y;
ret.z = p.z * (M13 + M23 + M33) + o.z; ret.z = p.z * (M13 + M23 + M33) + o.z;
return ret; return ret;
} }
/** @} LinkLayerGeometry_Coord */ /** @} LinkLayerGeometry_Coord */
df_datalink_sp/src/networking/link_layer/geometry/coord.h
+3 -3
View File
@@ -33,9 +33,9 @@
*/ */
typedef struct typedef struct
{ {
double x; double x;
double y; double y;
double z; double z;
} COORD; } COORD;
extern const COORD COORD_NIL; extern const COORD COORD_NIL;
df_datalink_sp/src/networking/link_layer/geometry/eulerangles.c
+20 -20
View File
@@ -34,51 +34,51 @@ const EULERANGLES EULERANGLES_ZERO = {0.0, 0.0, 0.0};
/** @brief Add two EulerAngles. */ /** @brief Add two EulerAngles. */
EULERANGLES EulerAngles_Add(EULERANGLES e1, EULERANGLES e2) EULERANGLES EulerAngles_Add(EULERANGLES e1, EULERANGLES e2)
{ {
EULERANGLES ret; EULERANGLES ret;
ret.alpha = e1.alpha + e2.alpha; ret.alpha = e1.alpha + e2.alpha;
ret.beta = e1.beta + e2.beta; ret.beta = e1.beta + e2.beta;
ret.gamma = e1.gamma + e2.gamma; ret.gamma = e1.gamma + e2.gamma;
return ret; return ret;
} }
/** @brief Subtracts two EulerAngles. */ /** @brief Subtracts two EulerAngles. */
EULERANGLES EulerAngles_Sub(EULERANGLES e1, EULERANGLES e2) EULERANGLES EulerAngles_Sub(EULERANGLES e1, EULERANGLES e2)
{ {
EULERANGLES ret; EULERANGLES ret;
ret.alpha = e1.alpha - e2.alpha; ret.alpha = e1.alpha - e2.alpha;
ret.beta = e1.beta - e2.beta; ret.beta = e1.beta - e2.beta;
ret.gamma = e1.gamma - e2.gamma; ret.gamma = e1.gamma - e2.gamma;
return ret; return ret;
} }
/** @brief Multiplies EulerAngles by a real number. */ /** @brief Multiplies EulerAngles by a real number. */
void EulerAngles_ScalarMult(EULERANGLES *e, double f) void EulerAngles_ScalarMult(EULERANGLES *e, double f)
{ {
e->alpha *= f; e->alpha *= f;
e->beta *= f; e->beta *= f;
e->gamma *= f; e->gamma *= f;
} }
/** @brief Multiplies EulerAngles by a real number. */ /** @brief Multiplies EulerAngles by a real number. */
EULERANGLES EulerAngles_GetScalarMult(EULERANGLES e, double f) EULERANGLES EulerAngles_GetScalarMult(EULERANGLES e, double f)
{ {
EulerAngles_ScalarMult(&e, f); EulerAngles_ScalarMult(&e, f);
return e; return e;
} }
/** @brief Normalize EulerAngles. */ /** @brief Normalize EulerAngles. */
void EulerAngles_Normalize(EULERANGLES *e) void EulerAngles_Normalize(EULERANGLES *e)
{ {
e->alpha = fmod(e->alpha, 2 * EUL_PI); e->alpha = fmod(e->alpha, 2 * EUL_PI);
e->beta = fmod(e->beta, 2 * EUL_PI); e->beta = fmod(e->beta, 2 * EUL_PI);
e->gamma = fmod(e->gamma, 2 * EUL_PI); e->gamma = fmod(e->gamma, 2 * EUL_PI);
} }
/** @brief Normalize EulerAngles. */ /** @brief Normalize EulerAngles. */
EULERANGLES EulerAngles_GetNormalize(EULERANGLES e) EULERANGLES EulerAngles_GetNormalize(EULERANGLES e)
{ {
EulerAngles_Normalize(&e); EulerAngles_Normalize(&e);
return e; return e;
} }
/** @} LinkLayerGeometry_EulerAngles */ /** @} LinkLayerGeometry_EulerAngles */
df_datalink_sp/src/networking/link_layer/geometry/quaternion.c
+277 -277
View File
@@ -34,14 +34,14 @@
typedef union typedef union
{ {
float value; float value;
uint32_t raw; uint32_t raw;
} SOFT_FLOAT; } SOFT_FLOAT;
typedef union typedef union
{ {
double value; double value;
uint64_t raw; uint64_t raw;
} SOFT_DOUBLE; } SOFT_DOUBLE;
const SOFT_FLOAT EPS_FLOAT = {.raw = 0x34000000}; const SOFT_FLOAT EPS_FLOAT = {.raw = 0x34000000};
@@ -53,424 +53,424 @@ const QUATERNION QUATERNION_NIL = {NAN, NAN, NAN, NAN};
/** @brief Subtracts two quaternions. */ /** @brief Subtracts two quaternions. */
QUATERNION Quaternion_Sub(QUATERNION q1, QUATERNION q2) QUATERNION Quaternion_Sub(QUATERNION q1, QUATERNION q2)
{ {
QUATERNION ret; QUATERNION ret;
ret.s = q1.s - q2.s; ret.s = q1.s - q2.s;
ret.i = q1.i - q2.i; ret.i = q1.i - q2.i;
ret.j = q1.j - q2.j; ret.j = q1.j - q2.j;
ret.k = q1.k - q2.k; ret.k = q1.k - q2.k;
return ret; return ret;
} }
/** @brief Adds two quaternions. */ /** @brief Adds two quaternions. */
QUATERNION Quaternion_Add(QUATERNION q1, QUATERNION q2) QUATERNION Quaternion_Add(QUATERNION q1, QUATERNION q2)
{ {
QUATERNION ret; QUATERNION ret;
ret.s = q1.s + q2.s; ret.s = q1.s + q2.s;
ret.i = q1.i + q2.i; ret.i = q1.i + q2.i;
ret.j = q1.j + q2.j; ret.j = q1.j + q2.j;
ret.k = q1.k + q2.k; ret.k = q1.k + q2.k;
return ret; return ret;
} }
/** @brief Multiplies a quaternion by a real number. */ /** @brief Multiplies a quaternion by a real number. */
void Quaternion_ScalarMult(QUATERNION *q, double t) void Quaternion_ScalarMult(QUATERNION *q, double t)
{ {
q->s *= t; q->s *= t;
q->i *= t; q->i *= t;
q->j *= t; q->j *= t;
q->k *= t; q->k *= t;
} }
/** @brief Multiplies a quaternion by a real number. */ /** @brief Multiplies a quaternion by a real number. */
QUATERNION Quaternion_GetScalarMult(QUATERNION a, double t) QUATERNION Quaternion_GetScalarMult(QUATERNION a, double t)
{ {
Quaternion_ScalarMult(&a, t); Quaternion_ScalarMult(&a, t);
return a; return a;
} }
/** @brief Divides a quaternion by a real number. */ /** @brief Divides a quaternion by a real number. */
void Quaternion_ScalarDiv(QUATERNION *q, double t) void Quaternion_ScalarDiv(QUATERNION *q, double t)
{ {
q->s /= t; q->s /= t;
q->i /= t; q->i /= t;
q->j /= t; q->j /= t;
q->k /= t; q->k /= t;
} }
/** @brief Divides a quaternion by a real number. */ /** @brief Divides a quaternion by a real number. */
QUATERNION Quaternion_GetScalarDiv(QUATERNION q, double t) QUATERNION Quaternion_GetScalarDiv(QUATERNION q, double t)
{ {
Quaternion_ScalarDiv(&q, t); Quaternion_ScalarDiv(&q, t);
return q; return q;
} }
/** @brief Multiplies two quaternions. */ /** @brief Multiplies two quaternions. */
QUATERNION Quaternion_Mult(QUATERNION q1, QUATERNION q2) QUATERNION Quaternion_Mult(QUATERNION q1, QUATERNION q2)
{ {
QUATERNION ret; QUATERNION ret;
ret.s = q1.s * q2.s - (q1.i * q2.i + q1.j * q2.j + q1.k * q2.k); 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.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.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; ret.k = q1.i * q2.j - q1.j * q2.i + q1.s * q2.k + q1.k * q2.s;
return ret; return ret;
} }
/** @brief Divides two quaternions. */ /** @brief Divides two quaternions. */
QUATERNION Quaternion_Div(QUATERNION q1, QUATERNION q2) QUATERNION Quaternion_Div(QUATERNION q1, QUATERNION q2)
{ {
Quaternion_Inverse(&q2); Quaternion_Inverse(&q2);
return Quaternion_Mult(q1, q2); return Quaternion_Mult(q1, q2);
} }
/** @brief Dot product. */ /** @brief Dot product. */
inline double Quaternion_Dot(QUATERNION q1, QUATERNION q2) 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); 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). */ /** @brief Calculate a quaternion length^2 (omits calling square root). */
inline double Quaternion_SquareLength(QUATERNION q) inline double Quaternion_SquareLength(QUATERNION q)
{ {
return (q.s * q.s + q.i * q.i + q.j * q.j + q.k * q.k); return (q.s * q.s + q.i * q.i + q.j * q.j + q.k * q.k);
} }
/** @brief Calculate a quaternion length. */ /** @brief Calculate a quaternion length. */
inline double Quaternion_Length(QUATERNION q) inline double Quaternion_Length(QUATERNION q)
{ {
return sqrt(Quaternion_SquareLength(q)); return sqrt(Quaternion_SquareLength(q));
} }
/** @brief Normalize quaternion. */ /** @brief Normalize quaternion. */
void Quaternion_Normalize(QUATERNION *q) void Quaternion_Normalize(QUATERNION *q)
{ {
Quaternion_ScalarDiv(q, Quaternion_Length(*q)); Quaternion_ScalarDiv(q, Quaternion_Length(*q));
} }
/** @brief Normalize quaternion. */ /** @brief Normalize quaternion. */
QUATERNION Quaternion_GetNormalized(QUATERNION q) QUATERNION Quaternion_GetNormalized(QUATERNION q)
{ {
Quaternion_Normalize(&q); Quaternion_Normalize(&q);
return q; return q;
} }
/** @brief Conjugated quaternion. */ /** @brief Conjugated quaternion. */
void Quaternion_Conjugated(QUATERNION *q) void Quaternion_Conjugated(QUATERNION *q)
{ {
q->i = -q->i; q->i = -q->i;
q->j = -q->j; q->j = -q->j;
q->k = -q->k; q->k = -q->k;
} }
/** @brief Conjugated quaternion. */ /** @brief Conjugated quaternion. */
QUATERNION Quaternion_GetConjugated(QUATERNION q) QUATERNION Quaternion_GetConjugated(QUATERNION q)
{ {
Quaternion_Conjugated(&q); Quaternion_Conjugated(&q);
return q; return q;
} }
/** @brief Inverse quaternion. */ /** @brief Inverse quaternion. */
void Quaternion_Inverse(QUATERNION *q) void Quaternion_Inverse(QUATERNION *q)
{ {
Quaternion_Conjugated(q); Quaternion_Conjugated(q);
Quaternion_ScalarDiv(q, Quaternion_SquareLength(*q)); Quaternion_ScalarDiv(q, Quaternion_SquareLength(*q));
} }
/** @brief Inverse quaternion. */ /** @brief Inverse quaternion. */
QUATERNION Quaternion_GeInverse(QUATERNION q) QUATERNION Quaternion_GeInverse(QUATERNION q)
{ {
Quaternion_Inverse(&q); Quaternion_Inverse(&q);
return q; return q;
} }
inline double Quaternion_GetRotationAngle(QUATERNION q) inline double Quaternion_GetRotationAngle(QUATERNION q)
{ {
return (acos(q.s) * 2); return (acos(q.s) * 2);
} }
/** @brief Get axis of quaternion. */ /** @brief Get axis of quaternion. */
COORD Quaternion_GetRotationAxis(QUATERNION q) COORD Quaternion_GetRotationAxis(QUATERNION q)
{ {
double sin_theta_inv; double sin_theta_inv;
COORD ret; COORD ret;
sin_theta_inv = 1.0 / sin(acos(q.s)); sin_theta_inv = 1.0 / sin(acos(q.s));
ret.x = q.i * sin_theta_inv; ret.x = q.i * sin_theta_inv;
ret.y = q.j * sin_theta_inv; ret.y = q.j * sin_theta_inv;
ret.z = q.k * sin_theta_inv; ret.z = q.k * sin_theta_inv;
return ret; return ret;
} }
/** @brief Get the axis and rotate angle of a unit Quaternion */ /** @brief Get the axis and rotate angle of a unit Quaternion */
void Quaternion_GetRotationAxisAndAngle(QUATERNION q, COORD *axis, double *angle) void Quaternion_GetRotationAxisAndAngle(QUATERNION q, COORD *axis, double *angle)
{ {
double sin_theta_inv; double sin_theta_inv;
*angle = acos(q.s); *angle = acos(q.s);
// pre-compute to save time // pre-compute to save time
sin_theta_inv = 1.0 / sin(*angle); sin_theta_inv = 1.0 / sin(*angle);
// now the vector // now the vector
axis->x = q.i * sin_theta_inv; axis->x = q.i * sin_theta_inv;
axis->y = q.j * sin_theta_inv; axis->y = q.j * sin_theta_inv;
axis->z = q.k * sin_theta_inv; axis->z = q.k * sin_theta_inv;
*angle = (*angle) * 2; *angle = (*angle) * 2;
} }
/** @brief Decompose the rotation on to 2 parts. */ /** @brief Decompose the rotation on to 2 parts. */
void Quaternion_GetSwingAndTwist(QUATERNION q, COORD axis, QUATERNION *swing, QUATERNION *twist) void Quaternion_GetSwingAndTwist(QUATERNION q, COORD axis, QUATERNION *swing, QUATERNION *twist)
{ {
double p; double p;
COORD v; COORD v;
v.x = q.i; v.x = q.i;
v.y = q.j; v.y = q.j;
v.z = q.k; v.z = q.k;
// return projection v1 on to v2 (parallel component) // return projection v1 on to v2 (parallel component)
p = Coord_Dot(v, axis); p = Coord_Dot(v, axis);
v = Coord_GetScalarMult(axis, p); v = Coord_GetScalarMult(axis, p);
twist->s = q.s; twist->s = q.s;
twist->i = v.x; twist->i = v.x;
twist->j = v.y; twist->j = v.y;
twist->k = v.z; twist->k = v.z;
Quaternion_Normalize(twist); Quaternion_Normalize(twist);
*swing = Quaternion_Mult(q, Quaternion_GetConjugated(*twist)); *swing = Quaternion_Mult(q, Quaternion_GetConjugated(*twist));
} }
/** @brief rotates v by this quaternion (quaternion must be unit) */ /** @brief rotates v by this quaternion (quaternion must be unit) */
COORD Quaternion_Rotate(QUATERNION q, COORD v) COORD Quaternion_Rotate(QUATERNION q, COORD v)
{ {
QUATERNION temp; QUATERNION temp;
COORD ret; COORD ret;
temp.s = 0; temp.s = 0;
temp.i = v.x; temp.i = v.x;
temp.j = v.y; temp.j = v.y;
temp.k = v.z; temp.k = v.z;
temp = Quaternion_Mult(q, temp); temp = Quaternion_Mult(q, temp);
Quaternion_Conjugated(&q); Quaternion_Conjugated(&q);
temp = Quaternion_Mult(temp, q); temp = Quaternion_Mult(temp, q);
ret.x = temp.i; ret.x = temp.i;
ret.y = temp.j; ret.y = temp.j;
ret.z = temp.k; ret.z = temp.k;
return ret; return ret;
} }
/** @brief returns the euler angles from a rotation Quaternion */ /** @brief returns the euler angles from a rotation Quaternion */
EULERANGLES Quaternion_ToEulerAngles(QUATERNION q, EULERANGLES_TYPE type) EULERANGLES Quaternion_ToEulerAngles(QUATERNION q, EULERANGLES_TYPE type)
{ {
EULERANGLES ret; EULERANGLES ret;
switch (type) switch (type)
{ {
case EULERANGLES_ZYX: case EULERANGLES_ZYX:
{ {
/* Covert form matlab 2023a */ /* Covert form matlab 2023a */
// Cap all inputs to asin to 1, since values >1 produce complex // Cap all inputs to asin to 1, since values >1 produce complex
// results // results
// Since the quaternion is of unit length, this should never happen, // Since the quaternion is of unit length, this should never happen,
// but some code generation configuration seem to hit this edge case // but some code generation configuration seem to hit this edge case
// under some circumstances. // under some circumstances.
double asinValue; double asinValue;
bool mask1, mask2; bool mask1, mask2;
asinValue = -2.0 * (q.i * q.k - q.s * q.j); asinValue = -2.0 * (q.i * q.k - q.s * q.j);
mask1 = asinValue >= 1.0 - 10 * EPS_DOUBLE.value; mask1 = asinValue >= 1.0 - 10 * EPS_DOUBLE.value;
mask2 = asinValue <= -1.0 + 10 * EPS_DOUBLE.value; mask2 = asinValue <= -1.0 + 10 * EPS_DOUBLE.value;
if (mask1) if (mask1)
asinValue = 1; asinValue = 1;
if (mask2) if (mask2)
asinValue = -1; asinValue = -1;
if (mask1 || mask2) if (mask1 || mask2)
{ {
ret.alpha = (asinValue > 0) ? (-2.0 * atan2(q.i, q.s)) : (2.0 * atan2(q.i, q.s)); ret.alpha = (asinValue > 0) ? (-2.0 * atan2(q.i, q.s)) : (2.0 * atan2(q.i, q.s));
ret.gamma = 0; ret.gamma = 0;
} }
else 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.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.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); ret.beta = asin(asinValue);
break; break;
} }
case EULERANGLES_ZYZ: case EULERANGLES_ZYZ:
{ {
// TODO; // TODO;
ret = EULERANGLES_NIL; ret = EULERANGLES_NIL;
break; break;
} }
case EULERANGLES_XYZ: case EULERANGLES_XYZ:
{ {
/* Covert form matlab 2023a */ /* Covert form matlab 2023a */
// Prevent singularities as done in ZYX case // Prevent singularities as done in ZYX case
// Alternative to rotm2eul(quat2rotm(q), 'XYZ') with fewer // Alternative to rotm2eul(quat2rotm(q), 'XYZ') with fewer
// operations // operations
// sin(y) = R13 = 2 * (qx*qz + qy*qw) // sin(y) = R13 = 2 * (qx*qz + qy*qw)
// tan(x) = sin(x) / cos(x) = -R23 / R33 // tan(x) = sin(x) / cos(x) = -R23 / R33
// = -2 * (qy*qz - qx*qw) / (1 - 2*(qx^2 + qy^2)) // = -2 * (qy*qz - qx*qw) / (1 - 2*(qx^2 + qy^2))
// = -2 * (qy*qz - qx*qw) / (qw^2 - qx^2 - qy^2 + qz^2) // = -2 * (qy*qz - qx*qw) / (qw^2 - qx^2 - qy^2 + qz^2)
// tan(z) = sin(z) / cos(z) = -R12 / R11 // tan(z) = sin(z) / cos(z) = -R12 / R11
// = -2 * (qx*qy - qz*qw) / (1 - 2*(qy^2 + qz^2)) // = -2 * (qx*qy - qz*qw) / (1 - 2*(qy^2 + qz^2))
// = -2 * (qy*qz - qx*qw) / (qw^2 + qx^2 - qy^2 - qz^2) // = -2 * (qy*qz - qx*qw) / (qw^2 + qx^2 - qy^2 - qz^2)
double asinValue; double asinValue;
bool mask1, mask2; bool mask1, mask2;
asinValue = 2.0 * (q.i * q.k + q.s * q.j); asinValue = 2.0 * (q.i * q.k + q.s * q.j);
mask1 = asinValue >= 1.0 - 10 * EPS_DOUBLE.value; mask1 = asinValue >= 1.0 - 10 * EPS_DOUBLE.value;
mask2 = asinValue <= -1.0 + 10 * EPS_DOUBLE.value; mask2 = asinValue <= -1.0 + 10 * EPS_DOUBLE.value;
if (mask1) if (mask1)
asinValue = 1; asinValue = 1;
if (mask2) if (mask2)
asinValue = -1; asinValue = -1;
if (mask1 || mask2) if (mask1 || mask2)
{ {
ret.alpha = 0; ret.alpha = 0;
ret.gamma = (asinValue > 0) ? (2.0 * atan2(q.i, q.s)) : (-2.0 * atan2(q.i, q.s)); ret.gamma = (asinValue > 0) ? (2.0 * atan2(q.i, q.s)) : (-2.0 * atan2(q.i, q.s));
} }
else 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.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.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); ret.beta = asin(asinValue);
break; break;
} }
default: default:
ret = EULERANGLES_NIL; ret = EULERANGLES_NIL;
} }
return ret; return ret;
} }
/** @brief returns the rotation Quaternion from a euler angles */ /** @brief returns the rotation Quaternion from a euler angles */
QUATERNION Quaternion_FromeEulerAngles(EULERANGLES e, EULERANGLES_TYPE type) QUATERNION Quaternion_FromeEulerAngles(EULERANGLES e, EULERANGLES_TYPE type)
{ {
QUATERNION ret; QUATERNION ret;
double cosx, sinx, cosy, siny, cosz, sinz; double cosx, sinx, cosy, siny, cosz, sinz;
cosx = cos(e.alpha * 0.5); cosx = cos(e.alpha * 0.5);
sinx = sin(e.alpha * 0.5); sinx = sin(e.alpha * 0.5);
cosy = cos(e.beta * 0.5); cosy = cos(e.beta * 0.5);
siny = sin(e.beta * 0.5); siny = sin(e.beta * 0.5);
cosz = cos(e.gamma * 0.5); cosz = cos(e.gamma * 0.5);
sinz = sin(e.gamma * 0.5); sinz = sin(e.gamma * 0.5);
switch (type) switch (type)
{ {
case EULERANGLES_ZYX: case EULERANGLES_ZYX:
ret.s = cosx * cosy * cosz + sinx * siny * sinz; ret.s = cosx * cosy * cosz + sinx * siny * sinz;
ret.i = cosx * cosy * sinz - sinx * siny * cosz; ret.i = cosx * cosy * sinz - sinx * siny * cosz;
ret.j = cosx * siny * cosz + sinx * cosy * sinz; ret.j = cosx * siny * cosz + sinx * cosy * sinz;
ret.k = sinx * cosy * cosz - cosx * siny * sinz; ret.k = sinx * cosy * cosz - cosx * siny * sinz;
break; break;
case EULERANGLES_ZYZ: case EULERANGLES_ZYZ:
ret.s = cosx * cosy * cosz - sinx * cosy * sinz; ret.s = cosx * cosy * cosz - sinx * cosy * sinz;
ret.i = cosx * siny * sinz - sinx * siny * cosz; ret.i = cosx * siny * sinz - sinx * siny * cosz;
ret.j = cosx * siny * cosz + sinx * siny * sinz; ret.j = cosx * siny * cosz + sinx * siny * sinz;
ret.k = sinx * cosy * cosz + cosx * cosy * sinz; ret.k = sinx * cosy * cosz + cosx * cosy * sinz;
break; break;
case EULERANGLES_XYZ: case EULERANGLES_XYZ:
ret.s = cosx * cosy * cosz - sinx * siny * sinz; ret.s = cosx * cosy * cosz - sinx * siny * sinz;
ret.i = sinx * cosy * cosz + cosx * siny * sinz; ret.i = sinx * cosy * cosz + cosx * siny * sinz;
ret.j = -sinx * cosy * sinz + cosx * siny * cosz; ret.j = -sinx * cosy * sinz + cosx * siny * cosz;
ret.k = cosx * cosy * sinz + sinx * siny * cosz; ret.k = cosx * cosy * sinz + sinx * siny * cosz;
break; break;
default: default:
ret = QUATERNION_NIL; ret = QUATERNION_NIL;
break; break;
} }
return ret; return ret;
} }
/** @brief adapted from https://svn.code.sf.net/p/irrlicht/code/trunk/include/quaternion.h */ /** @brief adapted from https://svn.code.sf.net/p/irrlicht/code/trunk/include/quaternion.h */
QUATERNION Quaternion_RotationFromTo(COORD from, COORD to) QUATERNION Quaternion_RotationFromTo(COORD from, COORD to)
{ {
// Based on Stan Melax's article in Game Programming Gems // Based on Stan Melax's article in Game Programming Gems
COORD axis; COORD axis;
QUATERNION ret; QUATERNION ret;
double c, s; double c, s;
Coord_Normalize(&from); Coord_Normalize(&from);
Coord_Normalize(&to); Coord_Normalize(&to);
c = Coord_Dot(from, to); c = Coord_Dot(from, to);
if (c >= 1.0) if (c >= 1.0)
{ // If dot == 1, vectors are the same { // If dot == 1, vectors are the same
return QUATERNION_IDENTITY; // identity return QUATERNION_IDENTITY; // identity
} }
else if (c <= -1.0) else if (c <= -1.0)
{ // exactly opposite { // exactly opposite
axis = Coord_Cross(COORD_X_AXIS, from); axis = Coord_Cross(COORD_X_AXIS, from);
if (Coord_Length(axis) == 0) if (Coord_Length(axis) == 0)
axis = Coord_Cross(COORD_Y_AXIS, from); axis = Coord_Cross(COORD_Y_AXIS, from);
// same as fromAxisAngle(axis, PI).normalize(); // same as fromAxisAngle(axis, PI).normalize();
ret.s = 0; ret.s = 0;
ret.i = axis.x; ret.i = axis.x;
ret.j = axis.y; ret.j = axis.y;
ret.k = axis.z; ret.k = axis.z;
Quaternion_Normalize(&ret); Quaternion_Normalize(&ret);
return ret; return ret;
} }
s = sqrt((1 + c) * 2); s = sqrt((1 + c) * 2);
axis = Coord_Cross(from, to); axis = Coord_Cross(from, to);
Coord_ScalarDiv(&axis, s); Coord_ScalarDiv(&axis, s);
ret.s = s * 0.5; ret.s = s * 0.5;
ret.i = axis.x; ret.i = axis.x;
ret.j = axis.y; ret.j = axis.y;
ret.k = axis.z; ret.k = axis.z;
Quaternion_Normalize(&ret); Quaternion_Normalize(&ret);
return ret; return ret;
} }
/** @brief linear quaternion interpolation */ /** @brief linear quaternion interpolation */
QUATERNION Quaternion_Lerp(QUATERNION q1, QUATERNION q2, double t) QUATERNION Quaternion_Lerp(QUATERNION q1, QUATERNION q2, double t)
{ {
QUATERNION ret; QUATERNION ret;
Quaternion_ScalarMult(&q1, 1 - t); Quaternion_ScalarMult(&q1, 1 - t);
Quaternion_ScalarMult(&q1, t); Quaternion_ScalarMult(&q1, t);
ret = Quaternion_Add(q1, q2); ret = Quaternion_Add(q1, q2);
Quaternion_Normalize(&ret); Quaternion_Normalize(&ret);
return ret; return ret;
} }
/** @brief spherical linear interpolation */ /** @brief spherical linear interpolation */
QUATERNION Quaternion_Slerp(QUATERNION q1, QUATERNION q2, double t) QUATERNION Quaternion_Slerp(QUATERNION q1, QUATERNION q2, double t)
{ {
double dot; double dot;
QUATERNION q3, ret; QUATERNION q3, ret;
dot = Quaternion_Dot(q1, q2); dot = Quaternion_Dot(q1, q2);
/* dot = cos(theta) /* dot = cos(theta)
if (dot < 0), q1 and q2 are more than 90 degrees apart, if (dot < 0), q1 and q2 are more than 90 degrees apart,
so we can invert one to reduce spinning */ so we can invert one to reduce spinning */
if (dot < 0) if (dot < 0)
{ {
dot = -dot; dot = -dot;
q3.s = -q2.s; q3.s = -q2.s;
q3.i = -q2.i; q3.i = -q2.i;
q3.j = -q2.j; q3.j = -q2.j;
q3.k = -q2.k; q3.k = -q2.k;
} }
else else
{ {
q3 = q2; q3 = q2;
} }
if (dot < 0.95) if (dot < 0.95)
{ {
double angle = acos(dot); double angle = acos(dot);
Quaternion_ScalarMult(&q1, sin(angle * (1 - t))); Quaternion_ScalarMult(&q1, sin(angle * (1 - t)));
Quaternion_ScalarMult(&q3, sin(angle * t)); Quaternion_ScalarMult(&q3, sin(angle * t));
ret = Quaternion_Add(q1, q3); ret = Quaternion_Add(q1, q3);
Quaternion_ScalarDiv(&ret, sin(angle)); Quaternion_ScalarDiv(&ret, sin(angle));
return ret; return ret;
} }
// if the angle is small, use linear interpolation // if the angle is small, use linear interpolation
else else
{ {
return Quaternion_Lerp(q1, q3, t); return Quaternion_Lerp(q1, q3, t);
} }
} }
/** @} LinkLayerGeometry_Quaternion */ /** @} LinkLayerGeometry_Quaternion */
df_datalink_sp/src/networking/link_layer/geometry/quaternion.h
+10 -10
View File
@@ -37,10 +37,10 @@
*/ */
typedef struct typedef struct
{ {
double s; double s;
double i; double i;
double j; double j;
double k; double k;
} QUATERNION; } QUATERNION;
extern const QUATERNION QUATERNION_IDENTITY; extern const QUATERNION QUATERNION_IDENTITY;
@@ -107,13 +107,13 @@ COORD Quaternion_GetRotationAxis(QUATERNION q);
void Quaternion_GetRotationAxisAndAngle(QUATERNION q, COORD *axis, double *angle); void Quaternion_GetRotationAxisAndAngle(QUATERNION q, COORD *axis, double *angle);
/** @brief Decompose the rotation on to 2 parts. /** @brief Decompose the rotation on to 2 parts.
1. Twist - rotation around the "direction" vector 1. Twist - rotation around the "direction" vector
2. Swing - rotation around axis that is perpendicular to "direction" vector 2. Swing - rotation around axis that is perpendicular to "direction" vector
The rotation can be composed back by rotation = swing * twist The rotation can be composed back by rotation = swing * twist
has singularity in case of swing_rotation close to 180 degrees rotation. 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 if the input quaternion is of non-unit length, the outputs are non-unit as well
otherwise, outputs are both unit otherwise, outputs are both unit
*/ */
void Quaternion_GetSwingAndTwist(QUATERNION q, COORD axis, QUATERNION *swing, QUATERNION *twist); void Quaternion_GetSwingAndTwist(QUATERNION q, COORD axis, QUATERNION *swing, QUATERNION *twist);
df_datalink_sp/src/networking/phy_layer/phy_antenna.c
-14
View File
@@ -71,7 +71,6 @@
// Pair by regex // Pair by regex
// Regex: (([0-9]+, ){8}) // Regex: (([0-9]+, ){8})
const uint16_t hopPattern_2KBPS_20MS[L1_FRAME_2KBPS_20MS_HOPS] = const uint16_t hopPattern_2KBPS_20MS[L1_FRAME_2KBPS_20MS_HOPS] =
{ {
1500 1500
@@ -97,12 +96,6 @@ const uint16_t hopPattern_10KBPS_10MS[L1_FRAME_10KBPS_10MS_HOPS] =
1500 1500
}; };
// uint16_t hopPattern_10KBPS_20MS[L1_FRAME_10KBPS_20MS_HOPS] =
//{
// 1500, 1600, 1700, 1800, 1900, 1400, 1300, 1200,
// 1100, 1000, 900
// };
const uint16_t hopPattern_10KBPS_20MS[L1_FRAME_10KBPS_20MS_HOPS] = const uint16_t hopPattern_10KBPS_20MS[L1_FRAME_10KBPS_20MS_HOPS] =
{ {
1500, 1500, 1500, 1500, 1500, 1500, 1500, 1500, 1500, 1500, 1500, 1500, 1500, 1500, 1500, 1500,
@@ -115,13 +108,6 @@ const uint16_t hopPattern_40KBPS_10MS[L1_FRAME_40KBPS_10MS_HOPS] =
1500 1500
}; };
// uint16_t hopPattern_40KBPS_20MS[L1_FRAME_40KBPS_20MS_HOPS] =
//{
// 1600, 1650, 1700, 1750, 1800, 1850, 1900, 1550,
// 1500, 1450, 1400, 1350, 1300, 1250, 1200, 1150,
// 1100, 1050, 1000
// };
const uint16_t hopPattern_40KBPS_20MS[L1_FRAME_40KBPS_20MS_HOPS] = const uint16_t hopPattern_40KBPS_20MS[L1_FRAME_40KBPS_20MS_HOPS] =
{ {
1700, 1800, 1900, 2000, 1700, 1700, 1700, 1700, 1700, 1800, 1900, 2000, 1700, 1700, 1700, 1700,
df_datalink_sp/src/system_monitor.h
+1 -1
View File
@@ -128,7 +128,7 @@ typedef struct __attribute__((packed))
} SYSBUSLOG; } SYSBUSLOG;
/** /**
* @name Linklayer_Log * @struct Linklayer_Log
* @brief Linklayer Log struct holds the Linklayer log * @brief Linklayer Log struct holds the Linklayer log
*/ */
typedef struct __attribute__((packed)) typedef struct __attribute__((packed))
df_datalink_sp/src/systembus/systembus.h
+13 -13
View File
@@ -34,9 +34,9 @@
*/ */
typedef struct typedef struct
{ {
uintptr_t baseAddr; ///< base address in memory uintptr_t baseAddr; ///< base address in memory
uint32_t txQueueMax; ///< maximum tx queue length uint32_t txQueueMax; ///< maximum tx queue length
uint32_t rxQueueMax; ///< maximum rx queue length uint32_t rxQueueMax; ///< maximum rx queue length
} SYSBUS_CONFIG; } SYSBUS_CONFIG;
/** /**
@@ -45,16 +45,16 @@ typedef struct
*/ */
typedef struct typedef struct
{ {
SYSBUS_CONFIG config; ///< SystemBus initialization parameters SYSBUS_CONFIG config; ///< SystemBus initialization parameters
SYSBUSLOG sysbusLog; ///< SystemBus log SYSBUSLOG sysbusLog; ///< SystemBus log
LINKLOG linkLog; ///< Link layer log LINKLOG linkLog; ///< Link layer log
PHYLOG phyLog; ///< Physical layer log PHYLOG phyLog; ///< Physical layer log
HWSTATE hwState; ///< Hardware status HWSTATE hwState; ///< Hardware status
SYSBUS_UART *uartInst; ///< physical uart controller instance SYSBUS_UART *uartInst; ///< physical uart controller instance
CIRC_QUEUE *txQueue; ///< tx queue CIRC_QUEUE *txQueue; ///< tx queue
CIRC_QUEUE *rxQueue; ///< rx queue CIRC_QUEUE *rxQueue; ///< rx queue
bool isRxDone; ///< tx done indicator bool isRxDone; ///< tx done indicator
bool isTxDone; ///< rx done indicator bool isTxDone; ///< rx done indicator
} SYSBUS; } SYSBUS;
/************************** Function Prototypes ******************************/ /************************** Function Prototypes ******************************/
df_datalink_sp/src/util/circ_queue.c
+57 -57
View File
@@ -28,22 +28,22 @@
inline bool isCircQueueFull(CIRC_QUEUE *queue) inline bool isCircQueueFull(CIRC_QUEUE *queue)
{ {
ASSERT(queue != NULL); ASSERT(queue != NULL);
return ((queue)->nelts == (queue)->bounds); return ((queue)->nelts == (queue)->bounds);
} }
inline bool isCircQueueEmpty(CIRC_QUEUE *queue) inline bool isCircQueueEmpty(CIRC_QUEUE *queue)
{ {
ASSERT(queue != NULL); ASSERT(queue != NULL);
return ((queue)->nelts == 0); return ((queue)->nelts == 0);
} }
void CircQueueFree(CIRC_QUEUE *queue) void CircQueueFree(CIRC_QUEUE *queue)
{ {
ASSERT(queue != NULL); ASSERT(queue != NULL);
if (queue->data) if (queue->data)
free(queue->data); free(queue->data);
free(queue); free(queue);
} }
/** /**
@@ -51,30 +51,30 @@ void CircQueueFree(CIRC_QUEUE *queue)
*/ */
bool CircQueueCreate(CIRC_QUEUE **q, uint32_t queueCapacity) bool CircQueueCreate(CIRC_QUEUE **q, uint32_t queueCapacity)
{ {
ASSERT(q != NULL); ASSERT(q != NULL);
CIRC_QUEUE *queue; CIRC_QUEUE *queue;
queue = malloc(sizeof(CIRC_QUEUE)); queue = malloc(sizeof(CIRC_QUEUE));
if (!queue) if (!queue)
{ {
return false; return false;
} }
*q = queue; *q = queue;
memset(queue, 0, sizeof(CIRC_QUEUE)); memset(queue, 0, sizeof(CIRC_QUEUE));
/* Set all the data in the queue to NULL */ /* Set all the data in the queue to NULL */
queue->data = malloc(queueCapacity * sizeof(void *)); queue->data = malloc(queueCapacity * sizeof(void *));
if (queue->data == NULL) if (queue->data == NULL)
{ {
goto error; goto error;
} }
queue->bounds = queueCapacity; queue->bounds = queueCapacity;
queue->nelts = 0; queue->nelts = 0;
queue->in = 0; queue->in = 0;
queue->out = 0; queue->out = 0;
return true; return true;
error: error:
free(queue); free(queue);
return false; return false;
} }
/** /**
@@ -82,22 +82,22 @@ error:
*/ */
bool CircQueuePush(CIRC_QUEUE *queue, void *data) bool CircQueuePush(CIRC_QUEUE *queue, void *data)
{ {
ASSERT(queue != NULL); ASSERT(queue != NULL);
if (isCircQueueFull(queue)) if (isCircQueueFull(queue))
{ {
return false; return false;
} }
queue->data[queue->in] = data; queue->data[queue->in] = data;
queue->in++; queue->in++;
if (queue->in >= queue->bounds) if (queue->in >= queue->bounds)
{ {
queue->in -= queue->bounds; queue->in -= queue->bounds;
} }
queue->nelts++; queue->nelts++;
return true; return true;
} }
/** /**
@@ -105,7 +105,7 @@ bool CircQueuePush(CIRC_QUEUE *queue, void *data)
*/ */
inline uint32_t CircQueueSize(CIRC_QUEUE *queue) inline uint32_t CircQueueSize(CIRC_QUEUE *queue)
{ {
return queue->nelts; return queue->nelts;
} }
/** /**
@@ -113,22 +113,22 @@ inline uint32_t CircQueueSize(CIRC_QUEUE *queue)
*/ */
bool CircQueuePop(CIRC_QUEUE *queue, void **data) bool CircQueuePop(CIRC_QUEUE *queue, void **data)
{ {
ASSERT(queue != NULL); ASSERT(queue != NULL);
if (isCircQueueEmpty(queue)) if (isCircQueueEmpty(queue))
{ {
return false; return false;
} }
*data = queue->data[queue->out]; *data = queue->data[queue->out];
queue->nelts--; queue->nelts--;
queue->out++; queue->out++;
if (queue->out >= queue->bounds) if (queue->out >= queue->bounds)
{ {
queue->out -= queue->bounds; queue->out -= queue->bounds;
} }
return true; return true;
} }
/** @} CircularQueue*/ /** @} CircularQueue*/
/** @} Utility*/ /** @} Utility*/
df_datalink_sp/src/util/clist.c
+157 -157
View File
@@ -37,13 +37,13 @@
clist_node_t * clist_node_t *
clist_node_new(void *val) clist_node_new(void *val)
{ {
clist_node_t *self; clist_node_t *self;
if (!(self = LIST_MALLOC(sizeof(clist_node_t)))) if (!(self = LIST_MALLOC(sizeof(clist_node_t))))
return NULL; return NULL;
self->prev = NULL; self->prev = NULL;
self->next = NULL; self->next = NULL;
self->val = val; self->val = val;
return self; return self;
} }
/** /**
@@ -52,15 +52,15 @@ clist_node_new(void *val)
clist_t * clist_t *
clist_new(void) clist_new(void)
{ {
clist_t *self; clist_t *self;
if (!(self = LIST_MALLOC(sizeof(clist_t)))) if (!(self = LIST_MALLOC(sizeof(clist_t))))
return NULL; return NULL;
self->head = NULL; self->head = NULL;
self->tail = NULL; self->tail = NULL;
self->free = NULL; self->free = NULL;
self->compare = NULL; self->compare = NULL;
self->len = 0; self->len = 0;
return self; return self;
} }
/** /**
@@ -69,19 +69,19 @@ clist_new(void)
*/ */
void clist_clear(clist_t *self) void clist_clear(clist_t *self)
{ {
clist_node_t *next; clist_node_t *next;
clist_node_t *curr = self->head; clist_node_t *curr = self->head;
while (curr != NULL) while (curr != NULL)
{ {
next = curr->next; next = curr->next;
if (self->free) if (self->free)
self->free(curr->val); self->free(curr->val);
LIST_FREE(curr); LIST_FREE(curr);
curr = next; curr = next;
} }
self->len = 0; self->len = 0;
self->head = NULL; self->head = NULL;
} }
/** /**
@@ -90,8 +90,8 @@ void clist_clear(clist_t *self)
*/ */
void clist_destroy(clist_t *self) void clist_destroy(clist_t *self)
{ {
clist_clear(self); clist_clear(self);
LIST_FREE(self); LIST_FREE(self);
} }
/** /**
@@ -102,24 +102,24 @@ void clist_destroy(clist_t *self)
clist_node_t * clist_node_t *
clist_rpush(clist_t *self, clist_node_t *node) clist_rpush(clist_t *self, clist_node_t *node)
{ {
if (!node) if (!node)
return NULL; return NULL;
if (self->len) if (self->len)
{ {
node->prev = self->tail; node->prev = self->tail;
node->next = NULL; node->next = NULL;
self->tail->next = node; self->tail->next = node;
self->tail = node; self->tail = node;
} }
else else
{ {
self->head = self->tail = node; self->head = self->tail = node;
node->prev = node->next = NULL; node->prev = node->next = NULL;
} }
++self->len; ++self->len;
return node; return node;
} }
/** /**
@@ -130,22 +130,22 @@ clist_rpush(clist_t *self, clist_node_t *node)
clist_node_t * clist_node_t *
clist_rpop(clist_t *self) clist_rpop(clist_t *self)
{ {
if (!self->len) if (!self->len)
return NULL; return NULL;
clist_node_t *node = self->tail; clist_node_t *node = self->tail;
if (--self->len) if (--self->len)
{ {
(self->tail = node->prev)->next = NULL; (self->tail = node->prev)->next = NULL;
} }
else else
{ {
self->tail = self->head = NULL; self->tail = self->head = NULL;
} }
node->next = node->prev = NULL; node->next = node->prev = NULL;
return node; return node;
} }
/** /**
@@ -156,22 +156,22 @@ clist_rpop(clist_t *self)
clist_node_t * clist_node_t *
clist_lpop(clist_t *self) clist_lpop(clist_t *self)
{ {
if (!self->len) if (!self->len)
return NULL; return NULL;
clist_node_t *node = self->head; clist_node_t *node = self->head;
if (--self->len) if (--self->len)
{ {
(self->head = node->next)->prev = NULL; (self->head = node->next)->prev = NULL;
} }
else else
{ {
self->head = self->tail = NULL; self->head = self->tail = NULL;
} }
node->next = node->prev = NULL; node->next = node->prev = NULL;
return node; return node;
} }
/** /**
@@ -183,24 +183,24 @@ clist_lpop(clist_t *self)
clist_node_t * clist_node_t *
clist_lpush(clist_t *self, clist_node_t *node) clist_lpush(clist_t *self, clist_node_t *node)
{ {
if (!node) if (!node)
return NULL; return NULL;
if (self->len) if (self->len)
{ {
node->next = self->head; node->next = self->head;
node->prev = NULL; node->prev = NULL;
self->head->prev = node; self->head->prev = node;
self->head = node; self->head = node;
} }
else else
{ {
self->head = self->tail = node; self->head = self->tail = node;
node->prev = node->next = NULL; node->prev = node->next = NULL;
} }
++self->len; ++self->len;
return node; return node;
} }
/** /**
@@ -212,31 +212,31 @@ clist_lpush(clist_t *self, clist_node_t *node)
clist_node_t * clist_node_t *
clist_find(clist_t *self, void *val) clist_find(clist_t *self, void *val)
{ {
clist_iterator_t *it = clist_iterator_new(self, LIST_HEAD); clist_iterator_t *it = clist_iterator_new(self, LIST_HEAD);
clist_node_t *node; clist_node_t *node;
while ((node = clist_iterator_next(it))) while ((node = clist_iterator_next(it)))
{ {
if (self->compare) if (self->compare)
{ {
if (self->compare(val, node->val) == 0) if (self->compare(val, node->val) == 0)
{ {
clist_iterator_destroy(it); clist_iterator_destroy(it);
return node; return node;
} }
} }
else else
{ {
if (val == node->val) if (val == node->val)
{ {
clist_iterator_destroy(it); clist_iterator_destroy(it);
return node; return node;
} }
} }
} }
clist_iterator_destroy(it); clist_iterator_destroy(it);
return NULL; return NULL;
} }
/** /**
@@ -248,25 +248,25 @@ clist_find(clist_t *self, void *val)
clist_node_t * clist_node_t *
clist_at(clist_t *self, int index) clist_at(clist_t *self, int index)
{ {
clist_direction_t direction = LIST_HEAD; clist_direction_t direction = LIST_HEAD;
if (index < 0) if (index < 0)
{ {
direction = LIST_TAIL; direction = LIST_TAIL;
index = ~index; index = ~index;
} }
if ((unsigned)index < self->len) if ((unsigned)index < self->len)
{ {
clist_iterator_t *it = clist_iterator_new(self, direction); clist_iterator_t *it = clist_iterator_new(self, direction);
clist_node_t *node = clist_iterator_next(it); clist_node_t *node = clist_iterator_next(it);
while (index--) while (index--)
node = clist_iterator_next(it); node = clist_iterator_next(it);
clist_iterator_destroy(it); clist_iterator_destroy(it);
return node; return node;
} }
return NULL; return NULL;
} }
/** /**
@@ -277,19 +277,19 @@ clist_at(clist_t *self, int index)
void clist_remove(clist_t *self, clist_node_t *node) void clist_remove(clist_t *self, clist_node_t *node)
{ {
node->prev node->prev
? (node->prev->next = node->next) ? (node->prev->next = node->next)
: (self->head = node->next); : (self->head = node->next);
node->next node->next
? (node->next->prev = node->prev) ? (node->next->prev = node->prev)
: (self->tail = node->prev); : (self->tail = node->prev);
if (self->free) if (self->free)
self->free(node->val); self->free(node->val);
LIST_FREE(node); LIST_FREE(node);
--self->len; --self->len;
} }
/** /**
@@ -300,10 +300,10 @@ void clist_remove(clist_t *self, clist_node_t *node)
clist_iterator_t * clist_iterator_t *
clist_iterator_new(clist_t *list, clist_direction_t direction) clist_iterator_new(clist_t *list, clist_direction_t direction)
{ {
clist_node_t *node = direction == LIST_HEAD clist_node_t *node = direction == LIST_HEAD
? list->head ? list->head
: list->tail; : list->tail;
return clist_iterator_new_from_node(node, direction); return clist_iterator_new_from_node(node, direction);
} }
/** /**
@@ -314,12 +314,12 @@ clist_iterator_new(clist_t *list, clist_direction_t direction)
clist_iterator_t * clist_iterator_t *
clist_iterator_new_from_node(clist_node_t *node, clist_direction_t direction) clist_iterator_new_from_node(clist_node_t *node, clist_direction_t direction)
{ {
clist_iterator_t *self; clist_iterator_t *self;
if (!(self = LIST_MALLOC(sizeof(clist_iterator_t)))) if (!(self = LIST_MALLOC(sizeof(clist_iterator_t))))
return NULL; return NULL;
self->next = node; self->next = node;
self->direction = direction; self->direction = direction;
return self; return self;
} }
/** /**
@@ -330,14 +330,14 @@ clist_iterator_new_from_node(clist_node_t *node, clist_direction_t direction)
clist_node_t * clist_node_t *
clist_iterator_next(clist_iterator_t *self) clist_iterator_next(clist_iterator_t *self)
{ {
clist_node_t *curr = self->next; clist_node_t *curr = self->next;
if (curr) if (curr)
{ {
self->next = self->direction == LIST_HEAD self->next = self->direction == LIST_HEAD
? curr->next ? curr->next
: curr->prev; : curr->prev;
} }
return curr; return curr;
} }
/** /**
@@ -346,8 +346,8 @@ clist_iterator_next(clist_iterator_t *self)
void clist_iterator_destroy(clist_iterator_t *self) void clist_iterator_destroy(clist_iterator_t *self)
{ {
LIST_FREE(self); LIST_FREE(self);
self = NULL; self = NULL;
} }
/** @} cList*/ /** @} cList*/
/** @} Utility*/ /** @} Utility*/
df_datalink_sp/src/util/crc.c
+292 -292
View File
@@ -27,223 +27,223 @@
// Reversed // Reversed
static const uint16_t crc16_1021_lsb_lut[256] = static const uint16_t crc16_1021_lsb_lut[256] =
{ {
0x0000, 0x1189, 0x2312, 0x329B, 0x4624, 0x57AD, 0x6536, 0x74BF, 0x0000, 0x1189, 0x2312, 0x329B, 0x4624, 0x57AD, 0x6536, 0x74BF,
0x8C48, 0x9DC1, 0xAF5A, 0xBED3, 0xCA6C, 0xDBE5, 0xE97E, 0xF8F7, 0x8C48, 0x9DC1, 0xAF5A, 0xBED3, 0xCA6C, 0xDBE5, 0xE97E, 0xF8F7,
0x1081, 0x0108, 0x3393, 0x221A, 0x56A5, 0x472C, 0x75B7, 0x643E, 0x1081, 0x0108, 0x3393, 0x221A, 0x56A5, 0x472C, 0x75B7, 0x643E,
0x9CC9, 0x8D40, 0xBFDB, 0xAE52, 0xDAED, 0xCB64, 0xF9FF, 0xE876, 0x9CC9, 0x8D40, 0xBFDB, 0xAE52, 0xDAED, 0xCB64, 0xF9FF, 0xE876,
0x2102, 0x308B, 0x0210, 0x1399, 0x6726, 0x76AF, 0x4434, 0x55BD, 0x2102, 0x308B, 0x0210, 0x1399, 0x6726, 0x76AF, 0x4434, 0x55BD,
0xAD4A, 0xBCC3, 0x8E58, 0x9FD1, 0xEB6E, 0xFAE7, 0xC87C, 0xD9F5, 0xAD4A, 0xBCC3, 0x8E58, 0x9FD1, 0xEB6E, 0xFAE7, 0xC87C, 0xD9F5,
0x3183, 0x200A, 0x1291, 0x0318, 0x77A7, 0x662E, 0x54B5, 0x453C, 0x3183, 0x200A, 0x1291, 0x0318, 0x77A7, 0x662E, 0x54B5, 0x453C,
0xBDCB, 0xAC42, 0x9ED9, 0x8F50, 0xFBEF, 0xEA66, 0xD8FD, 0xC974, 0xBDCB, 0xAC42, 0x9ED9, 0x8F50, 0xFBEF, 0xEA66, 0xD8FD, 0xC974,
0x4204, 0x538D, 0x6116, 0x709F, 0x0420, 0x15A9, 0x2732, 0x36BB, 0x4204, 0x538D, 0x6116, 0x709F, 0x0420, 0x15A9, 0x2732, 0x36BB,
0xCE4C, 0xDFC5, 0xED5E, 0xFCD7, 0x8868, 0x99E1, 0xAB7A, 0xBAF3, 0xCE4C, 0xDFC5, 0xED5E, 0xFCD7, 0x8868, 0x99E1, 0xAB7A, 0xBAF3,
0x5285, 0x430C, 0x7197, 0x601E, 0x14A1, 0x0528, 0x37B3, 0x263A, 0x5285, 0x430C, 0x7197, 0x601E, 0x14A1, 0x0528, 0x37B3, 0x263A,
0xDECD, 0xCF44, 0xFDDF, 0xEC56, 0x98E9, 0x8960, 0xBBFB, 0xAA72, 0xDECD, 0xCF44, 0xFDDF, 0xEC56, 0x98E9, 0x8960, 0xBBFB, 0xAA72,
0x6306, 0x728F, 0x4014, 0x519D, 0x2522, 0x34AB, 0x0630, 0x17B9, 0x6306, 0x728F, 0x4014, 0x519D, 0x2522, 0x34AB, 0x0630, 0x17B9,
0xEF4E, 0xFEC7, 0xCC5C, 0xDDD5, 0xA96A, 0xB8E3, 0x8A78, 0x9BF1, 0xEF4E, 0xFEC7, 0xCC5C, 0xDDD5, 0xA96A, 0xB8E3, 0x8A78, 0x9BF1,
0x7387, 0x620E, 0x5095, 0x411C, 0x35A3, 0x242A, 0x16B1, 0x0738, 0x7387, 0x620E, 0x5095, 0x411C, 0x35A3, 0x242A, 0x16B1, 0x0738,
0xFFCF, 0xEE46, 0xDCDD, 0xCD54, 0xB9EB, 0xA862, 0x9AF9, 0x8B70, 0xFFCF, 0xEE46, 0xDCDD, 0xCD54, 0xB9EB, 0xA862, 0x9AF9, 0x8B70,
0x8408, 0x9581, 0xA71A, 0xB693, 0xC22C, 0xD3A5, 0xE13E, 0xF0B7, 0x8408, 0x9581, 0xA71A, 0xB693, 0xC22C, 0xD3A5, 0xE13E, 0xF0B7,
0x0840, 0x19C9, 0x2B52, 0x3ADB, 0x4E64, 0x5FED, 0x6D76, 0x7CFF, 0x0840, 0x19C9, 0x2B52, 0x3ADB, 0x4E64, 0x5FED, 0x6D76, 0x7CFF,
0x9489, 0x8500, 0xB79B, 0xA612, 0xD2AD, 0xC324, 0xF1BF, 0xE036, 0x9489, 0x8500, 0xB79B, 0xA612, 0xD2AD, 0xC324, 0xF1BF, 0xE036,
0x18C1, 0x0948, 0x3BD3, 0x2A5A, 0x5EE5, 0x4F6C, 0x7DF7, 0x6C7E, 0x18C1, 0x0948, 0x3BD3, 0x2A5A, 0x5EE5, 0x4F6C, 0x7DF7, 0x6C7E,
0xA50A, 0xB483, 0x8618, 0x9791, 0xE32E, 0xF2A7, 0xC03C, 0xD1B5, 0xA50A, 0xB483, 0x8618, 0x9791, 0xE32E, 0xF2A7, 0xC03C, 0xD1B5,
0x2942, 0x38CB, 0x0A50, 0x1BD9, 0x6F66, 0x7EEF, 0x4C74, 0x5DFD, 0x2942, 0x38CB, 0x0A50, 0x1BD9, 0x6F66, 0x7EEF, 0x4C74, 0x5DFD,
0xB58B, 0xA402, 0x9699, 0x8710, 0xF3AF, 0xE226, 0xD0BD, 0xC134, 0xB58B, 0xA402, 0x9699, 0x8710, 0xF3AF, 0xE226, 0xD0BD, 0xC134,
0x39C3, 0x284A, 0x1AD1, 0x0B58, 0x7FE7, 0x6E6E, 0x5CF5, 0x4D7C, 0x39C3, 0x284A, 0x1AD1, 0x0B58, 0x7FE7, 0x6E6E, 0x5CF5, 0x4D7C,
0xC60C, 0xD785, 0xE51E, 0xF497, 0x8028, 0x91A1, 0xA33A, 0xB2B3, 0xC60C, 0xD785, 0xE51E, 0xF497, 0x8028, 0x91A1, 0xA33A, 0xB2B3,
0x4A44, 0x5BCD, 0x6956, 0x78DF, 0x0C60, 0x1DE9, 0x2F72, 0x3EFB, 0x4A44, 0x5BCD, 0x6956, 0x78DF, 0x0C60, 0x1DE9, 0x2F72, 0x3EFB,
0xD68D, 0xC704, 0xF59F, 0xE416, 0x90A9, 0x8120, 0xB3BB, 0xA232, 0xD68D, 0xC704, 0xF59F, 0xE416, 0x90A9, 0x8120, 0xB3BB, 0xA232,
0x5AC5, 0x4B4C, 0x79D7, 0x685E, 0x1CE1, 0x0D68, 0x3FF3, 0x2E7A, 0x5AC5, 0x4B4C, 0x79D7, 0x685E, 0x1CE1, 0x0D68, 0x3FF3, 0x2E7A,
0xE70E, 0xF687, 0xC41C, 0xD595, 0xA12A, 0xB0A3, 0x8238, 0x93B1, 0xE70E, 0xF687, 0xC41C, 0xD595, 0xA12A, 0xB0A3, 0x8238, 0x93B1,
0x6B46, 0x7ACF, 0x4854, 0x59DD, 0x2D62, 0x3CEB, 0x0E70, 0x1FF9, 0x6B46, 0x7ACF, 0x4854, 0x59DD, 0x2D62, 0x3CEB, 0x0E70, 0x1FF9,
0xF78F, 0xE606, 0xD49D, 0xC514, 0xB1AB, 0xA022, 0x92B9, 0x8330, 0xF78F, 0xE606, 0xD49D, 0xC514, 0xB1AB, 0xA022, 0x92B9, 0x8330,
0x7BC7, 0x6A4E, 0x58D5, 0x495C, 0x3DE3, 0x2C6A, 0x1EF1, 0x0F78 0x7BC7, 0x6A4E, 0x58D5, 0x495C, 0x3DE3, 0x2C6A, 0x1EF1, 0x0F78
}; };
// CRC-16-MAXIM G(X)=X16+X15+X2+1 // CRC-16-MAXIM G(X)=X16+X15+X2+1
const uint16_t crc16_8005_lsb_lut[256] = const uint16_t crc16_8005_lsb_lut[256] =
{ {
0x0000, 0xC0C1, 0xC181, 0x0140, 0xC301, 0x03C0, 0x0280, 0xC241, 0x0000, 0xC0C1, 0xC181, 0x0140, 0xC301, 0x03C0, 0x0280, 0xC241,
0xC601, 0x06C0, 0x0780, 0xC741, 0x0500, 0xC5C1, 0xC481, 0x0440, 0xC601, 0x06C0, 0x0780, 0xC741, 0x0500, 0xC5C1, 0xC481, 0x0440,
0xCC01, 0x0CC0, 0x0D80, 0xCD41, 0x0F00, 0xCFC1, 0xCE81, 0x0E40, 0xCC01, 0x0CC0, 0x0D80, 0xCD41, 0x0F00, 0xCFC1, 0xCE81, 0x0E40,
0x0A00, 0xCAC1, 0xCB81, 0x0B40, 0xC901, 0x09C0, 0x0880, 0xC841, 0x0A00, 0xCAC1, 0xCB81, 0x0B40, 0xC901, 0x09C0, 0x0880, 0xC841,
0xD801, 0x18C0, 0x1980, 0xD941, 0x1B00, 0xDBC1, 0xDA81, 0x1A40, 0xD801, 0x18C0, 0x1980, 0xD941, 0x1B00, 0xDBC1, 0xDA81, 0x1A40,
0x1E00, 0xDEC1, 0xDF81, 0x1F40, 0xDD01, 0x1DC0, 0x1C80, 0xDC41, 0x1E00, 0xDEC1, 0xDF81, 0x1F40, 0xDD01, 0x1DC0, 0x1C80, 0xDC41,
0x1400, 0xD4C1, 0xD581, 0x1540, 0xD701, 0x17C0, 0x1680, 0xD641, 0x1400, 0xD4C1, 0xD581, 0x1540, 0xD701, 0x17C0, 0x1680, 0xD641,
0xD201, 0x12C0, 0x1380, 0xD341, 0x1100, 0xD1C1, 0xD081, 0x1040, 0xD201, 0x12C0, 0x1380, 0xD341, 0x1100, 0xD1C1, 0xD081, 0x1040,
0xF001, 0x30C0, 0x3180, 0xF141, 0x3300, 0xF3C1, 0xF281, 0x3240, 0xF001, 0x30C0, 0x3180, 0xF141, 0x3300, 0xF3C1, 0xF281, 0x3240,
0x3600, 0xF6C1, 0xF781, 0x3740, 0xF501, 0x35C0, 0x3480, 0xF441, 0x3600, 0xF6C1, 0xF781, 0x3740, 0xF501, 0x35C0, 0x3480, 0xF441,
0x3C00, 0xFCC1, 0xFD81, 0x3D40, 0xFF01, 0x3FC0, 0x3E80, 0xFE41, 0x3C00, 0xFCC1, 0xFD81, 0x3D40, 0xFF01, 0x3FC0, 0x3E80, 0xFE41,
0xFA01, 0x3AC0, 0x3B80, 0xFB41, 0x3900, 0xF9C1, 0xF881, 0x3840, 0xFA01, 0x3AC0, 0x3B80, 0xFB41, 0x3900, 0xF9C1, 0xF881, 0x3840,
0x2800, 0xE8C1, 0xE981, 0x2940, 0xEB01, 0x2BC0, 0x2A80, 0xEA41, 0x2800, 0xE8C1, 0xE981, 0x2940, 0xEB01, 0x2BC0, 0x2A80, 0xEA41,
0xEE01, 0x2EC0, 0x2F80, 0xEF41, 0x2D00, 0xEDC1, 0xEC81, 0x2C40, 0xEE01, 0x2EC0, 0x2F80, 0xEF41, 0x2D00, 0xEDC1, 0xEC81, 0x2C40,
0xE401, 0x24C0, 0x2580, 0xE541, 0x2700, 0xE7C1, 0xE681, 0x2640, 0xE401, 0x24C0, 0x2580, 0xE541, 0x2700, 0xE7C1, 0xE681, 0x2640,
0x2200, 0xE2C1, 0xE381, 0x2340, 0xE101, 0x21C0, 0x2080, 0xE041, 0x2200, 0xE2C1, 0xE381, 0x2340, 0xE101, 0x21C0, 0x2080, 0xE041,
0xA001, 0x60C0, 0x6180, 0xA141, 0x6300, 0xA3C1, 0xA281, 0x6240, 0xA001, 0x60C0, 0x6180, 0xA141, 0x6300, 0xA3C1, 0xA281, 0x6240,
0x6600, 0xA6C1, 0xA781, 0x6740, 0xA501, 0x65C0, 0x6480, 0xA441, 0x6600, 0xA6C1, 0xA781, 0x6740, 0xA501, 0x65C0, 0x6480, 0xA441,
0x6C00, 0xACC1, 0xAD81, 0x6D40, 0xAF01, 0x6FC0, 0x6E80, 0xAE41, 0x6C00, 0xACC1, 0xAD81, 0x6D40, 0xAF01, 0x6FC0, 0x6E80, 0xAE41,
0xAA01, 0x6AC0, 0x6B80, 0xAB41, 0x6900, 0xA9C1, 0xA881, 0x6840, 0xAA01, 0x6AC0, 0x6B80, 0xAB41, 0x6900, 0xA9C1, 0xA881, 0x6840,
0x7800, 0xB8C1, 0xB981, 0x7940, 0xBB01, 0x7BC0, 0x7A80, 0xBA41, 0x7800, 0xB8C1, 0xB981, 0x7940, 0xBB01, 0x7BC0, 0x7A80, 0xBA41,
0xBE01, 0x7EC0, 0x7F80, 0xBF41, 0x7D00, 0xBDC1, 0xBC81, 0x7C40, 0xBE01, 0x7EC0, 0x7F80, 0xBF41, 0x7D00, 0xBDC1, 0xBC81, 0x7C40,
0xB401, 0x74C0, 0x7580, 0xB541, 0x7700, 0xB7C1, 0xB681, 0x7640, 0xB401, 0x74C0, 0x7580, 0xB541, 0x7700, 0xB7C1, 0xB681, 0x7640,
0x7200, 0xB2C1, 0xB381, 0x7340, 0xB101, 0x71C0, 0x7080, 0xB041, 0x7200, 0xB2C1, 0xB381, 0x7340, 0xB101, 0x71C0, 0x7080, 0xB041,
0x5000, 0x90C1, 0x9181, 0x5140, 0x9301, 0x53C0, 0x5280, 0x9241, 0x5000, 0x90C1, 0x9181, 0x5140, 0x9301, 0x53C0, 0x5280, 0x9241,
0x9601, 0x56C0, 0x5780, 0x9741, 0x5500, 0x95C1, 0x9481, 0x5440, 0x9601, 0x56C0, 0x5780, 0x9741, 0x5500, 0x95C1, 0x9481, 0x5440,
0x9C01, 0x5CC0, 0x5D80, 0x9D41, 0x5F00, 0x9FC1, 0x9E81, 0x5E40, 0x9C01, 0x5CC0, 0x5D80, 0x9D41, 0x5F00, 0x9FC1, 0x9E81, 0x5E40,
0x5A00, 0x9AC1, 0x9B81, 0x5B40, 0x9901, 0x59C0, 0x5880, 0x9841, 0x5A00, 0x9AC1, 0x9B81, 0x5B40, 0x9901, 0x59C0, 0x5880, 0x9841,
0x8801, 0x48C0, 0x4980, 0x8941, 0x4B00, 0x8BC1, 0x8A81, 0x4A40, 0x8801, 0x48C0, 0x4980, 0x8941, 0x4B00, 0x8BC1, 0x8A81, 0x4A40,
0x4E00, 0x8EC1, 0x8F81, 0x4F40, 0x8D01, 0x4DC0, 0x4C80, 0x8C41, 0x4E00, 0x8EC1, 0x8F81, 0x4F40, 0x8D01, 0x4DC0, 0x4C80, 0x8C41,
0x4400, 0x84C1, 0x8581, 0x4540, 0x8701, 0x47C0, 0x4680, 0x8641, 0x4400, 0x84C1, 0x8581, 0x4540, 0x8701, 0x47C0, 0x4680, 0x8641,
0x8201, 0x42C0, 0x4380, 0x8341, 0x4100, 0x81C1, 0x8081, 0x4040 0x8201, 0x42C0, 0x4380, 0x8341, 0x4100, 0x81C1, 0x8081, 0x4040
}; };
// CRC-8-MAXIM G(X)=X8+X5+X4+1 // CRC-8-MAXIM G(X)=X8+X5+X4+1
const uint8_t crc8_31_lsb_lut[256] = const uint8_t crc8_31_lsb_lut[256] =
{ {
0x00, 0x5E, 0xBC, 0xE2, 0x61, 0x3F, 0xDD, 0x83, 0x00, 0x5E, 0xBC, 0xE2, 0x61, 0x3F, 0xDD, 0x83,
0xC2, 0x9C, 0x7E, 0x20, 0xA3, 0xFD, 0x1F, 0x41, 0xC2, 0x9C, 0x7E, 0x20, 0xA3, 0xFD, 0x1F, 0x41,
0x9D, 0xC3, 0x21, 0x7F, 0xFC, 0xA2, 0x40, 0x1E, 0x9D, 0xC3, 0x21, 0x7F, 0xFC, 0xA2, 0x40, 0x1E,
0x5F, 0x01, 0xE3, 0xBD, 0x3E, 0x60, 0x82, 0xDC, 0x5F, 0x01, 0xE3, 0xBD, 0x3E, 0x60, 0x82, 0xDC,
0x23, 0x7D, 0x9F, 0xC1, 0x42, 0x1C, 0xFE, 0xA0, 0x23, 0x7D, 0x9F, 0xC1, 0x42, 0x1C, 0xFE, 0xA0,
0xE1, 0xBF, 0x5D, 0x03, 0x80, 0xDE, 0x3C, 0x62, 0xE1, 0xBF, 0x5D, 0x03, 0x80, 0xDE, 0x3C, 0x62,
0xBE, 0xE0, 0x02, 0x5C, 0xDF, 0x81, 0x63, 0x3D, 0xBE, 0xE0, 0x02, 0x5C, 0xDF, 0x81, 0x63, 0x3D,
0x7C, 0x22, 0xC0, 0x9E, 0x1D, 0x43, 0xA1, 0xFF, 0x7C, 0x22, 0xC0, 0x9E, 0x1D, 0x43, 0xA1, 0xFF,
0x46, 0x18, 0xFA, 0xA4, 0x27, 0x79, 0x9B, 0xC5, 0x46, 0x18, 0xFA, 0xA4, 0x27, 0x79, 0x9B, 0xC5,
0x84, 0xDA, 0x38, 0x66, 0xE5, 0xBB, 0x59, 0x07, 0x84, 0xDA, 0x38, 0x66, 0xE5, 0xBB, 0x59, 0x07,
0xDB, 0x85, 0x67, 0x39, 0xBA, 0xE4, 0x06, 0x58, 0xDB, 0x85, 0x67, 0x39, 0xBA, 0xE4, 0x06, 0x58,
0x19, 0x47, 0xA5, 0xFB, 0x78, 0x26, 0xC4, 0x9A, 0x19, 0x47, 0xA5, 0xFB, 0x78, 0x26, 0xC4, 0x9A,
0x65, 0x3B, 0xD9, 0x87, 0x04, 0x5A, 0xB8, 0xE6, 0x65, 0x3B, 0xD9, 0x87, 0x04, 0x5A, 0xB8, 0xE6,
0xA7, 0xF9, 0x1B, 0x45, 0xC6, 0x98, 0x7A, 0x24, 0xA7, 0xF9, 0x1B, 0x45, 0xC6, 0x98, 0x7A, 0x24,
0xF8, 0xA6, 0x44, 0x1A, 0x99, 0xC7, 0x25, 0x7B, 0xF8, 0xA6, 0x44, 0x1A, 0x99, 0xC7, 0x25, 0x7B,
0x3A, 0x64, 0x86, 0xD8, 0x5B, 0x05, 0xE7, 0xB9, 0x3A, 0x64, 0x86, 0xD8, 0x5B, 0x05, 0xE7, 0xB9,
0x8C, 0xD2, 0x30, 0x6E, 0xED, 0xB3, 0x51, 0x0F, 0x8C, 0xD2, 0x30, 0x6E, 0xED, 0xB3, 0x51, 0x0F,
0x4E, 0x10, 0xF2, 0xAC, 0x2F, 0x71, 0x93, 0xCD, 0x4E, 0x10, 0xF2, 0xAC, 0x2F, 0x71, 0x93, 0xCD,
0x11, 0x4F, 0xAD, 0xF3, 0x70, 0x2E, 0xCC, 0x92, 0x11, 0x4F, 0xAD, 0xF3, 0x70, 0x2E, 0xCC, 0x92,
0xD3, 0x8D, 0x6F, 0x31, 0xB2, 0xEC, 0x0E, 0x50, 0xD3, 0x8D, 0x6F, 0x31, 0xB2, 0xEC, 0x0E, 0x50,
0xAF, 0xF1, 0x13, 0x4D, 0xCE, 0x90, 0x72, 0x2C, 0xAF, 0xF1, 0x13, 0x4D, 0xCE, 0x90, 0x72, 0x2C,
0x6D, 0x33, 0xD1, 0x8F, 0x0C, 0x52, 0xB0, 0xEE, 0x6D, 0x33, 0xD1, 0x8F, 0x0C, 0x52, 0xB0, 0xEE,
0x32, 0x6C, 0x8E, 0xD0, 0x53, 0x0D, 0xEF, 0xB1, 0x32, 0x6C, 0x8E, 0xD0, 0x53, 0x0D, 0xEF, 0xB1,
0xF0, 0xAE, 0x4C, 0x12, 0x91, 0xCF, 0x2D, 0x73, 0xF0, 0xAE, 0x4C, 0x12, 0x91, 0xCF, 0x2D, 0x73,
0xCA, 0x94, 0x76, 0x28, 0xAB, 0xF5, 0x17, 0x49, 0xCA, 0x94, 0x76, 0x28, 0xAB, 0xF5, 0x17, 0x49,
0x08, 0x56, 0xB4, 0xEA, 0x69, 0x37, 0xD5, 0x8B, 0x08, 0x56, 0xB4, 0xEA, 0x69, 0x37, 0xD5, 0x8B,
0x57, 0x09, 0xEB, 0xB5, 0x36, 0x68, 0x8A, 0xD4, 0x57, 0x09, 0xEB, 0xB5, 0x36, 0x68, 0x8A, 0xD4,
0x95, 0xCB, 0x29, 0x77, 0xF4, 0xAA, 0x48, 0x16, 0x95, 0xCB, 0x29, 0x77, 0xF4, 0xAA, 0x48, 0x16,
0xE9, 0xB7, 0x55, 0x0B, 0x88, 0xD6, 0x34, 0x6A, 0xE9, 0xB7, 0x55, 0x0B, 0x88, 0xD6, 0x34, 0x6A,
0x2B, 0x75, 0x97, 0xC9, 0x4A, 0x14, 0xF6, 0xA8, 0x2B, 0x75, 0x97, 0xC9, 0x4A, 0x14, 0xF6, 0xA8,
0x74, 0x2A, 0xC8, 0x96, 0x15, 0x4B, 0xA9, 0xF7, 0x74, 0x2A, 0xC8, 0x96, 0x15, 0x4B, 0xA9, 0xF7,
0xB6, 0xE8, 0x0A, 0x54, 0xD7, 0x89, 0x6B, 0x35 0xB6, 0xE8, 0x0A, 0x54, 0xD7, 0x89, 0x6B, 0x35
}; };
#else #else
// CRC-16-CCITT G(X)=X16+X12+X5+1 // CRC-16-CCITT G(X)=X16+X12+X5+1
static const uint16_t crc16_1021_msb_lut[256] = static const uint16_t crc16_1021_msb_lut[256] =
{ {
0x0000, 0x1021, 0x2042, 0x3063, 0x4084, 0x50A5, 0x60C6, 0x70E7, 0x0000, 0x1021, 0x2042, 0x3063, 0x4084, 0x50A5, 0x60C6, 0x70E7,
0x8108, 0x9129, 0xA14A, 0xB16B, 0xC18C, 0xD1AD, 0xE1CE, 0xF1EF, 0x8108, 0x9129, 0xA14A, 0xB16B, 0xC18C, 0xD1AD, 0xE1CE, 0xF1EF,
0x1231, 0x0210, 0x3273, 0x2252, 0x52B5, 0x4294, 0x72F7, 0x62D6, 0x1231, 0x0210, 0x3273, 0x2252, 0x52B5, 0x4294, 0x72F7, 0x62D6,
0x9339, 0x8318, 0xB37B, 0xA35A, 0xD3BD, 0xC39C, 0xF3FF, 0xE3DE, 0x9339, 0x8318, 0xB37B, 0xA35A, 0xD3BD, 0xC39C, 0xF3FF, 0xE3DE,
0x2462, 0x3443, 0x0420, 0x1401, 0x64E6, 0x74C7, 0x44A4, 0x5485, 0x2462, 0x3443, 0x0420, 0x1401, 0x64E6, 0x74C7, 0x44A4, 0x5485,
0xA56A, 0xB54B, 0x8528, 0x9509, 0xE5EE, 0xF5CF, 0xC5AC, 0xD58D, 0xA56A, 0xB54B, 0x8528, 0x9509, 0xE5EE, 0xF5CF, 0xC5AC, 0xD58D,
0x3653, 0x2672, 0x1611, 0x0630, 0x76D7, 0x66F6, 0x5695, 0x46B4, 0x3653, 0x2672, 0x1611, 0x0630, 0x76D7, 0x66F6, 0x5695, 0x46B4,
0xB75B, 0xA77A, 0x9719, 0x8738, 0xF7DF, 0xE7FE, 0xD79D, 0xC7BC, 0xB75B, 0xA77A, 0x9719, 0x8738, 0xF7DF, 0xE7FE, 0xD79D, 0xC7BC,
0x48C4, 0x58E5, 0x6886, 0x78A7, 0x0840, 0x1861, 0x2802, 0x3823, 0x48C4, 0x58E5, 0x6886, 0x78A7, 0x0840, 0x1861, 0x2802, 0x3823,
0xC9CC, 0xD9ED, 0xE98E, 0xF9AF, 0x8948, 0x9969, 0xA90A, 0xB92B, 0xC9CC, 0xD9ED, 0xE98E, 0xF9AF, 0x8948, 0x9969, 0xA90A, 0xB92B,
0x5AF5, 0x4AD4, 0x7AB7, 0x6A96, 0x1A71, 0x0A50, 0x3A33, 0x2A12, 0x5AF5, 0x4AD4, 0x7AB7, 0x6A96, 0x1A71, 0x0A50, 0x3A33, 0x2A12,
0xDBFD, 0xCBDC, 0xFBBF, 0xEB9E, 0x9B79, 0x8B58, 0xBB3B, 0xAB1A, 0xDBFD, 0xCBDC, 0xFBBF, 0xEB9E, 0x9B79, 0x8B58, 0xBB3B, 0xAB1A,
0x6CA6, 0x7C87, 0x4CE4, 0x5CC5, 0x2C22, 0x3C03, 0x0C60, 0x1C41, 0x6CA6, 0x7C87, 0x4CE4, 0x5CC5, 0x2C22, 0x3C03, 0x0C60, 0x1C41,
0xEDAE, 0xFD8F, 0xCDEC, 0xDDCD, 0xAD2A, 0xBD0B, 0x8D68, 0x9D49, 0xEDAE, 0xFD8F, 0xCDEC, 0xDDCD, 0xAD2A, 0xBD0B, 0x8D68, 0x9D49,
0x7E97, 0x6EB6, 0x5ED5, 0x4EF4, 0x3E13, 0x2E32, 0x1E51, 0x0E70, 0x7E97, 0x6EB6, 0x5ED5, 0x4EF4, 0x3E13, 0x2E32, 0x1E51, 0x0E70,
0xFF9F, 0xEFBE, 0xDFDD, 0xCFFC, 0xBF1B, 0xAF3A, 0x9F59, 0x8F78, 0xFF9F, 0xEFBE, 0xDFDD, 0xCFFC, 0xBF1B, 0xAF3A, 0x9F59, 0x8F78,
0x9188, 0x81A9, 0xB1CA, 0xA1EB, 0xD10C, 0xC12D, 0xF14E, 0xE16F, 0x9188, 0x81A9, 0xB1CA, 0xA1EB, 0xD10C, 0xC12D, 0xF14E, 0xE16F,
0x1080, 0x00A1, 0x30C2, 0x20E3, 0x5004, 0x4025, 0x7046, 0x6067, 0x1080, 0x00A1, 0x30C2, 0x20E3, 0x5004, 0x4025, 0x7046, 0x6067,
0x83B9, 0x9398, 0xA3FB, 0xB3DA, 0xC33D, 0xD31C, 0xE37F, 0xF35E, 0x83B9, 0x9398, 0xA3FB, 0xB3DA, 0xC33D, 0xD31C, 0xE37F, 0xF35E,
0x02B1, 0x1290, 0x22F3, 0x32D2, 0x4235, 0x5214, 0x6277, 0x7256, 0x02B1, 0x1290, 0x22F3, 0x32D2, 0x4235, 0x5214, 0x6277, 0x7256,
0xB5EA, 0xA5CB, 0x95A8, 0x8589, 0xF56E, 0xE54F, 0xD52C, 0xC50D, 0xB5EA, 0xA5CB, 0x95A8, 0x8589, 0xF56E, 0xE54F, 0xD52C, 0xC50D,
0x34E2, 0x24C3, 0x14A0, 0x0481, 0x7466, 0x6447, 0x5424, 0x4405, 0x34E2, 0x24C3, 0x14A0, 0x0481, 0x7466, 0x6447, 0x5424, 0x4405,
0xA7DB, 0xB7FA, 0x8799, 0x97B8, 0xE75F, 0xF77E, 0xC71D, 0xD73C, 0xA7DB, 0xB7FA, 0x8799, 0x97B8, 0xE75F, 0xF77E, 0xC71D, 0xD73C,
0x26D3, 0x36F2, 0x0691, 0x16B0, 0x6657, 0x7676, 0x4615, 0x5634, 0x26D3, 0x36F2, 0x0691, 0x16B0, 0x6657, 0x7676, 0x4615, 0x5634,
0xD94C, 0xC96D, 0xF90E, 0xE92F, 0x99C8, 0x89E9, 0xB98A, 0xA9AB, 0xD94C, 0xC96D, 0xF90E, 0xE92F, 0x99C8, 0x89E9, 0xB98A, 0xA9AB,
0x5844, 0x4865, 0x7806, 0x6827, 0x18C0, 0x08E1, 0x3882, 0x28A3, 0x5844, 0x4865, 0x7806, 0x6827, 0x18C0, 0x08E1, 0x3882, 0x28A3,
0xCB7D, 0xDB5C, 0xEB3F, 0xFB1E, 0x8BF9, 0x9BD8, 0xABBB, 0xBB9A, 0xCB7D, 0xDB5C, 0xEB3F, 0xFB1E, 0x8BF9, 0x9BD8, 0xABBB, 0xBB9A,
0x4A75, 0x5A54, 0x6A37, 0x7A16, 0x0AF1, 0x1AD0, 0x2AB3, 0x3A92, 0x4A75, 0x5A54, 0x6A37, 0x7A16, 0x0AF1, 0x1AD0, 0x2AB3, 0x3A92,
0xFD2E, 0xED0F, 0xDD6C, 0xCD4D, 0xBDAA, 0xAD8B, 0x9DE8, 0x8DC9, 0xFD2E, 0xED0F, 0xDD6C, 0xCD4D, 0xBDAA, 0xAD8B, 0x9DE8, 0x8DC9,
0x7C26, 0x6C07, 0x5C64, 0x4C45, 0x3CA2, 0x2C83, 0x1CE0, 0x0CC1, 0x7C26, 0x6C07, 0x5C64, 0x4C45, 0x3CA2, 0x2C83, 0x1CE0, 0x0CC1,
0xEF1F, 0xFF3E, 0xCF5D, 0xDF7C, 0xAF9B, 0xBFBA, 0x8FD9, 0x9FF8, 0xEF1F, 0xFF3E, 0xCF5D, 0xDF7C, 0xAF9B, 0xBFBA, 0x8FD9, 0x9FF8,
0x6E17, 0x7E36, 0x4E55, 0x5E74, 0x2E93, 0x3EB2, 0x0ED1, 0x1EF0 0x6E17, 0x7E36, 0x4E55, 0x5E74, 0x2E93, 0x3EB2, 0x0ED1, 0x1EF0
}; };
// CRC-16-MAXIM G(X)=X16+X15+X2+1 // CRC-16-MAXIM G(X)=X16+X15+X2+1
const uint16_t crc16_8005_msb_lut[256] = const uint16_t crc16_8005_msb_lut[256] =
{ {
0x0000, 0x8005, 0x800F, 0x000A, 0x801B, 0x001E, 0x0014, 0x8011, 0x0000, 0x8005, 0x800F, 0x000A, 0x801B, 0x001E, 0x0014, 0x8011,
0x8033, 0x0036, 0x003C, 0x8039, 0x0028, 0x802D, 0x8027, 0x0022, 0x8033, 0x0036, 0x003C, 0x8039, 0x0028, 0x802D, 0x8027, 0x0022,
0x8063, 0x0066, 0x006C, 0x8069, 0x0078, 0x807D, 0x8077, 0x0072, 0x8063, 0x0066, 0x006C, 0x8069, 0x0078, 0x807D, 0x8077, 0x0072,
0x0050, 0x8055, 0x805F, 0x005A, 0x804B, 0x004E, 0x0044, 0x8041, 0x0050, 0x8055, 0x805F, 0x005A, 0x804B, 0x004E, 0x0044, 0x8041,
0x80C3, 0x00C6, 0x00CC, 0x80C9, 0x00D8, 0x80DD, 0x80D7, 0x00D2, 0x80C3, 0x00C6, 0x00CC, 0x80C9, 0x00D8, 0x80DD, 0x80D7, 0x00D2,
0x00F0, 0x80F5, 0x80FF, 0x00FA, 0x80EB, 0x00EE, 0x00E4, 0x80E1, 0x00F0, 0x80F5, 0x80FF, 0x00FA, 0x80EB, 0x00EE, 0x00E4, 0x80E1,
0x00A0, 0x80A5, 0x80AF, 0x00AA, 0x80BB, 0x00BE, 0x00B4, 0x80B1, 0x00A0, 0x80A5, 0x80AF, 0x00AA, 0x80BB, 0x00BE, 0x00B4, 0x80B1,
0x8093, 0x0096, 0x009C, 0x8099, 0x0088, 0x808D, 0x8087, 0x0082, 0x8093, 0x0096, 0x009C, 0x8099, 0x0088, 0x808D, 0x8087, 0x0082,
0x8183, 0x0186, 0x018C, 0x8189, 0x0198, 0x819D, 0x8197, 0x0192, 0x8183, 0x0186, 0x018C, 0x8189, 0x0198, 0x819D, 0x8197, 0x0192,
0x01B0, 0x81B5, 0x81BF, 0x01BA, 0x81AB, 0x01AE, 0x01A4, 0x81A1, 0x01B0, 0x81B5, 0x81BF, 0x01BA, 0x81AB, 0x01AE, 0x01A4, 0x81A1,
0x01E0, 0x81E5, 0x81EF, 0x01EA, 0x81FB, 0x01FE, 0x01F4, 0x81F1, 0x01E0, 0x81E5, 0x81EF, 0x01EA, 0x81FB, 0x01FE, 0x01F4, 0x81F1,
0x81D3, 0x01D6, 0x01DC, 0x81D9, 0x01C8, 0x81CD, 0x81C7, 0x01C2, 0x81D3, 0x01D6, 0x01DC, 0x81D9, 0x01C8, 0x81CD, 0x81C7, 0x01C2,
0x0140, 0x8145, 0x814F, 0x014A, 0x815B, 0x015E, 0x0154, 0x8151, 0x0140, 0x8145, 0x814F, 0x014A, 0x815B, 0x015E, 0x0154, 0x8151,
0x8173, 0x0176, 0x017C, 0x8179, 0x0168, 0x816D, 0x8167, 0x0162, 0x8173, 0x0176, 0x017C, 0x8179, 0x0168, 0x816D, 0x8167, 0x0162,
0x8123, 0x0126, 0x012C, 0x8129, 0x0138, 0x813D, 0x8137, 0x0132, 0x8123, 0x0126, 0x012C, 0x8129, 0x0138, 0x813D, 0x8137, 0x0132,
0x0110, 0x8115, 0x811F, 0x011A, 0x810B, 0x010E, 0x0104, 0x8101, 0x0110, 0x8115, 0x811F, 0x011A, 0x810B, 0x010E, 0x0104, 0x8101,
0x8303, 0x0306, 0x030C, 0x8309, 0x0318, 0x831D, 0x8317, 0x0312, 0x8303, 0x0306, 0x030C, 0x8309, 0x0318, 0x831D, 0x8317, 0x0312,
0x0330, 0x8335, 0x833F, 0x033A, 0x832B, 0x032E, 0x0324, 0x8321, 0x0330, 0x8335, 0x833F, 0x033A, 0x832B, 0x032E, 0x0324, 0x8321,
0x0360, 0x8365, 0x836F, 0x036A, 0x837B, 0x037E, 0x0374, 0x8371, 0x0360, 0x8365, 0x836F, 0x036A, 0x837B, 0x037E, 0x0374, 0x8371,
0x8353, 0x0356, 0x035C, 0x8359, 0x0348, 0x834D, 0x8347, 0x0342, 0x8353, 0x0356, 0x035C, 0x8359, 0x0348, 0x834D, 0x8347, 0x0342,
0x03C0, 0x83C5, 0x83CF, 0x03CA, 0x83DB, 0x03DE, 0x03D4, 0x83D1, 0x03C0, 0x83C5, 0x83CF, 0x03CA, 0x83DB, 0x03DE, 0x03D4, 0x83D1,
0x83F3, 0x03F6, 0x03FC, 0x83F9, 0x03E8, 0x83ED, 0x83E7, 0x03E2, 0x83F3, 0x03F6, 0x03FC, 0x83F9, 0x03E8, 0x83ED, 0x83E7, 0x03E2,
0x83A3, 0x03A6, 0x03AC, 0x83A9, 0x03B8, 0x83BD, 0x83B7, 0x03B2, 0x83A3, 0x03A6, 0x03AC, 0x83A9, 0x03B8, 0x83BD, 0x83B7, 0x03B2,
0x0390, 0x8395, 0x839F, 0x039A, 0x838B, 0x038E, 0x0384, 0x8381, 0x0390, 0x8395, 0x839F, 0x039A, 0x838B, 0x038E, 0x0384, 0x8381,
0x0280, 0x8285, 0x828F, 0x028A, 0x829B, 0x029E, 0x0294, 0x8291, 0x0280, 0x8285, 0x828F, 0x028A, 0x829B, 0x029E, 0x0294, 0x8291,
0x82B3, 0x02B6, 0x02BC, 0x82B9, 0x02A8, 0x82AD, 0x82A7, 0x02A2, 0x82B3, 0x02B6, 0x02BC, 0x82B9, 0x02A8, 0x82AD, 0x82A7, 0x02A2,
0x82E3, 0x02E6, 0x02EC, 0x82E9, 0x02F8, 0x82FD, 0x82F7, 0x02F2, 0x82E3, 0x02E6, 0x02EC, 0x82E9, 0x02F8, 0x82FD, 0x82F7, 0x02F2,
0x02D0, 0x82D5, 0x82DF, 0x02DA, 0x82CB, 0x02CE, 0x02C4, 0x82C1, 0x02D0, 0x82D5, 0x82DF, 0x02DA, 0x82CB, 0x02CE, 0x02C4, 0x82C1,
0x8243, 0x0246, 0x024C, 0x8249, 0x0258, 0x825D, 0x8257, 0x0252, 0x8243, 0x0246, 0x024C, 0x8249, 0x0258, 0x825D, 0x8257, 0x0252,
0x0270, 0x8275, 0x827F, 0x027A, 0x826B, 0x026E, 0x0264, 0x8261, 0x0270, 0x8275, 0x827F, 0x027A, 0x826B, 0x026E, 0x0264, 0x8261,
0x0220, 0x8225, 0x822F, 0x022A, 0x823B, 0x023E, 0x0234, 0x8231, 0x0220, 0x8225, 0x822F, 0x022A, 0x823B, 0x023E, 0x0234, 0x8231,
0x8213, 0x0216, 0x021C, 0x8219, 0x0208, 0x820D, 0x8207, 0x0202 0x8213, 0x0216, 0x021C, 0x8219, 0x0208, 0x820D, 0x8207, 0x0202
}; };
// CRC-8-MAXIM G(X)=X8+X5+X4+1 // CRC-8-MAXIM G(X)=X8+X5+X4+1
const uint8_t crc8_31_msb_lut[256] = const uint8_t crc8_31_msb_lut[256] =
{ {
0x00, 0x31, 0x62, 0x53, 0xC4, 0xF5, 0xA6, 0x97, 0x00, 0x31, 0x62, 0x53, 0xC4, 0xF5, 0xA6, 0x97,
0xB9, 0x88, 0xDB, 0xEA, 0x7D, 0x4C, 0x1F, 0x2E, 0xB9, 0x88, 0xDB, 0xEA, 0x7D, 0x4C, 0x1F, 0x2E,
0x43, 0x72, 0x21, 0x10, 0x87, 0xB6, 0xE5, 0xD4, 0x43, 0x72, 0x21, 0x10, 0x87, 0xB6, 0xE5, 0xD4,
0xFA, 0xCB, 0x98, 0xA9, 0x3E, 0x0F, 0x5C, 0x6D, 0xFA, 0xCB, 0x98, 0xA9, 0x3E, 0x0F, 0x5C, 0x6D,
0x86, 0xB7, 0xE4, 0xD5, 0x42, 0x73, 0x20, 0x11, 0x86, 0xB7, 0xE4, 0xD5, 0x42, 0x73, 0x20, 0x11,
0x3F, 0x0E, 0x5D, 0x6C, 0xFB, 0xCA, 0x99, 0xA8, 0x3F, 0x0E, 0x5D, 0x6C, 0xFB, 0xCA, 0x99, 0xA8,
0xC5, 0xF4, 0xA7, 0x96, 0x01, 0x30, 0x63, 0x52, 0xC5, 0xF4, 0xA7, 0x96, 0x01, 0x30, 0x63, 0x52,
0x7C, 0x4D, 0x1E, 0x2F, 0xB8, 0x89, 0xDA, 0xEB, 0x7C, 0x4D, 0x1E, 0x2F, 0xB8, 0x89, 0xDA, 0xEB,
0x3D, 0x0C, 0x5F, 0x6E, 0xF9, 0xC8, 0x9B, 0xAA, 0x3D, 0x0C, 0x5F, 0x6E, 0xF9, 0xC8, 0x9B, 0xAA,
0x84, 0xB5, 0xE6, 0xD7, 0x40, 0x71, 0x22, 0x13, 0x84, 0xB5, 0xE6, 0xD7, 0x40, 0x71, 0x22, 0x13,
0x7E, 0x4F, 0x1C, 0x2D, 0xBA, 0x8B, 0xD8, 0xE9, 0x7E, 0x4F, 0x1C, 0x2D, 0xBA, 0x8B, 0xD8, 0xE9,
0xC7, 0xF6, 0xA5, 0x94, 0x03, 0x32, 0x61, 0x50, 0xC7, 0xF6, 0xA5, 0x94, 0x03, 0x32, 0x61, 0x50,
0xBB, 0x8A, 0xD9, 0xE8, 0x7F, 0x4E, 0x1D, 0x2C, 0xBB, 0x8A, 0xD9, 0xE8, 0x7F, 0x4E, 0x1D, 0x2C,
0x02, 0x33, 0x60, 0x51, 0xC6, 0xF7, 0xA4, 0x95, 0x02, 0x33, 0x60, 0x51, 0xC6, 0xF7, 0xA4, 0x95,
0xF8, 0xC9, 0x9A, 0xAB, 0x3C, 0x0D, 0x5E, 0x6F, 0xF8, 0xC9, 0x9A, 0xAB, 0x3C, 0x0D, 0x5E, 0x6F,
0x41, 0x70, 0x23, 0x12, 0x85, 0xB4, 0xE7, 0xD6, 0x41, 0x70, 0x23, 0x12, 0x85, 0xB4, 0xE7, 0xD6,
0x7A, 0x4B, 0x18, 0x29, 0xBE, 0x8F, 0xDC, 0xED, 0x7A, 0x4B, 0x18, 0x29, 0xBE, 0x8F, 0xDC, 0xED,
0xC3, 0xF2, 0xA1, 0x90, 0x07, 0x36, 0x65, 0x54, 0xC3, 0xF2, 0xA1, 0x90, 0x07, 0x36, 0x65, 0x54,
0x39, 0x08, 0x5B, 0x6A, 0xFD, 0xCC, 0x9F, 0xAE, 0x39, 0x08, 0x5B, 0x6A, 0xFD, 0xCC, 0x9F, 0xAE,
0x80, 0xB1, 0xE2, 0xD3, 0x44, 0x75, 0x26, 0x17, 0x80, 0xB1, 0xE2, 0xD3, 0x44, 0x75, 0x26, 0x17,
0xFC, 0xCD, 0x9E, 0xAF, 0x38, 0x09, 0x5A, 0x6B, 0xFC, 0xCD, 0x9E, 0xAF, 0x38, 0x09, 0x5A, 0x6B,
0x45, 0x74, 0x27, 0x16, 0x81, 0xB0, 0xE3, 0xD2, 0x45, 0x74, 0x27, 0x16, 0x81, 0xB0, 0xE3, 0xD2,
0xBF, 0x8E, 0xDD, 0xEC, 0x7B, 0x4A, 0x19, 0x28, 0xBF, 0x8E, 0xDD, 0xEC, 0x7B, 0x4A, 0x19, 0x28,
0x06, 0x37, 0x64, 0x55, 0xC2, 0xF3, 0xA0, 0x91, 0x06, 0x37, 0x64, 0x55, 0xC2, 0xF3, 0xA0, 0x91,
0x47, 0x76, 0x25, 0x14, 0x83, 0xB2, 0xE1, 0xD0, 0x47, 0x76, 0x25, 0x14, 0x83, 0xB2, 0xE1, 0xD0,
0xFE, 0xCF, 0x9C, 0xAD, 0x3A, 0x0B, 0x58, 0x69, 0xFE, 0xCF, 0x9C, 0xAD, 0x3A, 0x0B, 0x58, 0x69,
0x04, 0x35, 0x66, 0x57, 0xC0, 0xF1, 0xA2, 0x93, 0x04, 0x35, 0x66, 0x57, 0xC0, 0xF1, 0xA2, 0x93,
0xBD, 0x8C, 0xDF, 0xEE, 0x79, 0x48, 0x1B, 0x2A, 0xBD, 0x8C, 0xDF, 0xEE, 0x79, 0x48, 0x1B, 0x2A,
0xC1, 0xF0, 0xA3, 0x92, 0x05, 0x34, 0x67, 0x56, 0xC1, 0xF0, 0xA3, 0x92, 0x05, 0x34, 0x67, 0x56,
0x78, 0x49, 0x1A, 0x2B, 0xBC, 0x8D, 0xDE, 0xEF, 0x78, 0x49, 0x1A, 0x2B, 0xBC, 0x8D, 0xDE, 0xEF,
0x82, 0xB3, 0xE0, 0xD1, 0x46, 0x77, 0x24, 0x15, 0x82, 0xB3, 0xE0, 0xD1, 0x46, 0x77, 0x24, 0x15,
0x3B, 0x0A, 0x59, 0x68, 0xFF, 0xCE, 0x9D, 0xAC 0x3B, 0x0A, 0x59, 0x68, 0xFF, 0xCE, 0x9D, 0xAC
}; };
#endif #endif
@@ -255,12 +255,12 @@ uint8_t/uint16_t/uint32_t crc_val = 0x0;
// LSB // LSB
for(int i = 0; i < sizeof(dataBuff); i++) for(int i = 0; i < sizeof(dataBuff); i++)
{ {
crc_val = (crc_val >> BITS_IN_BYTE) ^ crc_lsb_lut[(uint8)(crc_val^dataBuff[i])]; crc_val = (crc_val >> BITS_IN_BYTE) ^ crc_lsb_lut[(uint8)(crc_val^dataBuff[i])];
} }
// MSB // MSB
for(int i = 0; i < sizeof(dataBuff); i++) for(int i = 0; i < sizeof(dataBuff); i++)
{ {
crc_val = (crc_val << BITS_IN_BYTE) ^ crc_msb_lut[(uint8)((crc_val>>8)^dataBuff[i])]; crc_val = (crc_val << BITS_IN_BYTE) ^ crc_msb_lut[(uint8)((crc_val>>8)^dataBuff[i])];
} }
*/ */
@@ -279,32 +279,32 @@ for(int i = 0; i < sizeof(dataBuff); i++)
******************************************************************************/ ******************************************************************************/
int crc8_gen(uint8_t *buffPtr, uint32_t dataLen, uint16_t poly, uint8_t *crcVal) int crc8_gen(uint8_t *buffPtr, uint32_t dataLen, uint16_t poly, uint8_t *crcVal)
{ {
uint8_t crc = CRC8_INIT; uint8_t crc = CRC8_INIT;
const uint8_t *crc_lut = NULL; const uint8_t *crc_lut = NULL;
switch (poly) switch (poly)
{ {
#ifdef __CRC_LSB #ifdef __CRC_LSB
case 0x31: case 0x31:
crc_lut = crc8_31_lsb_lut; crc_lut = crc8_31_lsb_lut;
break; break;
default: default:
return FAILURE; return FAILURE;
#else #else
case 0x31: case 0x31:
crc_lut = crc8_31_msb_lut; crc_lut = crc8_31_msb_lut;
break; break;
default: default:
return FAILURE; return FAILURE;
#endif #endif
} }
for (int i = 0; i < dataLen; i++) for (int i = 0; i < dataLen; i++)
{ {
crc = crc_lut[(uint8_t)(crc ^ buffPtr[i])]; crc = crc_lut[(uint8_t)(crc ^ buffPtr[i])];
} }
*crcVal = crc; *crcVal = crc;
return SUCCESS; return SUCCESS;
} }
/*****************************************************************************/ /*****************************************************************************/
@@ -322,40 +322,40 @@ int crc8_gen(uint8_t *buffPtr, uint32_t dataLen, uint16_t poly, uint8_t *crcVal)
******************************************************************************/ ******************************************************************************/
int crc16_gen(uint8_t *buffPtr, uint32_t dataLen, uint16_t poly, uint16_t *crcVal) int crc16_gen(uint8_t *buffPtr, uint32_t dataLen, uint16_t poly, uint16_t *crcVal)
{ {
uint16_t crc = CRC16_INIT; uint16_t crc = CRC16_INIT;
const uint16_t *crc_lut = NULL; const uint16_t *crc_lut = NULL;
switch (poly) switch (poly)
{ {
#ifdef __CRC_LSB #ifdef __CRC_LSB
case 0x8005: case 0x8005:
crc_lut = crc16_8005_lsb_lut; crc_lut = crc16_8005_lsb_lut;
break; break;
case 0x1021: case 0x1021:
crc_lut = crc16_1021_lsb_lut; crc_lut = crc16_1021_lsb_lut;
break; break;
default: default:
return FAILURE; return FAILURE;
#else #else
case 0x8005: case 0x8005:
crc_lut = crc16_8005_msb_lut; crc_lut = crc16_8005_msb_lut;
break; break;
case 0x1021: case 0x1021:
crc_lut = crc16_1021_msb_lut; crc_lut = crc16_1021_msb_lut;
break; break;
default: default:
return FAILURE; return FAILURE;
#endif #endif
} }
for (int i = 0; i < dataLen; i++) for (int i = 0; i < dataLen; i++)
{ {
#ifdef __CRC_LSB #ifdef __CRC_LSB
crc = (crc >> 8) ^ crc_lut[(uint8_t)(crc ^ buffPtr[i])]; crc = (crc >> 8) ^ crc_lut[(uint8_t)(crc ^ buffPtr[i])];
#else #else
crc = (crc << 8) ^ crc_lut[(uint8_t)((crc >> 8) ^ buffPtr[i])]; crc = (crc << 8) ^ crc_lut[(uint8_t)((crc >> 8) ^ buffPtr[i])];
#endif #endif
} }
*crcVal = crc; *crcVal = crc;
return SUCCESS; return SUCCESS;
} }
/*****************************************************************************/ /*****************************************************************************/
@@ -373,30 +373,30 @@ int crc16_gen(uint8_t *buffPtr, uint32_t dataLen, uint16_t poly, uint16_t *crcVa
******************************************************************************/ ******************************************************************************/
bool crc8_check(uint8_t *buffPtr, uint32_t dataLen, uint32_t crcOffset, uint16_t poly) bool crc8_check(uint8_t *buffPtr, uint32_t dataLen, uint32_t crcOffset, uint16_t poly)
{ {
int Status = SUCCESS; int Status = SUCCESS;
uint8_t crc_comp; uint8_t crc_comp;
uint8_t crc; uint8_t crc;
if (dataLen == 0) if (dataLen == 0)
{ {
return false; return false;
} }
crc_comp = (uint8_t)(*(buffPtr + crcOffset)); crc_comp = (uint8_t)(*(buffPtr + crcOffset));
Status = crc8_gen(buffPtr, dataLen, poly, &crc); Status = crc8_gen(buffPtr, dataLen, poly, &crc);
if (Status == SUCCESS) if (Status == SUCCESS)
{ {
if (crc == crc_comp) if (crc == crc_comp)
{ {
return true; return true;
} }
else else
{ {
return false; return false;
} }
} }
else else
{ {
return false; return false;
} }
} }
/*****************************************************************************/ /*****************************************************************************/
@@ -414,31 +414,31 @@ bool crc8_check(uint8_t *buffPtr, uint32_t dataLen, uint32_t crcOffset, uint16_t
******************************************************************************/ ******************************************************************************/
bool crc16_check(uint8_t *buffPtr, uint32_t dataLen, uint32_t crcOffset, uint16_t poly) bool crc16_check(uint8_t *buffPtr, uint32_t dataLen, uint32_t crcOffset, uint16_t poly)
{ {
int Status = SUCCESS; int Status = SUCCESS;
uint16_t crc_comp; uint16_t crc_comp;
uint16_t crc; uint16_t crc;
if (dataLen == 0) if (dataLen == 0)
{ {
return false; return false;
} }
crc_comp = (uint16_t)(*(buffPtr + crcOffset)); crc_comp = (uint16_t)(*(buffPtr + crcOffset));
crc_comp |= (uint16_t)((*(buffPtr + crcOffset + 1)) << 8); crc_comp |= (uint16_t)((*(buffPtr + crcOffset + 1)) << 8);
Status = crc16_gen(buffPtr, dataLen, poly, &crc); Status = crc16_gen(buffPtr, dataLen, poly, &crc);
if (Status == SUCCESS) if (Status == SUCCESS)
{ {
if (crc == crc_comp) if (crc == crc_comp)
{ {
return true; return true;
} }
else else
{ {
return false; return false;
} }
} }
else else
{ {
return false; return false;
} }
} }
/** @}*/ /** @}*/
df_datalink_sp/src/util/list.c
+1 -1
View File
@@ -1,5 +1,5 @@
/******************************************************************************* /*******************************************************************************
* @file list.c * @file list.c
* @brief List library implementation * @brief List library implementation
* @author Mihail Chindris ([email protected]) * @author Mihail Chindris ([email protected])
******************************************************************************** ********************************************************************************