dji-sdk - matrice600pro 在现实世界中不会被 osdk 旋转
问题描述
我正在开发 Matrice600pro osdk 软件。
当我尝试使用此代码旋转无人机时:
bool rotateToTargetYaw(float targetYaw)
{
float nowYaw;
float yawDiff;
bool blnEmergencyAvoidanceOccured;
int responseTimeout = 1;
int timeoutInMilSec = 10000;
int controlFreqInHz = 50; // Hz
int cycleTimeInMs = 1000 / controlFreqInHz;
int elapsedTimeInMs = 0;
int brakeCounter = 0;
int withinControlBoundsTimeReqmt = 50 * cycleTimeInMs;
int withinBoundsCounter = 0;
int outOfBounds = 0;
int outOfControlBoundsTimeLimit = 10 * cycleTimeInMs; // 10 cycles
//! Main closed-loop receding setpoint position control
while (elapsedTimeInMs < timeoutInMilSec)
{
uint8_t ctrl_flag = ( Control::VERTICAL_VELOCITY | Control::HORIZONTAL_VELOCITY | Control::YAW_ANGLE | Control::HORIZONTAL_BODY | Control::STABLE_ENABLE);
Control::CtrlData data(ctrl_flag, 0, 0, 0, targetYaw);
gVehicle->control->flightCtrl(data);
usleep(cycleTimeInMs * 1000);
elapsedTimeInMs += cycleTimeInMs;
nowYaw = Util::GetYawDeg();
yawDiff = std::abs(Util::degRangeConv(nowYaw - targetYaw));
if(yawDiff < 1.0){
withinBoundsCounter += cycleTimeInMs;
Util::log("withinBoundsCounter add (=%d)\n",withinBoundsCounter);
} else {
if (withinBoundsCounter != 0){
outOfBounds += cycleTimeInMs;
Util::log("outOfBounds add(=%d)\n",outOfBounds);
}
}
//! 3. Reset withinBoundsCounter if necessary
if (outOfBounds > outOfControlBoundsTimeLimit)
{
Util::log("withinBoundsCounter and outOfBounds reset\n");
withinBoundsCounter = 0;
outOfBounds = 0;
}
if (withinBoundsCounter >= withinControlBoundsTimeReqmt)
{
Util::log("goal withinBoundsCounter(%d)withinControlBoundsTimeReqmt(%d)\n",withinBoundsCounter,withinControlBoundsTimeReqmt);
break;
}
}
//! Set velocity to zero, to prevent any residual velocity from position
//! command
while (brakeCounter < withinControlBoundsTimeReqmt) {
gVehicle->control->emergencyBrake();
usleep(cycleTimeInMs * 10);
brakeCounter += cycleTimeInMs;
}
return ACK::SUCCESS;
}
// return current Yaw(deg)
float Util::GetYawDeg() {
TypeMap<TOPIC_QUATERNION>::type quaternion;
quaternion = gVehicle->subscribe->getValue<TOPIC_QUATERNION>();
float yawDeg = std::atan2(
2.0 * (quaternion.q3 * quaternion.q0 + quaternion.q1 * quaternion.q2) ,
- 1.0 + 2.0 * (quaternion.q0 * quaternion.q0 + quaternion.q1 * quaternion.q1));
return yawDeg * RAD2DEG;
}
当我使用模拟器(DJI 助手)调用此函数时,rotateToTargetYaw() 确实有效,但它在现实世界中根本不起作用。
订阅 QUATERNION 确实没有问题。
我用 DJI 助手检查了“启用 API 控制”设置是否为真。
有人有什么建议吗?
解决方案
推荐阅读
- php - Php artisan schedule dosen't execute the command
- apache-spark - AWS EMR 火花提交选项 - 失败
- angular - Programmatically add @Input to Component through Attribute Directive
- amazon-web-services - How to fetch the list of possible non-partnered carrier services from Amazon MWS Fulfilment Inbound Shipment API
- d3.js - Center align two text elements without overlap in a rectangle
- python - 如何在 pandas 中为 columns 参数进行多于一列的 dcast
- linux - EPT PTE和主机PTE条目有什么关系?
- python - 我正在使用矩阵乘法但有问题
- python - 如何使用 Google Play Publisher API 将应用程序包 (.aab) 上传到 Play 商店
- sql-server - 如何在使用获取游标时删除最后一个“联合”