飞控问题

已完成

评论

9 条评论

  • DJI Developer Support
    您好,感谢您的耐心等待,您方便给我们看下您的控制指令或代码片段吗?
    0
    评论操作 固定链接
  • xmyyzy123
    int controlFreqInHz = 50;  // Hz

        int cycleTimeInMs = 1000 / controlFreqInHz;

        int outOfControlBoundsTimeLimit = 10 * cycleTimeInMs;    // 10 cycles

        int withinControlBoundsTimeReqmt = 100 * cycleTimeInMs;  // 100 cycles

        int elapsedTimeInMs = 0;

        int withinBoundsCounter = 0;

        int outOfBounds = 0;

        int brakeCounter = 0;

        T_DjiFcSubscriptionPositionFused originGPSPosition;

        T_DjiFcSubscriptionQuaternion originalQuaternion;

        dji_f32_t originHeightBaseHomePoint;

        float originalyawInDeg = 0.0;

        T_DjiFcSubscriptionPositionFused currentGPSPosition;

        T_DjiFcSubscriptionQuaternion currentQuaternion;

        dji_f32_t currentHeight;

        T_DjiFlightControllerJoystickMode joystickMode = {

            DJI_FLIGHT_CONTROLLER_HORIZONTAL_POSITION_CONTROL_MODE,

            DJI_FLIGHT_CONTROLLER_VERTICAL_POSITION_CONTROL_MODE,

            DJI_FLIGHT_CONTROLLER_YAW_ANGLE_CONTROL_MODE,

            DJI_FLIGHT_CONTROLLER_HORIZONTAL_GROUND_COORDINATE,

            DJI_FLIGHT_CONTROLLER_STABLE_CONTROL_MODE_ENABLE,

        };
    returnCode = DjiFlightController_ObtainJoystickCtrlAuthority();

    //! get origin position and relative height(from home point)of aircraft.

        originGPSPosition = DjiTest_FlightControlGetValueOfPositionFused();

        originHeightBaseHomePoint = DjiTest_FlightControlGetValueOfRelativeHeight();

        originalQuaternion = DjiTest_FlightControlGetValueOfQuaternion();

        originalyawInDeg = DjiTest_FlightControlQuaternionToEulerAngle(originalQuaternion).z / s_degToRad;
    offsetDesired.x = droneOffset.x * cos(originalyawInDeg * s_degToRad) -

                          droneOffset.y * sin(originalyawInDeg * s_degToRad);

        offsetDesired.y = droneOffset.y * cos(originalyawInDeg * s_degToRad) +

                          droneOffset.x * sin(originalyawInDeg * s_degToRad);

        offsetDesired.z = droneOffset.z;

        if (keepYaw) {

            yawDesiredInDeg = originalyawInDeg;

        }   

       if(originHeightBaseHomePoint==-1)

        {

            return;

        }

        DjiFlightController_SetJoystickMode(joystickMode);

        while (elapsedTimeInMs < timeoutInMilSec) {

            currentGPSPosition = DjiTest_FlightControlGetValueOfPositionFused();

            currentQuaternion = DjiTest_FlightControlGetValueOfQuaternion();

            currentHeight = DjiTest_FlightControlGetValueOfRelativeHeight();

            if(originHeightBaseHomePoint==-1)

            {

                return;

            }

            float yawInRad = DjiTest_FlightControlQuaternionToEulerAngle(currentQuaternion).z;

            //! get the vector between aircraft and origin point.

            T_DjiTestFlightControlVector3f localOffset = DjiTest_FlightControlLocalOffsetFromGpsAndFusedHeightOffset(

                currentGPSPosition,

                originGPSPosition,

                currentHeight,

                originHeightBaseHomePoint);

            //! get the vector between aircraft and target point.

            T_DjiTestFlightControlVector3f offsetRemaining = DjiTest_FlightControlVector3FSub(offsetDesired, localOffset);

            T_DjiTestFlightControlVector3f positionCommand = offsetRemaining;

            DjiTest_FlightControlHorizCommandLimit(speedFactor, &positionCommand.x, &positionCommand.y);

            T_DjiFlightControllerJoystickCommand joystickCommand = {positionCommand.x, positionCommand.y,

                                                                    offsetDesired.z + originHeightBaseHomePoint,

                                                                    yawDesiredInDeg};

            DjiFlightController_ExecuteJoystickAction(joystickCommand);

            if (DjiTest_FlightControlVectorNorm(offsetRemaining) < posThresholdInM &&

                fabs(yawInRad / s_degToRad - yawDesiredInDeg) < yawThresholdInDeg) {

                //! 1. We are within bounds; start incrementing our in-bound counter

                withinBoundsCounter += cycleTimeInMs;

            } else {

                if (withinBoundsCounter != 0) {

                    //! 2. Start incrementing an out-of-bounds counter

                    outOfBounds += cycleTimeInMs;

                }

            }

            //! 3. Reset withinBoundsCounter if necessary

            if (outOfBounds > outOfControlBoundsTimeLimit) {

                withinBoundsCounter = 0;

                outOfBounds = 0;

            }

            //! 4. If within bounds, set flag and break

            if (withinBoundsCounter >= withinControlBoundsTimeReqmt) {

                break;

            }

            osalHandler->TaskSleepMs(cycleTimeInMs);

            elapsedTimeInMs += cycleTimeInMs;

        }

        while (brakeCounter < withinControlBoundsTimeReqmt) {

            osalHandler->TaskSleepMs(cycleTimeInMs);

            brakeCounter += cycleTimeInMs;

        }

     

       

     
    0
    评论操作 固定链接
  • DJI Developer Support
    您好,感谢您的耐心等待和补充的信息,飞机在执行航线时,控制器会优先考虑水平偏移量(东西方向)而不是目标点的方向(东北方向),这个会直接体现出您遇到的现象,飞行器先执行往东飞,再执行往北飞,您可以在计算`offsetDesired`时,确保水平偏移量`droneOffset.x`和`droneOffset.y`是基于期望的飞行方向而不是当前飞机的方向。
    0
    评论操作 固定链接
  • xmyyzy123

    你好,感谢回复,现在的逻辑是根据目标点的位置换算为NED坐标系,然后控制无人机保持当前偏航角不变,进行移动,所以现在就是基于期望的飞行方向吧

    0
    评论操作 固定链接
  • DJI Developer Support
    您好,感谢您的耐心等待,是的,您的理解是正确的。根据目标点的位置,将其换算为NED(北东地)坐标系后,就可以确定无人机应该飞向哪个方向。
    0
    评论操作 固定链接
  • xmyyzy123

    你好,现在的代码就是基于期望的飞行方向飞的,然后飞行器还是先执行往东飞,再执行往北飞,有没有办法可以直接往东北方向飞

    0
    评论操作 固定链接
  • DJI Developer Support
    您好,我们这边直接用DjiTest_FlightControlPositionControlSample函数是可以直接实现的,论坛上给您传图片可能您看不到,您可以用邮箱联系我们,我们再给您附图,看详细的设置。
    0
    评论操作 固定链接
  • xmyyzy123

    感谢,我看不到你的邮箱,可以发我邮箱嘛,xmyyzy123@qq.com

    0
    评论操作 固定链接
  • DJI Developer Support
    尊敬的开发者, 您好,感谢您联系DJI 大疆创新。 抱歉让您久等了,关于您在论坛上咨询的问题,我们现将相关资料发您邮箱,看您是否能成功收到,包括一个截图,以及一个代码文件。您可以替换后直接跑C++的flight control示例。 感谢您对大疆产品的支持!祝您一切顺利! Best Regards, DJI 大疆创新SDK技术支持
    0
    评论操作 固定链接

请先登录再写评论。