C语言roll程序,惯性导航C语言程序

#include "stm32f10x_lib.h"

#include "stdlib.h"

#include "stdio.h"

#include "math.h"

#define w_ie 7.29212e-5

#define g 9.78

#define R 6371000

#define RAD 3.1415926/180

#define D 180/3.1415926

#define T 0.01

#define PI 3.1415926

void solve_init(void);

void IMU_sample(void);

void calw_nie(void);

void calw_nen(void);

void calw_nin(void);

void calc_nb(void);

void calw_bin(void);

void calw_bnb(volatile float w_bnb[3],volatile float

w_bib[3]);

void updateQ(void);

void updatec_bn(void);

void cal_angl(void);

void convacce(void);

void updateV(void);

void initC(void);

void updateC(void);

void calPOS(void);

void calvelocity(void);

void jiaozheng(void);

volatile float w_nie[3],w_nen[3],w_nin[3],w_bin[3];

volatile double L_ins,lamda_ins;

volatile float h_ins,v_ins[3],angl[3],v_ins_r[3];

volatile float w_bib1[3],w_bib2[3],w_bib3[3];

volatile float w_bnb1[3],w_bnb2[3],w_bnb3[3];

volatile float c_bn[3][3],c_nb[3][3];

volatile float q[4];

volatile float pitch_ins,roll_ins,yaw_ins;

volatile float f_b[3],f_n[3];

volatile float cc[3][3];

volatile float POS[3];

volatile float v;

volatile float v_ins1[3];//用于位置更新

float tempfhi_cbn[3][3];

float mulitc_bn[3][3];

int I=0;

extern volatile float gx,gy,gz,ax,ay,az;

extern volatile double X[4][1];

extern float pitch0,roll0,yaw0;

extern long smile;

extern int kalman_flag;

extern volatile float L_GPS,lamda_GPS,H_GPS;

extern volatile int jiaozhen;

void cuduizhun(void) //初始对准，用模块输出的三个角求姿态矩阵c_bn的初始值

{

c_bn[0][0]=cos(pitch0)*cos(yaw0);

//北西天

c_bn[0][1]=sin(pitch0)*cos(yaw0)*sin(roll0)-sin(yaw0)*cos(roll0);

c_bn[0][2]=sin(pitch0)*cos(yaw0)*cos(roll0)+sin(yaw0)*sin(roll0);

c_bn[1][0]=cos(pitch0)*sin(yaw0);

c_bn[1][1]=sin(pitch0)*sin(yaw0)*sin(roll0)+cos(yaw0)*cos(roll0);

c_bn[1][2]=sin(pitch0)*sin(yaw0)*cos(roll0)-cos(yaw0)*sin(roll0);

c_bn[2][0]=-sin(pitch0);

c_bn[2][1]=cos(pitch0)*sin(roll0);

c_bn[2][2]=cos(pitch0)*cos(roll0);

q[0]=sqrt(fabs(1+c_bn[0][0]+c_bn[1][1]+c_bn[2][2]))/2;

//求初始四元数

if((c_bn[2][1]-c_bn[1][2])>=0)

q[1]=sqrt(fabs(1+c_bn[0][0]-c_bn[1][1]-c_bn[2][2]))/2;

else

q[1]=-sqrt(fabs(1+c_bn[0][0]-c_bn[1][1]-c_bn[2][2]))/2;

if((c_bn[0][2]-c_bn[2][0])>=0)

q[2]=sqrt(fabs(1-c_bn[0][0]+c_bn[1][1]-c_bn[2][2]))/2;

else</