اولا که این خیلی هم «معمولی» نیست بعدش
تابع شما یک ورودی بصورت استاکچر داره به نام data از نوع IMU_DATA که نتیجه محاسبه pitch roll yaw توی همون استراکچر ریخته میشه
نکته بعدی اینه که در محاسبه یک استراکچر دیگه وجود داره به نام MPU_Data از نوع MPU6050Data که باید قبلا تعریف و مقدار دهی شده باشه ظاهرا داده های یک سخت افزار توی اون هست حالا توی اینجا من یک سری مقدار رو به اون دادم مثل .1 و .23 .که خودتون باید درستش رو بعدا اصلاح کنید
#include <stdio.h>
#include <math.h>
typedef struct IMU_DATA{
float pitch;
float yaw;
float roll;
} IMU_DATA;
typedef struct MPU6050Data{
double Accel_X;
double Accel_Y;
double Accel_Z;
double Gyro_X ;
double Gyro_Y;
double Gyro_Z;
MPU6050Data():
Accel_X(0.1),
Accel_Y(0.23),
Accel_Z(0.12),
Gyro_X(0.43),
Gyro_Y(0.56),
Gyro_Z(0.25){}
} ;
void IMU_Poll(IMU_DATA *data);
int main()
{
IMU_DATA data;
IMU_Poll(&data);
printf("\n pitch=%f\n yaw=%f\n roll=%f",data.pitch,data.yaw,data.roll);
}
void IMU_Poll(IMU_DATA *data)
{
static float pitch_n , pitch,pitch_w;
static float yaw_n , yaw,yaw_w;
static float roll_n , roll,roll_w;
static MPU6050Data MPU_Data;
pitch_n=(acos((float)MPU_Data.Accel_Z/sqrt(MPU_Data.Accel_X*MPU_Data.Accel_X+MPU_Data.Ac cel_Z*MPU_Data.Accel_Z))-1.58925 )*57.297469;// Accelerator degree
pitch_w=( ((float)(MPU_Data.Gyro_Y)) / 131.07 ); // angular speed calculation
pitch=0.98*(pitch+pitch_w*0.01)+0.02*pitch_n ; // complementry filter
data->pitch=pitch;
roll_n=(acos((float)MPU_Data.Accel_Y/sqrt(MPU_Data.Accel_Y*MPU_Data.Accel_Y+MPU_Data.Ac cel_Z*MPU_Data.Accel_Z))-1.58925 )*57.297469;// Accelerator degree
roll_w=( ((float)(MPU_Data.Gyro_X)) / 131.07 ); // angular speed calculation
roll=0.98*(pitch+roll_w*0.01)+0.02*roll_n ; // complementry filter
data->roll=roll;
yaw_n=(acos((float)MPU_Data.Accel_X/sqrt(MPU_Data.Accel_X*MPU_Data.Accel_X+MPU_Data.Ac cel_Y*MPU_Data.Accel_Y))-1.58925 )*57.297469;// Accelerator degree
yaw_w=( ((float)(MPU_Data.Gyro_Z)) / 131.07 ); // angular speed calculation
yaw=0.98*(yaw+yaw_w*0.01)+0.02*yaw_n ; // complementry filter
data->yaw=yaw;
}