0

I used MPU9250 to collect the data of the xyz axis as well as the acceleration, angular velocity, gravity, and want to use these data to construct a plane graph to record the trajectory of the movement of the sphere, but I don't know where to start, the following is my program

#include <ansi_c.h> #include <analysis.h> #include <utility.h> #include <rs232.h> #include <cvirte.h>
#include <userint.h> #include "Bluetooth.h" #include "RS232Set.h"

#define Wavelength 10000
#define Showlength 500
#define Acel_2000mg 0.06103515625 // Unit = mg/points = 2000/2^15 = 2000/32768 #define Acel_4000mg 0.12207031250 #define Gyro_250dps 0.00762939453125 // Unit = dps/points = 250/32768 #define Gyro_500dps 0.01525878906250 #define acelKm 0.1 //0.1 #define gyroKm 0.1 //0.1 #define compassKm 0.1 //0.1 #define PI 3.141593f FILE *file;

static int panelHandle; static int rs232panel;
int port_open, portnum,baud,parity,databit,stopbit,inputq,outputq;
char devicename[30]; double readbuf[13][Wavelength]; unsigned int ind=0,nj=0; short acel_x, acel_y, acel_z, gyro_x, gyro_y, gyro_z; double acel_x_sum_pre=0, acel_y_sum_pre=0, acel_z_sum_pre=0, gyro_x_sum_pre=0, gyro_y_sum_pre=0, gyro_z_sum_pre=0 ; double acel_x_sum_pre_avg=0, acel_y_sum_pre_avg=0, acel_z_sum_pre_avg=0, gyro_x_sum_pre_avg=0, gyro_y_sum_pre_avg=0, gyro_z_sum_pre_avg=0 ; double acel_x_sd_pre=0, acel_y_sd_pre=0, acel_z_sd_pre=0, gyro_x_sd_pre=0, gyro_y_sd_pre=0, gyro_z_sd_pre=0 ; double gyro_x_p=0,gyro_x_n=0,gyro_y_p=0,gyro_y_n=0,gyro_z_p=0,gyro_z_n=0; unsigned int counter; double data[6]; int preindex=0, precount=0, precounter=0, stopbit=0; double acel_ratio,acel_ratio_pre; double acel_data[3], acel_angle[3],acel_data_pre[3],acel_angle_avg[3]; double gyro_print[3]; static double currentdatetime; int hour,minute,month,day,year; double second; char savefilename[100];

/* AI */ /*int portnum2,d=0,data_length=40; char devicename2[30]; int X1,X2,X3,X4,X5,X6,X7,X8,X9,Y1,Y2,Y3,Y4,Y5,Y6,Y7,Y8,Y9;*/

/******************************************************************************************** Acel:加速計(重力) Gyro:陀螺儀(角速度) *********************************************************************************************/

/******************************************************************************************** * Kalman recurcive: 將數據雜訊濾掉 * Golbal variables: acelKm,gyroKm,compassKm (由SETUP tab page 取得) * intput: double predata[18] 前一組量測數據,已經單位化了 * double curdata[18] 要處理的量測數據 * int axies_num 9軸或是6軸 * output: double curdata[18] 改寫完,輸出
*********************************************************************************************/ void Kalman(double* data, int axies_num) { static double pre_data[9]={0.0f}; static int count=0; int i;

if(count==0)
count++ ; else { for(i=0; i<3 ; i++) { data[i]=(data[i]-pre_data[i])*acelKm+pre_data[i]; //加速計 data[i+3]=(data[i+3]-pre_data[i+3])*gyroKm+pre_data[i+3]; //陀螺儀 if(axies_num > 6) data[i+6]= (data[i+6]- pre_data[i+6])*compassKm + pre_data[i+6]; //磁力計 } } memmove(pre_data, data, axies_num*sizeof(double)); } /***********************************************************************************************/

/*********************************************************************************************** * gravitytoangle gravity conver to angle * intput: acel_data[3] 輸入值 * output: acel_angle[3] 輸出角度 (度度量) * acel_ratio 輸出與重力場的比例 abs(|a|-1000mg)/1000mg *Note:只有重力部分, 需要先解X軸在帶入軸解.但都存在GimberLock ************************************************************************************************/ double gravitytoangle(double *acel_data, double *acel_angle) { double gx,gy,gz; double temp; double ratio=0.0f; int i;
double theata=34.0f/180.0f*PI;

acel_angle\[2\]=0.0f;

//Normalization vector temp=0.0f; for (i=0; i<3; i++) temp += acel_data[i]*acel_data[i]; temp=sqrt(temp); ratio = fabs((temp-1000.0f)/1000.0f ); gx=acel_data[0]/temp; gy=acel_data[1]/temp; gz=acel_data[2]/temp; if((gx != 1.0f) && (gx != -1.0f)) { acel_angle[0]= atan2(gy,gz); acel_angle[1]=atan2( -1.0f*gx, gz/cos(acel_angle[0])); } else { if( gx == 1.0f) {
acel_angle[0] =999.0f; acel_angle[1] = PI/(-2.0f); }
else { acel_angle[0] =999.0f;
acel_angle[1] = PI/2.0f;
}
} acel_angle[0] = acel_angle[0]*180/PI; // 範圍: -PI~PI acel_angle[1] = acel_angle[1]*180/PI; // 範圍: -0.5PI~0.5PI acel_angle[2] = acel_angle[2]*180/PI; // 重力場只有二維，因此沒有Z軸資料 return ratio; } /**************************************************************************************/

int main (int argc, char *argv[]) { if (InitCVIRTE (0, argv, 0) == 0) return -1; /* out of memory */ if ((panelHandle = LoadPanel (0, "Bluetooth.uir", PANEL)) < 0) return -1; DisplayPanel (panelHandle); RunUserInterface (); DiscardPanel (panelHandle);// return 0; }

int CVICALLBACK viewdata (int panel, int control, int event, void *callbackData, int eventData1, int eventData2) { int AutoManual; unsigned int i,j,n,k; unsigned char read_data[14]={0}; unsigned char read_data2[40]={0};

SetSleepPolicy (VAL_SLEEP_NONE);//
ProcessSystemEvents ();// DisableBreakOnLibraryErrors ();//

switch (event) { case EVENT_COMMIT:

  GetCurrentDateTime (&currentdatetime);
  GetDateTimeElements (currentdatetime, &hour, &minute, &second, &month, &day, &year);
  sprintf(savefilename,"%dy%dm%dd%dh%dm.txt",year,month,day,hour,minute);
  file = fopen(savefilename,"w"); /\* ------------------------------------- 存檔 -------------------------------------\*/  
  
    SetCtrlAttribute (panelHandle,PANEL_COMM_SET,ATTR_DIMMED, 1);                 
  GetCtrlVal(panelHandle,PANEL_BINARYSWITCH,&AutoManual);
kennycheng
  • 1

0 Answers0