** 1 **
** 前言 **
上期我们介绍了如何使用STM32F103C8T6来获取HMC5883L数字磁力计的XYZ轴的磁场数据。
并且我们利用磁场数据来计算角度值以及Z轴倾角。
但是这种调试的方法来查看数据确实太捞了，于是我们在这个的基础上，利用Python简单的对数据进行一下三维显示。
在实际的使用过程中发现
** 2 **
** Python **
这方面不做过多赘述了，如果有朋友感兴趣的话，可以主页找我联系方式获取或者交流。
主要是通过串口获取数据，之后用两个3D开源库pygame和OpenGL来显示数据。
def parse_serial_data(self, data): try: # 解析数据字符串 values = data.strip().split(',') if len(values) == 3: h_value = float(values[0].split(':')[1]) m_value = float(values[1].split(':')[1]) i_value = float(values[2].split(':')[1]) self.rotation_x = i_value self.rotation_y = h_value return True except: return False
例如这里利用串口获取数据之后，解析获取角度还有倾角。之后绘图进行简单显示，这里就不作过多赘述了。
** 3 **
** 后言 **
在使用传感器的过程中我发现：当我的Y轴方向和北极方向重叠（角度为0的时候）Z轴的倾角为30度左右，这个在上期测试的时候就发现了。
在Y轴方向和磁场南极重合的时候，Z轴倾角为负30度。Y轴方向和在东西方向时则没有偏差，Z轴倾角都是0。
这是由于地磁场和水平方向存在着夹角，这个夹角和纬度相关。
杭州的纬度在30°左右，所以磁场方向和水平角度相差为30°左右，当我把模块方向转过来的时候，就会就正好相反。
当我们的防止方向在东西方向，由于东西方向不存在磁场，磁场与Y轴正交。因此Z轴的磁场强度分量为0。因此当模块旋转九十度之后，倾角会不变。因此模块在水平摆放的时候，对水平角度旋转的测量极为精确，但是改变倾角，其他角度旋转的时候，就没这么简单了。
这源自于我的计算方法的错误，模块的Y轴指向磁场方向的时候，这时候XYZ坐标和地理意义上的南北极坐标方向一致。（Z轴偏差先不考虑）
但是当旋转了模块之后，其实XYZ的坐标系也发生了变换，并不能简单的用之前的方法来计算XYZ轴。
如果我们记原本的向量坐标为a1=（x1,y1,z1）那么之后我们得到的向量的结果为a2= (x2,y2,z2)。
a2是在a1的基础上通过旋转获得的，因此我们可以找一个旋转矩阵P。
旋转矩阵的表达式可以使用罗德里格公式
在此之前我们需要知道a2和a1的旋转轴，但是由于我们是任意旋转的，不知道旋转轴。因此我们要先假设一个旋转轴v，可以用a1叉乘a2归一化来获得。当然原理比较麻烦，直接上程序：
void calculateRotationMatrix(HMC5883L_Measure_t rawData, HMC5883L_Measure_t measures, float R[3][3]) { // 1. 提取原始向量和旋转后的向量 float raw[3] = { rawData.gauss.x, rawData.gauss.y, rawData.gauss.z }; float rotated[3] = { measures.gauss.x, measures.gauss.y, measures.gauss.z }; // 2. 计算旋转轴 (叉积) float axis[3]; axis[0] = raw[1] * rotated[2] - raw[2] * rotated[1]; // y1*z2 - z1*y2 axis[1] = raw[2] * rotated[0] - raw[0] * rotated[2]; // z1*x2 - x1*z2 axis[2] = raw[0] * rotated[1] - raw[1] * rotated[0]; // x1*y2 - y1*x2 // 计算旋转轴的长度 (叉积的模) float axisLength = sqrt(axis[0]*axis[0] + axis[1]*axis[1] + axis[2]*axis[2]); // 如果旋转轴长度为0，说明两个向量是平行的，不需要旋转 if (axisLength == 0.0f) { // 向量平行时，旋转矩阵为单位矩阵 R[0][0] = R[1][1] = R[2][2] = 1.0f; R[0][1] = R[0][2] = R[1][0] = R[1][2] = R[2][0] = R[2][1] = 0.0f; return; } // 将旋转轴单位化 float kx = axis[0] / axisLength; float ky = axis[1] / axisLength; float kz = axis[2] / axisLength; // 3. 计算旋转角度 (通过点积) float dotProduct = raw[0]*rotated[0] + raw[1]*rotated[1] + raw[2]*rotated[2]; float magnitudeRaw = sqrt(raw[0]*raw[0] + raw[1]*raw[1] + raw[2]*raw[2]); float magnitudeRotated = sqrt(rotated[0]*rotated[0] + rotated[1]*rotated[1] + rotated[2]*rotated[2]); float cosTheta = dotProduct / (magnitudeRaw * magnitudeRotated); float theta = acos(cosTheta); // 旋转角度 // 4. 使用罗德里格公式计算旋转矩阵 float c = cos(theta); float s = sin(theta); // 旋转矩阵 R R[0][0] = c + kx*kx*(1 - c); R[0][1] = kx*ky*(1 - c) - kz*s; R[0][2] = kx*kz*(1 - c) + ky*s; R[1][0] = ky*kx*(1 - c) + kz*s; R[1][1] = c + ky*ky*(1 - c); R[1][2] = ky*kz*(1 - c) - kx*s; R[2][0] = kz*kx*(1 - c) - ky*s; R[2][1] = kz*ky*(1 - c) + kx*s; R[2][2] = c + kz*kz*(1 - c);}
就可以得到a2 = R * a1；
我们可以通过a2和a1逆推旋转矩阵R。
之后再利用旋转矩阵，分别计算出XYZ三轴的旋转分量。
float radToDeg(float radians) { return radians * 180.0f / 3.1415;}
void extractEulerAngles(float R[3][3], float* thetaX, float* thetaY, float* thetaZ) { // 计算绕 Y 轴的旋转角度 (pitch) *thetaY = asin(-R[2][0]); // 计算绕 X 轴的旋转角度 (roll) *thetaX = atan2(R[2][1], R[2][2]); // 计算绕 Z 轴的旋转角度 (yaw) *thetaZ = atan2(R[1][0], R[0][0]); // 将弧度转换为度数 *thetaX = radToDeg(*thetaX); *thetaY = radToDeg(*thetaY); *thetaZ = radToDeg(*thetaZ);}
这样子就获得了在原本坐标系下的旋转角度。我们可以用这个值来修正模块姿态变换导致的测量变换。
（不过怎么修正还是个问题，过段时间在研究）