00001 #ifndef COMM_UTIL_H 00002 #define COMM_UTIL_H 00003 00004 #include <stdint.h> 00005 #include <math.h> 00006 00007 #include "mav_common/comm_types.h" 00008 00009 #ifdef __cplusplus 00010 extern "C" { 00011 #endif 00012 00013 void normalizeSIAnglePi (float * angle); 00014 void normalizeSIAngle2Pi(float * angle); 00015 00016 /* 00017 // ****************** accel *********************** 00018 00019 int32_t SItoCommAcc(float acc); 00020 float commToSIAcc(int32_t acc); 00021 00022 // ****************** angle *********************** 00023 00024 float commToSIAngle(uint16_t angle); 00025 uint16_t SIToCommAngle(float angle); 00026 00027 // ****************** position *********************** 00028 00029 float commToSIPosition(int32_t pos); 00030 int32_t SIToCommPosition(float pos); 00031 00032 float commToSIKFCov(uint32_t cov); 00033 uint32_t SIToCommKFCov(float cov); 00034 float commToSIVelocity(int16_t vel); 00035 int16_t SIToCommVelocity(float vel); 00036 00037 // **** for direct motor control 00038 00039 int16_t SItoCommCmdRoll (float roll); 00040 int16_t SItoCommCmdPitch (float pitch); 00041 int32_t SItoCommCmdYawRate (float yaw_rate); 00042 int16_t SItoCommCmdThrust (float thrust); 00043 00044 float commToSICmdRoll (int16_t roll); 00045 float commToSICmdPitch (int16_t pitch); 00046 float commToSICmdYawRate (int32_t yaw_rate); 00047 float commToSICmdThrust (int16_t thrust); 00048 */ 00049 #ifdef __cplusplus 00050 }; 00051 #endif 00052 00053 #endif