Question

我正在开发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 Assistant）调用此函数时，

rotateToTargetYaw（）可以工作，但它根本无法在现实世界中工作。

订阅QUATERNION确实没有问题。

我在DJI Assistant中检查“ API控制启用”设置为True。

有人有什么建议吗？

osdk在现实世界中不会旋转matrice600pro

0 个答案: