Ickey_king
获得 1 次赞
帮助过28人
超声波模块测量有局限。一般都是3m左右。 想要再测量高一些的话,使用气压模块ms5611模块。 可以融合超声波和气压测量。
使用四元数算法进行计算,下面有代码看一下 使用mpu6050读取的参数,代入到下面的函数,就可以了。 void mix_gyrAcc_crossMethod(quaternion * attitude,const float gyr[3],const float acc[3],float interval) { const static float FACTOR = 0.001;//取接近0的数 // float w_q = attitude->w; float x_q = attitude->x; float y_q = attitude->y; float z_q = attitude->z; float x_q_2 = x_q * 2; float y_q_2 = y_q * 2; float z_q_2 = z_q * 2; // // 加速度计的读数,单位化。 float a_rsqrt = math_rsqrt(acc[0]*acc[0]+acc[1]*acc[1]+acc[2]*acc[2]); float x_aa = acc[0] * a_rsqrt; float y_aa = acc[1] * a_rsqrt; float z_aa = acc[2] * a_rsqrt; //加速度计测量出的加速度向量(载体坐标系下) // // 载体坐标下的重力加速度向量,单位化。 float x_ac = x_q*z_q_2 - w_q*y_q_2; float y_ac = y_q*z_q_2 + w_q*x_q_2; //通过四元数旋转矩阵与地理坐标系下的重力加速度向量[0 0 0 1]叉乘得到载体坐标系下的重力加速度向量 float z_ac = 1 - x_q*x_q_2 - y_q*y_q_2;//(主要)角速度计测出的四元数表示的载体坐标系下的重力加速度向量(这里已转换成载体坐标系下) // // 测量值与常量的叉积。 float x_ca = y_aa * z_ac - z_aa * y_ac; float y_ca = z_aa * x_ac - x_aa * z_ac; float z_ca = x_aa * y_ac - y_aa * x_ac;//角速度计测出的角度误差,叠加的FACTOR大小可以实验试凑 // // 构造增量旋转。 float delta_x = gyr[0] * interval / 2 + x_ca * FACTOR; float delta_y = gyr[1] * interval / 2 + y_ca * FACTOR; float delta_z = gyr[2] * interval / 2 + z_ca * FACTOR; // // 融合,四元数乘法。 attitude->w = w_q - x_q*delta_x - y_q*delta_y - z_q*delta_z; attitude->x = w_q*delta_x + x_q + y_q*delta_z - z_q*delta_y; attitude->y = w_q*delta_y - x_q*delta_z + y_q + z_q*delta_x; attitude->z = w_q*delta_z + x_q*delta_y - y_q*delta_x + z_q; quaternion_normalize(attitude);//归一化 }
这个移植ucos到A15的芯片上,没有什么意义吧。直接使用linux的程序就OK了。 移植ucos参考一下,移植到stm32上面的参考方法试试。