通过dmp获取到四元数后,对四元数进行归一化:
void quaternionNormalize(Quaternion *quaternion){
float magnitude;
magnitude = sqrtf((quaternion->x * quaternion->x) +
(quaternion->y * quaternion->y) +
(quaternion->z * quaternion->z) +
(quaternion->w * quaternion->w));
quaternion->x /= magnitude;
quaternion->y /= magnitude;
quaternion->z /= magnitude;
quaternion->w /= magnitude;
}
通过串口发送到上位机,上位机是用C#+csgl写的,姿态显示正确: