rag_agent/RAG-TEST-TOOLS/PrjAttCtrlMng/AttMath.h

/*
 * 
 *
 * 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