如何将四元数转换成无人机姿态角？

适用机型：M600系列、A3、N3、M210系列、M210 V2系列、M30、M300、M350、M30、M3E/T

SDK版本：OSDK3.7&OSDK3.8 PSDK3.5+

telemetry只提供了四元数话题（TOPIC_QUATERNION），并没有提供姿态角的获取方式。而四元数可以借助欧拉公式转换成姿态角，转换程序如下：

Vector3f FlightSample::quaternionToEulerAngle(
const Telemetry::Quaternion& quat) {
Telemetry::Vector3f eulerAngle;
double q2sqr = quat.q2 * quat.q2;
double t0 = -2.0 * (q2sqr + quat.q3 * quat.q3) + 1.0;
double t1 = 2.0 * (quat.q1 * quat.q2 + quat.q0 * quat.q3);
double t2 = -2.0 * (quat.q1 * quat.q3 - quat.q0 * quat.q2);
double t3 = 2.0 * (quat.q2 * quat.q3 + quat.q0 * quat.q1);
double t4 = -2.0 * (quat.q1 * quat.q1 + q2sqr) + 1.0;
t2 = (t2 > 1.0) ? 1.0 : t2;
t2 = (t2 < -1.0) ? -1.0 : t2;
eulerAngle.x = atan2(t3, t4); //roll
eulerAngle.y = asin(t2);      //pitch
eulerAngle.z = atan2(t1, t0); //yaw
return eulerAngle;
}

使用示例，在C sample的main函数中使用：

    T_DjiFcSubscriptionQuaternion quaternion = {0};

    T_DjiDataTimestamp timestamp = {0};

    static T_DjiAttitude3d s_aircraftAttitude = {0};

    returnCode = DjiFcSubscription_SubscribeTopic(DJI_FC_SUBSCRIPTION_TOPIC_BATTERY_INFO, DJI_DATA_SUBSCRIPTION_TOPIC_10_HZ,

                                               NULL);
      while (1) {
         returnCode = DjiFcSubscription_GetLatestValueOfTopic(DJI_FC_SUBSCRIPTION_TOPIC_BATTERY_INFO,

                                                              (uint8_t *) &quaternion,

                                                              sizeof(T_DjiFcSubscriptionQuaternion),

                                                              &timestamp);

            if (returnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS) {

                USER_LOG_ERROR("get topic quaternion value error.");

            }

            returnCode = DjiTest_GimbalCalculateGroundAttitudeBaseQuaternion(quaternion, &s_aircraftAttitude);

            if (returnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS) {

                USER_LOG_ERROR("calculate and update aircraft attitude error.");

            } else {

                 USER_LOG_INFO("============leon========yaw is %f, timestamp is %u", s_aircraftAttitude.yaw, timestamp);

            }

        sleep(1);

    }

}

static T_DjiReturnCode DjiTest_GimbalCalculateGroundAttitudeBaseQuaternion(T_DjiFcSubscriptionQuaternion quaternion,

                                                                           T_DjiAttitude3d *attitude)

{

    double aircraftPitchInRad;

    double aircraftRollInRad;

    double aircraftYawInRad;

    if (attitude == NULL) {

        USER_LOG_ERROR("Input argument is null.");

        return DJI_ERROR_SYSTEM_MODULE_CODE_INVALID_PARAMETER;

    }

    aircraftPitchInRad = asin(2 * ((double) quaternion.q0 * quaternion.q2 - (double) quaternion.q3 * quaternion.q1));

    attitude->pitch = aircraftPitchInRad * 180 / DJI_PI * 10;

    aircraftRollInRad = atan2(2 * ((double) quaternion.q0 * quaternion.q1 + (double) quaternion.q2 * quaternion.q3),

                              (double) 1 -

                              2 * ((double) quaternion.q1 * quaternion.q1 + (double) quaternion.q2 * quaternion.q2));

    attitude->roll = aircraftRollInRad * 180 / DJI_PI * 10;

    aircraftYawInRad = atan2(2 * ((double) quaternion.q0 * quaternion.q3 + (double) quaternion.q1 * quaternion.q2),

                             (double) 1 -

                             2 * ((double) quaternion.q2 * quaternion.q2 + (double) quaternion.q3 * quaternion.q3));

    attitude->yaw = aircraftYawInRad * 180 / DJI_PI * 10;

    return DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS;

}

此组别内的文章

评论

此组别内的文章

相关文章