132 lines
4.4 KiB
C
132 lines
4.4 KiB
C
/*
|
||
*
|
||
*
|
||
* Created: 2022/11/4 11:02:40
|
||
* Author: wangzk zhengmengxing
|
||
*/
|
||
#ifndef _ZKMATH_H_
|
||
#define _ZKMATH_H_
|
||
|
||
//#define TYPE_CAL float /*计算所用类型，flaot或者 double，看情况*/
|
||
|
||
#include "math.h"
|
||
#include "..\PrjCommon\TypeDef.h"
|
||
|
||
/*数学库,根据实际函数名再更改 */
|
||
#define POSE_SINF sin
|
||
#define POSE_COSF cos
|
||
#define POSE_TANF tan
|
||
#define POSE_ASINF asin
|
||
|
||
//#ifndef MINMODULE_TYPE
|
||
#define POSE_ACOSF acos
|
||
//#else /*最小模式ACOS*/
|
||
//#define POSE_ACOSF Poseacosf
|
||
//#endif /*END OF #ifndef MINMODULE_TYPE*/
|
||
|
||
#define POSE_ATANF atan
|
||
#define POSE_ATAN2F atan2
|
||
|
||
//#ifndef MINMODULE_TYPE
|
||
#define POSE_ABSF fabs
|
||
#define POSE_SQRTF sqrt
|
||
//#else /*最小模式SQRT*/
|
||
//#define POSE_ABSF Poseabsf
|
||
//#define POSE_SQRTF Posesqrt
|
||
//#endif /*END OF #ifndef MINMODULE_TYPE*/
|
||
|
||
#define POSE_FLOOR floor
|
||
#define POSE_POW pow
|
||
#define POSE_fmod fmod
|
||
|
||
#define POSE_IN (0xAA) /*IN*/
|
||
#define POSE_OK (0x55) /*OK*/
|
||
#define POSE_NO (0x00) /*NO*/
|
||
|
||
|
||
#define POWER_ON (0xAA)
|
||
#define POWER_OFF (0x55)
|
||
|
||
|
||
#define POSE_PI (3.14159265F) /*PI*/
|
||
#define POSE_2PI (6.283185307F) /*2*PI*/
|
||
#define POSE_05PI (1.570796327F) /*0.5PI*/
|
||
#define POSE_SQRT2 (0.707106781F) /*2^(-1/2)*/
|
||
#define POSE_2T13 (0.0001220703125F) /*2^(-13)*/
|
||
#define POSE_2T11 (0.00048828125F) /*2^(-11)*/
|
||
#define RADIAN2ANG (57.295779513F) /*180/Pi*/
|
||
#define ANG2RADIAN (0.017453292F) /*Pi/180*/
|
||
#define ATTPERIOD_S (0.5F) //软件运行周期
|
||
#define RPM2RADS (0.104719755F) /*Pi/30*/
|
||
#define RADS2RPM (9.549296596425383F) /*30/Pi*/
|
||
//#define RADS2RPM (9.54929659642F) /*30/Pi*/
|
||
|
||
#define POSE_ZERO (1.0E-8) /*为0的判断*/
|
||
#define Mag_NtTT (1.0E-9)
|
||
#define SENSOR_ZERO (1.0E-10) /*原包数据为常值的判断*/
|
||
|
||
/* LEFT_1OR2宏：选择偏移1位或2位 */
|
||
#define TWO_BIT1 0X0003 /*左移位后取反，标识某种数据异常原因 */
|
||
#define ONE_BIT1 0X0001
|
||
#define BIT_1OR2(bitNum) (bitNum==1 ? ONE_BIT1:TWO_BIT1)
|
||
#define BITNUM_1OR2(bitNum) (bitNum==1 ? 1:2)
|
||
#define ZERO_TO_ONE(dataSta,errNo,bitNum) ((dataSta) | ((BIT_1OR2(bitNum)<<(errNo*BITNUM_1OR2(bitNum)))))
|
||
|
||
|
||
//#ifdef MINMODULE_TYPE
|
||
//TYPE_CAL Posesqrt(TYPE_CAL m);
|
||
//TYPE_CAL Poseabsf(TYPE_CAL x);
|
||
//TYPE_CAL Poseacosf(TYPE_CAL x);
|
||
//#endif
|
||
|
||
//#ifndef MINMODULE_TYPE
|
||
//void CalInterPolation(TYPE_CAL * x, TYPE_CAL * y, UINT32 iLen, TYPE_CAL *Xi,TYPE_CAL* tmpTF);
|
||
//void MatrixIcons( TYPE_CAL *A, UINT8 N );
|
||
//void M2Att( TYPE_CAL *M , TYPE_CAL *ca );
|
||
//void Q2Q44( TYPE_CAL *p, TYPE_CAL *rM4, UINT8 kind );
|
||
//void CalTransMatrix_IO( TYPE_CAL *cuR, TYPE_CAL *cuV, TYPE_CAL * cAoi );
|
||
//BOOL CalEarthLonLat(TYPE_CAL * cuR, TYPE_CAL * alf, TYPE_CAL * dlt);
|
||
//void CalTransMatrix_IG( TYPE_CAL * cuR, TYPE_CAL *cAie );
|
||
//void CalArcSpdOI(TYPE_CAL Omg0, TYPE_CAL * Omgoi);
|
||
//void CalQPlusQ(TYPE_CAL * Qb1, TYPE_CAL * Qb2, TYPE_CAL * Qres);
|
||
//#endif
|
||
|
||
|
||
TYPE_CAL PoseLimitInOne(const TYPE_CAL X);
|
||
char CalPFlag(TYPE_CAL Data);
|
||
void Posedot(TYPE_CAL* x1, TYPE_CAL* x2, UINT8 ucN, TYPE_CAL * pRes);
|
||
void cross(TYPE_CAL * x1, TYPE_CAL * x2, TYPE_CAL * x3);
|
||
void CalNormal( TYPE_CAL* x, UINT8 ucN, TYPE_CAL * pRes );
|
||
void CalMakeCrossMatrix(TYPE_CAL * inLine, TYPE_CAL * outMatrix, UINT16 iLen);
|
||
void MatrixProductHL( TYPE_CAL *A1, TYPE_CAL *A2, TYPE_CAL *A3, UINT8 H, UINT8 L, UINT8 F );
|
||
void MatrixTransposeHL(TYPE_CAL * A1, TYPE_CAL * A2, UINT8 H, UINT8 L );
|
||
void InvsymMatrix3(TYPE_CAL *cI, TYPE_CAL *cO, UINT8 *cs, TYPE_CAL * ef);
|
||
UINT8 CalVecArc(TYPE_CAL * VecX, TYPE_CAL * VecY, TYPE_CAL *pRet);
|
||
void unitary(TYPE_CAL* pVector, UINT8 ucN);
|
||
void SortUint16( UINT16 *cd, UINT8 n );
|
||
void MatrixPlusVectorN(TYPE_CAL* pMatrix, TYPE_CAL* pVector, TYPE_CAL* pReturn, UINT8 M, UINT8 N );
|
||
void Q2Att( TYPE_CAL *cq, TYPE_CAL *ca );
|
||
void Att2M( TYPE_CAL *att, TYPE_CAL *A );
|
||
void Att2Q( TYPE_CAL *iM3, TYPE_CAL *cq );
|
||
void CalTransMatrix_IO( TYPE_CAL *cuR, TYPE_CAL *cuV, TYPE_CAL * cAoi );
|
||
void CalQuatInv(TYPE_CAL *Qb1, TYPE_CAL *Qres);
|
||
UINT8 DoGetCrcCheckU8(UINT8 * BeginAddr,UINT8 CheckLength);
|
||
float PoseGetInt32From24(UINT8* Ldata);
|
||
void AglSpdOofICal(TYPE_CAL *cuR, TYPE_CAL *cuV, TYPE_CAL *Woi);
|
||
void AttLongKutaCal(TYPE_CAL * pWbxl, TYPE_CAL * pQbxl, TYPE_CAL * pQbx);
|
||
void Eul312A2Quat( TYPE_CAL *ca, TYPE_CAL *cq );
|
||
void Matrix2Eul312A( TYPE_CAL *M , TYPE_CAL *ca );
|
||
void DeviAngleLimit(TYPE_CAL * Ang);
|
||
UINT8 count_onebit(UINT8 n,UINT8 eff_bit);
|
||
|
||
|
||
|
||
|
||
|
||
|
||
|
||
#endif
|
||
|
||
|
||
|