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 /*<2A><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ͣ<EFBFBD>flaot<6F><74><EFBFBD><EFBFBD> double<6C><65><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>*/
|
|||
|
|
|
|||
|
|
#include "math.h"
|
|||
|
|
#include "..\PrjCommon\TypeDef.h"
|
|||
|
|
|
|||
|
|
/*<2A><>ѧ<EFBFBD><D1A7>,<2C><><EFBFBD><EFBFBD>ʵ<EFBFBD>ʺ<EFBFBD><CABA><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ٸ<EFBFBD><D9B8><EFBFBD> */
|
|||
|
|
#define POSE_SINF sin
|
|||
|
|
#define POSE_COSF cos
|
|||
|
|
#define POSE_TANF tan
|
|||
|
|
#define POSE_ASINF asin
|
|||
|
|
|
|||
|
|
//#ifndef MINMODULE_TYPE
|
|||
|
|
#define POSE_ACOSF acos
|
|||
|
|
//#else /*<2A><>Сģʽ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 /*<2A><>Сģʽ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) //<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
#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<CEAA><30><EFBFBD>ж<EFBFBD>*/
|
|||
|
|
#define Mag_NtTT (1.0E-9)
|
|||
|
|
#define SENSOR_ZERO (1.0E-10) /*ԭ<><D4AD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ϊ<EFBFBD><CEAA>ֵ<EFBFBD><D6B5><EFBFBD>ж<EFBFBD>*/
|
|||
|
|
|
|||
|
|
/* LEFT_1OR2<52>꣺ѡ<EAA3BA><D1A1>ƫ<EFBFBD><C6AB>1λ<31><CEBB>2λ */
|
|||
|
|
#define TWO_BIT1 0X0003 /*<2A><><EFBFBD><EFBFBD>λ<EFBFBD><CEBB>ȡ<EFBFBD><C8A1><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʶij<CAB6><C4B3><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>쳣ԭ<ECB3A3><D4AD> */
|
|||
|
|
#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
|
|||
|
|
|
|||
|
|
|
|||
|
|
|