适用机型：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;
}
PSDK中的订阅流程如下，使用示例，在C sample的main函数中使用（注：以下的转换可能不是最新的，请以最新的sample转换为准）：
T_DjiFcSubscriptionQuaternion quaternion = {0};
T_DjiDataTimestamp timestamp = {0};
static T_DjiAttitude3d s_aircraftAttitude = {0};
returnCode = DjiFcSubscription_SubscribeTopic(DJI_FC_SUBSCRIPTION_TOPIC_QUATERNION, DJI_DATA_SUBSCRIPTION_TOPIC_1_HZ,
DjiTest_FcSubscriptionReceiveQuaternionCallback);
while (1) {
sleep(1);
}
}
static T_DjiReturnCode DjiTest_FcSubscriptionReceiveQuaternionCallback(const uint8_t *data, uint16_t dataSize,
const T_DjiDataTimestamp *timestamp)
{
T_DjiFcSubscriptionQuaternion *quaternion = (T_DjiFcSubscriptionQuaternion *) data;
dji_f64_t pitch, yaw, roll;
USER_UTIL_UNUSED(dataSize);
pitch = (dji_f64_t) asinf(-2 * quaternion->q1 * quaternion->q3 + 2 * quaternion->q0 * quaternion->q2) * 57.3;
roll = (dji_f64_t) atan2f(2 * quaternion->q2 * quaternion->q3 + 2 * quaternion->q0 * quaternion->q1,
-2 * quaternion->q1 * quaternion->q1 - 2 * quaternion->q2 * quaternion->q2 + 1) * 57.3;
yaw = (dji_f64_t) atan2f(2 * quaternion->q1 * quaternion->q2 + 2 * quaternion->q0 * quaternion->q3,
-2 * quaternion->q2 * quaternion->q2 - 2 * quaternion->q3 * quaternion->q3 + 1) *
57.3;
if (s_userFcSubscriptionDataShow == true) {
if (s_userFcSubscriptionDataCnt++ % DJI_DATA_SUBSCRIPTION_TOPIC_50_HZ == 0) {
USER_LOG_INFO("receive quaternion data.");
USER_LOG_INFO("timestamp: millisecond %u microsecond %u.", timestamp->millisecond,
timestamp->microsecond);
USER_LOG_INFO("quaternion: %f %f %f %f.", quaternion->q0, quaternion->q1, quaternion->q2,
quaternion->q3);
USER_LOG_INFO("euler angles: pitch = %.2f roll = %.2f yaw = %.2f.\r\n", pitch, roll, yaw);
DjiTest_WidgetLogAppend("pitch = %.2f roll = %.2f yaw = %.2f.", pitch, roll, yaw);
}
}
USER_LOG_INFO("receive quaternion data:timestamp: millisecond %u microsecond %u. quaternion: %f %f %f %f.", timestamp->millisecond,
timestamp->microsecond, quaternion->q0, quaternion->q1, quaternion->q2,
quaternion->q3);
return DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS;
}
评论
0 条评论
请登录写评论。