关于大疆OSDK使用的前置步骤请参考OSDK前置说明
参考文件：
-
Onboard-SDK-master\osdk-core\api\inc\dji_broadcast.hpp
-
Onboard-SDK-master\osdk-core\api\src\dji_broadcast.cpp
-
Onboard-SDK-master\sample\platform\linux\telemetry\telemetry_sample.cpp
飞机参数获取
飞机参数获取方面主要是获取飞机的GPS、姿态角、机身速度等。
飞机的坐标系采用的是北东地坐标系，机头为正北。机身速度结构体包含北向速度、东向速度以及天向速度，负值则方向相反。
辅助函数
飞机参数无法通过函数直接获取，需要通过订阅消息的方式进行获取。
下面是“一键订阅”飞机数据的函数代码，可修改，如果不想修改的话直接复制到主函数文件中使用也是ok的。
辅助函数已包含消息订阅初始化和广播频率等步骤。
namespace {
struct SubscriptionPackageSpec {
int pkgIndex;
int freqHz;
std::vector<TopicName> topics;
};
void teardownTelemetrySubscription(Vehicle *vehicle,
const std::vector<int> &packageIds,
int responseTimeout)
{
if (vehicle == nullptr || vehicle->subscribe == nullptr) {
return;
}
for (int pkgId : packageIds) {
vehicle->subscribe->removePackage(pkgId, responseTimeout);
}
}
Telemetry::TimeStamp buildTimeStampFromGps(uint32_t gpsDate, uint32_t gpsTime)
{
Telemetry::TimeStamp stamp{};
if (gpsTime == 0) {
const auto now = std::chrono::steady_clock::now().time_since_epoch();
stamp.time_ms = static_cast<uint32_t>(
std::chrono::duration_cast<std::chrono::milliseconds>(now).count());
const auto remainderNs =
std::chrono::duration_cast<std::chrono::nanoseconds>(now).count() % 1000000;
stamp.time_ns = static_cast<uint32_t>(remainderNs);
return stamp;
}
const uint32_t hours = gpsTime / 10000U;
const uint32_t minutes = (gpsTime % 10000U) / 100U;
const uint32_t seconds = gpsTime % 100U;
stamp.time_ms = ((hours * 3600U) + (minutes * 60U) + seconds) * 1000U;
stamp.time_ns = 0;
(void)gpsDate;
return stamp;
}
Telemetry::GlobalPosition fusedToGlobalPosition(const Telemetry::GPSFused &fused)
{
Telemetry::GlobalPosition position{};
position.latitude = fused.latitude;
position.longitude = fused.longitude;
position.altitude = fused.altitude;
position.height = 0.0f;
position.health = static_cast<uint8_t>(
std::min<uint16_t>(fused.visibleSatelliteNumber, 5));
return position;
}
bool initTelemetrySubscription(Vehicle *vehicle,
int responseTimeout,
std::vector<int> &activePackages)
{
if (vehicle == nullptr || vehicle->subscribe == nullptr) {
return false;
}
activePackages.clear();
ACK::ErrorCode subscribeStatus = vehicle->subscribe->verify(responseTimeout);
if (ACK::getError(subscribeStatus) != ACK::SUCCESS) {
ACK::getErrorCodeMessage(subscribeStatus, __func__);
return false;
}
static_assert(sizeof(TypeMap<TOPIC_QUATERNION>::type) +
sizeof(TypeMap<TOPIC_VELOCITY>::type) <= 242,
"50 Hz package exceeds telemetry size limit");
static_assert(sizeof(TypeMap<TOPIC_GPS_FUSED>::type) <= 242,
"10 Hz package exceeds telemetry size limit");
static_assert(sizeof(TypeMap<TOPIC_GPS_DATE>::type) +
sizeof(TypeMap<TOPIC_GPS_TIME>::type) <= 242,
"1 Hz package exceeds telemetry size limit");
const std::vector<SubscriptionPackageSpec> packages = {
{1, 50, {TOPIC_QUATERNION, TOPIC_VELOCITY}},
{2, 10, {TOPIC_GPS_FUSED}},
{3, 1, {TOPIC_GPS_DATE, TOPIC_GPS_TIME}},
};
for (const auto &pkg : packages) {
bool pkgStatus = vehicle->subscribe->initPackageFromTopicList(
pkg.pkgIndex,
static_cast<int>(pkg.topics.size()),
const_cast<TopicName *>(pkg.topics.data()),
false,
pkg.freqHz);
if (!pkgStatus) {
teardownTelemetrySubscription(vehicle, activePackages, responseTimeout);
return false;
}
subscribeStatus =
vehicle->subscribe->startPackage(pkg.pkgIndex, responseTimeout);
if (ACK::getError(subscribeStatus) != ACK::SUCCESS) {
ACK::getErrorCodeMessage(subscribeStatus, __func__);
vehicle->subscribe->removePackage(pkg.pkgIndex, responseTimeout);
teardownTelemetrySubscription(vehicle, activePackages, responseTimeout);
return false;
}
activePackages.push_back(pkg.pkgIndex);
}
return true;
}
} // namespace
姿态数据处理
需要注意的是，Telemetry::Quaternion（姿态结构体）是四元组数据，需要用欧拉公式转换。原理自己了解，这里提供一版可用的欧拉转换函数。
struct EulerAnglesDeg {
double roll; // 横滚角 (deg) φ
double pitch; // 俯仰角 (deg) θ
double yaw; // 偏航角 (deg) ψ
};
/**
* @brief 四元数转欧拉角（ZYX顺序 → 偏航-俯仰-横滚）
* 航空/飞控/机器人领域标准顺序
* @param q 输入四元数（q0=w）
* @param euler 输出欧拉角（度）
* @return 0=成功 -1=四元数模长为0
*/
static inline void Quat_To_EulerZYX_Deg(const DJI::OSDK::Telemetry::Quaternion& q, EulerAnglesDeg& euler)
{
float w = q.q0;
float x = q.q1;
float y = q.q2;
float z = q.q3;
// 快速归一化
float norm = sqrtf(w*w + x*x + y*y + z*z);
if (norm > 1e-8f) {
float inv = 1.0f / norm;
w *= inv; x *= inv; y *= inv; z *= inv;
} else {
euler.roll = euler.pitch = euler.yaw = 0.0f;
return;
}
const float DEG_PER_RAD = 57.29577951308232f;
// Roll
float sinr_cosp = 2.0f * (w*x + y*z);
float cosr_cosp = 1.0f - 2.0f*(x*x + y*y);
euler.roll = atan2f(sinr_cosp, cosr_cosp) * DEG_PER_RAD;
// Pitch
float sinp = 2.0f * (w*y - x*z);
if (fabsf(sinp) >= 1.0f)
euler.pitch = copysignf(90.0f, sinp);
else
euler.pitch = asinf(sinp) * DEG_PER_RAD;
// Yaw
float siny_cosp = 2.0f * (w*z + x*y);
float cosy_cosp = 1.0f - 2.0f*(y*y + z*z);
euler.yaw = atan2f(siny_cosp, cosy_cosp) * DEG_PER_RAD;
}GPS处理
飞机获取的GPS数据是弧度制，需要转为角度，才是常见的108.56°和34.123°这种值。
弧度与角度的转换函数：
#include <cmath>
constexpr double DEG2RAD = M_PI / 180.0;
constexpr double RAD2DEG = 180.0 / M_PI;
// 度 → 弧度
double deg_to_rad(double deg) { return deg * DEG2RAD; }
// 弧度 → 度
double rad_to_deg(double rad) { return rad * RAD2DEG; }示例代码
//此处手动复制一下辅助函数、姿态处理函数和GPS处理函数即可
int main()
{
LinuxSetup linuxEnvironment(argc, argv); // 固定流程
Vehicle *vehicle = linuxEnvironment.getVehicle(); // 固定流程
const char *acm_dev = linuxEnvironment.getEnvironment()->getDeviceAcm().c_str(); // ACM接口 固定流程
// initTelemetrySubscription() 函数调用后自动设置广播和订阅
std::vector<int> activeTelemetryPackages;
const int subscribeTimeout = 1;
if (!initTelemetrySubscription(vehicle, subscribeTimeout, activeTelemetryPackages)) {
std::cerr << "Failed to initialise telemetry subscriptions for M210" << std::endl;
return nullptr;
}
while(1)
{
const Telemetry::Quaternion currentQuaternion =
vehicle->subscribe->getValue<TOPIC_QUATERNION>();
const auto velocityData =
vehicle->subscribe->getValue<TOPIC_VELOCITY>();
const Telemetry::Vector3f currentVelocity = velocityData.data;
const Telemetry::GPSFused fusedGps =
vehicle->subscribe->getValue<TOPIC_GPS_FUSED>();
const uint32_t gpsDate =
vehicle->subscribe->getValue<TOPIC_GPS_DATE>();
const uint32_t gpsTime =
vehicle->subscribe->getValue<TOPIC_GPS_TIME>();
const auto gimbalSnapshot = gimbal->getGimbalData(PAYLOAD_INDEX_0);
{
std::lock_guard<std::mutex> lock(flightdataMutex);
flightdata.velocity = currentVelocity;
flightdata.gimbalData.pitch = gimbalSnapshot.pitch;
flightdata.gimbalData.roll = gimbalSnapshot.roll;
flightdata.gimbalData.yaw = gimbalSnapshot.yaw;
Quat_To_EulerZYX_Deg(currentQuaternion , flightdata.planeAngle);
flightdata.gpsposition.longitude = rad_to_deg(fusedGps.longitude);
flightdata.gpsposition.latitude = rad_to_deg(fusedGps.latitude);
flightdata.gpsposition.altitude = fusedGps.altitude;
// 查看完整数据建议设置一下保留12位小数的打印方式，不过一般GPS保留8位小数就很精准了
// 其它数据通常保留4位小数
cout << "velocity: " << flightdata.velocity.x << " " << flightdata.velocity.y << " " << flightdata.velocity.z << "\n";
cout << std::fixed << std::setprecision(4) << "plane pitch/roll/yaw: " << " " << flightdata.planeAngle.pitch << " " << flightdata.planeAngle.roll << " " << flightdata.planeAngle.yaw << "\n";
cout << std::fixed << std::setprecision(6)<< "GPSposition longitude/latitude/altitude: "<< flightdata.gpsposition.longitude
<< " " << flightdata.gpsposition.latitude << " " << flightdata.gpsposition.altitude << "\n";
cout << "=====================================\n\n" ;
usleep(5000);
}
}
扫一扫
4142
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
