目录
前言
学习日志：
任务（一）昨天没有写完程序框架 下午（2020.10.19）完成程序框架并且上传
任务（二）看完姿态解算 不然不休息（最多凌晨三点）
一、Crazepony 姿态解算部分代码
IMUSO3Thread（）；//这一段代码
定义了：
void IMUSO3Thread(void)
{
//! Time constant
float dt = 0.01f;
static uint32_t tPrev=0,startTime=0; //us static变量的作用是希望函数中的局部变量的值在函数调用结束后不消失而继续保留原值，即其占用的存储单元不释放，在下一次再调用该函数时，该变量已有值（就是上一次函数调用结束时的值）。
uint32_t now;
uint8_t i;
float euler[3] = {0.0f, 0.0f, 0.0f}; //rad
float Rot_matrix[9] = {1.f, 0.0f, 0.0f, 0.0f, 1.f, 0.0f, 0.0f, 0.0f, 1.f };/*定义单位矩阵 */
float acc[3] = {0.0f, 0.0f, 0.0f}; //m/s^2
float gyro[3] = {0.0f, 0.0f, 0.0f}; //rad/s
float mag[3] = {0.0f, 0.0f, 0.0f};
//在imu开始工作前，需要计算陀螺偏置
static float gyro_offsets_sum[3]= { 0.0f, 0.0f, 0.0f }; // gyro_offsets[3] = { 0.0f, 0.0f, 0.0f },
static uint16_t offset_count = 0;
now=micros();//记录现在的时间
dt=(tPrev>0)?(now-tPrev)/1000000.0f:0;//max = a>b ? a : b; // 如果a>b，则max的值为a，否则max的值为b，也即取a与b中的较大者 第一次的值等于0 以后的值等于两次的时间差
tPrev=now;//更替时间
ReadIMUSensorHandle();//采用低通滤波 读取三轴加速度accb 和角速度 gyro 的AD值 并把它们乘以量程 转化为实际值
/*这一段程序是校准程序 它使用了一段时间来求取初始静止状态的平均值 最后会把校准标志位置1*/
if(!imu.ready)// 如果没有校准的话进入这个子程序
{
if(startTime==0)
startTime=now;
gyro_offsets_sum[0] += imu.gyroRaw[0];
gyro_offsets_sum[1] += imu.gyroRaw[1];
gyro_offsets_sum[2] += imu.gyroRaw[2]; //没校准的话将第一次的 imu.gyroRaw赋值作为初始值
offset_count++; //偏移次数加一
if(now > startTime + GYRO_CALC_TIME)//校准需要一定的时间
{
imu.gyroOffset[0] = gyro_offsets_sum[0]/offset_count;
imu.gyroOffset[1] = gyro_offsets_sum[1]/offset_count;
imu.gyroOffset[2] = gyro_offsets_sum[2]/offset_count; //求取这段时间偏移值的平均值
offset_count=0; //偏移次数等清零 即将校准结束
gyro_offsets_sum[0]=0;
gyro_offsets_sum[1]=0;
gyro_offsets_sum[2]=0;
imu.ready = 1;
startTime=0;
}
return;
}
//采集后的数据还是要放数组的 因为减去了偏移数组 最开始应该为0 等校准结束后飞机移动就不为0了
gyro[0] = imu.gyro[0] - imu.gyroOffset[0];
gyro[1] = imu.gyro[1] - imu.gyroOffset[1];
gyro[2] = imu.gyro[2] - imu.gyroOffset[2];
acc[0] = -imu.accb[0];
acc[1] = -imu.accb[1];
acc[2] = -imu.accb[2];
// NOTE : Accelerometer is reversed.
// Because proper mount of PX4 will give you a reversed accelerometer readings.
//注意:加速度计反转了。
//因为正确安装PX4会给你一个反向的加速度计读数。
/*
输入参数gx，gy，gz分别对应三个轴的角速度，单位是弧度/秒。
输入参数ax，ay，az分别对应三个轴的加速度数据，由于加速度的噪声较大，在从MPU6050读取的时候该数据是采用LPF2pApply_x低通滤波函数的。
输出参数mx，my，mz分别对应三轴的电子罗盘数据，Crazepony四轴暂时没有使用到。
输入参数twoKp，twoKi是控制加速度计修正陀螺仪积分姿态的速度，是定义的一个常量。
输入参数dt是从上一次解算到本次解算的时间差，也就是角速度积分用的时间项。
输入参数twoKp，twoKi的宏定义
*/
NonlinearSO3AHRSupdate(gyro[0], gyro[1], gyro[2],//这是低通滤波器得到的gyro角速度
-acc[0], -acc[1], -acc[2],//这是低通滤波器得到的accb 加速度
mag[0], mag[1], mag[2],//这个是电子罗盘的数据
so3_comp_params_Kp,
so3_comp_params_Ki,
dt);
// Convert q->R, This R converts inertial frame to body frame. 将惯性坐标系转化为机体坐标系
Rot_matrix[0] = q0q0 + q1q1 - q2q2 - q3q3;// 11
Rot_matrix[1] = 2.f * (q1*q2 + q0*q3); // 12
Rot_matrix[2] = 2.f * (q1*q3 - q0*q2); // 13
Rot_matrix[3] = 2.f * (q1*q2 - q0*q3); // 21
Rot_matrix[4] = q0q0 - q1q1 + q2q2 - q3q3;// 22
Rot_matrix[5] = 2.f * (q2*q3 + q0*q1); // 23
Rot_matrix[6] = 2.f * (q1*q3 + q0*q2); // 31
Rot_matrix[7] = 2.f * (q2*q3 - q0*q1); // 32
Rot_matrix[8] = q0q0 - q1q1 - q2q2 + q3q3;// 33
//1-2-3 Representation.
//Equation (290)
//Representing Attitude: Euler Angles, Unit Quaternions, and Rotation Vectors, James Diebel.
// Existing PX4 EKF code was generated by MATLAB which uses coloum major order matrix.
euler[0] = atan2f(Rot_matrix[5], Rot_matrix[8]); //! Roll
euler[1] = -asinf(Rot_matrix[2]); //! Pitch
euler[2] = atan2f(Rot_matrix[1], Rot_matrix[0]);
//DCM . ground to body
for(i=0; i<9; i++)
{
*(&(imu.DCMgb[0][0]) + i)=Rot_matrix[i];
}
imu.rollRad=euler[0];
imu.pitchRad=euler[1];
imu.yawRad=euler[2];
imu.roll=euler[0] * 180.0f / M_PI_F;
imu.pitch=euler[1] * 180.0f / M_PI_F;
imu.yaw=euler[2] * 180.0f / M_PI_F;
}
static void NonlinearSO3AHRSupdate(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz, float twoKp, float twoKi, float dt)
首先是将开机的重力利用加速度计测量出来偏角 转化为四元数
再通过此函数计算加速度方向 与初始重力方向 的叉积误差 进行KI运算存储到 gx gy gz
通过 gx gy gz更新了四元数 （没看懂）
static void NonlinearSO3AHRSupdate(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz, float twoKp, float twoKi, float dt)
{
float recipNorm;//单位化
float halfex = 0.0f, halfey = 0.0f, halfez = 0.0f;
if(bFilterInit == 0)
{
NonlinearSO3AHRSinit(ax,ay,az,mx,my,mz);
bFilterInit = 1;
}
if(!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f)))
{
float halfvx, halfvy, halfvz;
// Normalise accelerometer measurement校正加速度计测量
//归一化，得到单位加速度
recipNorm = invSqrt(ax * ax + ay * ay + az * az);
ax *= recipNorm;
ay *= recipNorm;
az *= recipNorm;
// Estimated direction of gravity and magnetic field估计重力和磁场的方向
halfvx = q1q3 - q0q2;
halfvy = q0q1 + q2q3;
halfvz = q0q0 - 0.5f + q3q3;
// Error is sum of cross product between estimated direction and measured direction of field vectors误差指的是两个向量的不平行程度，和夹角的正弦成正比
halfex += ay * halfvz - az * halfvy;
halfey += az * halfvx - ax * halfvz;
halfez += ax * halfvy - ay * halfvx;
}
// Apply feedback only when valid data has been gathered from the accelerometer or magnetometer//仅当加速度计或磁强计收集到有效数据时才应用反馈
if(halfex != 0.0f && halfey != 0.0f && halfez != 0.0f) {
// Compute and apply integral feedback if enabled//仅当从加速度计或磁强计收集到有效数据时，才应用反馈。//如果启用，则应用积分反馈
if(twoKi > 0.0f) {
gyro_bias[0] += twoKi * halfex * dt; // integral error scaled by Ki
gyro_bias[1] += twoKi * halfey * dt;
gyro_bias[2] += twoKi * halfez * dt;
// apply integral feedback
gx += gyro_bias[0];
gy += gyro_bias[1];
gz += gyro_bias[2];
}
else {
gyro_bias[0] = 0.0f; // prevent integral windup
gyro_bias[1] = 0.0f;
gyro_bias[2] = 0.0f;
}
// Apply proportional feedback
gx += twoKp * halfex;
gy += twoKp * halfey;
gz += twoKp * halfez;
}
// Time derivative of quaternion. q_dot = 0.5*q\otimes omega.
//! q_k = q_{k-1} + dt*\dot{q}
//! \dot{q} = 0.5*q \otimes P(\omega)
dq0 = 0.5f*(-q1 * gx - q2 * gy - q3 * gz);
dq1 = 0.5f*(q0 * gx + q2 * gz - q3 * gy);
dq2 = 0.5f*(q0 * gy - q1 * gz + q3 * gx);
dq3 = 0.5f*(q0 * gz + q1 * gy - q2 * gx);
q0 += dt*dq0;
q1 += dt*dq1;
q2 += dt*dq2;
q3 += dt*dq3;
// Normalise quaternion
recipNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
q0 *= recipNorm;
q1 *= recipNorm;
q2 *= recipNorm;
q3 *= recipNorm;
// Auxiliary variables to avoid repeated arithmetic
q0q0 = q0 * q0;
q0q1 = q0 * q1;
q0q2 = q0 * q2;
q0q3 = q0 * q3;
q1q1 = q1 * q1;
q1q2 = q1 * q2;
q1q3 = q1 * q3;
q2q2 = q2 * q2;
q2q3 = q2 * q3;
q3q3 = q3 * q3;
}
NonlinearSO3AHRSinit（）；
这里应该是假设飞机还没有起飞 用初始位置 估计重力和磁场的方向，计算出 重力下 四元数的值
返回姿态解算函数NonlinearSO3AHRSupdate（）
static void NonlinearSO3AHRSinit(float ax, float ay, float az, float mx, float my, float mz)
{
float initialRoll, initialPitch;
float cosRoll, sinRoll, cosPitch, sinPitch;
float magX, magY;
float initialHdg, cosHeading, sinHeading;
initialRoll = atan2(-ay, -az);//利用加速度计求滚转角
initialPitch = atan2(ax, -az);//利用加速度计求俯仰角
//求出滚转和俯仰角的正余弦值
cosRoll = cosf(initialRoll);
sinRoll = sinf(initialRoll);
cosPitch = cosf(initialPitch);
sinPitch = sinf(initialPitch);
magX = mx * cosPitch + my * sinRoll * sinPitch + mz * cosRoll * sinPitch;
magY = my * cosRoll - mz * sinRoll;
initialHdg = atan2f(-magY, magX);
//求出滚转 俯仰 偏航 的 正余弦 sita /2 值 准备进行四元数转换
cosRoll = cosf(initialRoll * 0.5f);
sinRoll = sinf(initialRoll * 0.5f);
cosPitch = cosf(initialPitch * 0.5f);
sinPitch = sinf(initialPitch * 0.5f);
cosHeading = cosf(initialHdg * 0.5f);
sinHeading = sinf(initialHdg * 0.5f);
//求解四元数
q0 = cosRoll * cosPitch * cosHeading + sinRoll * sinPitch * sinHeading;
q1 = sinRoll * cosPitch * cosHeading - cosRoll * sinPitch * sinHeading;
q2 = cosRoll * sinPitch * cosHeading + sinRoll * cosPitch * sinHeading;
q3 = cosRoll * cosPitch * sinHeading - sinRoll * sinPitch * cosHeading;
// auxillary variables to reduce number of repeated operations, for 1st pass
q0q0 = q0 * q0;
q0q1 = q0 * q1;
q0q2 = q0 * q2;
q0q3 = q0 * q3;
q1q1 = q1 * q1;
q1q2 = q1 * q2;
q1q3 = q1 * q3;
q2q2 = q2 * q2;
q2q3 = q2 * q3;
q3q3 = q3 * q3;
}
四元数→旋转矩阵→欧拉角
// Convert q->R, This R converts inertial frame to body frame. 将四元数转化为旋转矩阵
Rot_matrix[0] = q0q0 + q1q1 - q2q2 - q3q3;// 11
Rot_matrix[1] = 2.f * (q1*q2 + q0*q3); // 12
Rot_matrix[2] = 2.f * (q1*q3 - q0*q2); // 13
Rot_matrix[3] = 2.f * (q1*q2 - q0*q3); // 21
Rot_matrix[4] = q0q0 - q1q1 + q2q2 - q3q3;// 22
Rot_matrix[5] = 2.f * (q2*q3 + q0*q1); // 23
Rot_matrix[6] = 2.f * (q1*q3 + q0*q2); // 31
Rot_matrix[7] = 2.f * (q2*q3 - q0*q1); // 32
Rot_matrix[8] = q0q0 - q1q1 - q2q2 + q3q3;// 33
//1-2-3 Representation.
//Equation (290)
//Representing Attitude: Euler Angles, Unit Quaternions, and Rotation Vectors, James Diebel.
// Existing PX4 EKF code was generated by MATLAB which uses coloum major order matrix.
//将旋转矩阵反解出欧拉角
euler[0] = atan2f(Rot_matrix[5], Rot_matrix[8]); //! Roll
euler[1] = -asinf(Rot_matrix[2]); //! Pitch
euler[2] = atan2f(Rot_matrix[1], Rot_matrix[0]);
//DCM . ground to body
for(i=0; i<9; i++)
{
*(&(imu.DCMgb[0][0]) + i)=Rot_matrix[i];
}
imu.rollRad=euler[0];
imu.pitchRad=euler[1];
imu.yawRad=euler[2];
imu.roll=euler[0] * 180.0f / M_PI_F;
imu.pitch=euler[1] * 180.0f / M_PI_F;
imu.yaw=euler[2] * 180.0f / M_PI_F;二、Crazepony 姿态解算部分流程
如上
总结
提示：这里对文章进行总结：
例如：以上就是今天要讲的内容，本文仅仅简单介绍了pandas的使用，而pandas提供了大量能使我们快速便捷地处理数据的函数和方法。
如上 主要是用了
IMUSO3Thread（）；定义变量
NonlinearSO3AHRSinit（）；假设初始静止状态 利用加计数据求取仅重力作用下的四元数
NonlinearSO3AHRSupdate（）；利用加计求取与初状态下的误差 采用KI运算
扫一扫
1281
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
