飞行控制

概述

使用 PSDK 的飞行器飞行控制功能，能够设置并获取飞行器各项基础参数，控制飞行器执行基础飞行动作，通过 Joystick 功能控制飞行器执行复杂的飞行动作。

基础概念

飞行器基础信息

PSDK 开放了设置飞行器基础参数的接口，开发者通过设置并获取飞行器的基础参数，能够实现对飞行器的精准控制：

开启或关闭 RTK 功能和避障功能（水平避障、上视觉避障、下视觉避障）
设置飞行器的返航点和返航高度
设置失控行为（Matrice 30/30T, Mavic 3 行业系列）
支持获取飞行器的国家区域码open in new window，便于产品适配不同国家的法规要求

具有信息管理open in new window 功能的应用程序，能够方便用户获取飞行器基础参数的信息：

获取 RTK 功能和避障功能的状态（水平避障、上视觉避障、下视觉避障）
获取飞行器返航点和返航高度

飞行器基础飞行动作

飞行器的基础飞行动作主要包含飞行器锁定、飞行器起飞降落和飞行器返航三类，开发者使用 PSDK 提供的接口，即可根据实际的控制需求，控制飞行器执行指定的飞行动作。

表. 基础飞行动作

功能类型	基本功能	功能说明
飞行器锁定	解锁	飞行器解锁后，飞行器的螺旋桨会怠速旋转，但不会飞离地面
飞行器锁定	上锁	飞行器上锁后，飞行器的螺旋桨由怠速旋转状态变为静止状态
起飞和降落	自动起飞	使用该功能时，飞行器会自动起飞 1.2 米（该高度不可调整）
	自动降落	使用该功能时，飞行器会自动降落
	取消自动降落	使用该功能时，飞行器在下降多过程中会停止降落，并悬停在空中
	降落确认	当飞行器已降落到离地面一定距离时，用户使用该功能可确认飞行器降落到地面
	强制降落	无视降停面的状态，强制飞行器降落（降落速度较快）
飞行器返航	返航	使用该功能时，飞行器会自动返航
飞行器返航	取消返航	使用该功能时，飞行器会悬停在空中

说明
不同型号的飞行器在执行起飞、降落和返航动作时的高度可能会有差异，详情请参见选购机型的说明书。
飞行器解锁后，若未接收到起飞指令，飞行器将自动上锁，螺旋桨会停止旋转，若飞行器解锁失败，请根据错误码查询解锁失败的原因。

Joystick 功能

Joystick 是一个飞行器综合控制功能，使用 Joystick 功能时，开发者根据实际的应用需要，通过调用 Joystick 中的接口，同时设置飞行器使用的坐标系、水平控制的模式、垂直控制的模式、yaw 角度控制的模式和悬停模式，才能设计出满足使用需求的飞行器飞行控制逻辑。

设置坐标系

机体坐标系
机体坐标系以飞行器的重心为原点，飞行器机头前进的方向为 X 轴，机头前进方向的右侧为 Y 轴，Z 轴是与 X 轴和 Y 轴在重心处相互垂直相交，并指向飞行器下方的轴（遵循 “右手法则”）。在机体坐标下，飞行器围绕 X 轴、Y 轴和 Z 轴旋转时的飞行动作，可称为横滚（飞行器仅绕 X 轴旋转）、俯仰（飞行器仅绕 Y 轴旋转）和偏航（飞行器仅绕 Z 轴旋转）。
大地坐标系
大地坐标系也称世界坐标系或当地水平坐标系，在该坐标系中，飞行器指向地球正北的方向为 X 轴，正东的方向为 Y 轴，X 轴与 Y 轴相互垂直，Z 轴竖直指向飞行器下方，在满足 “右手法则” 的前提下，Z 轴将根据飞行器飞行的实际情况调节角度，因此该坐标系也称为 “北 - 东 - 地（N-E-D）坐标系”。

设置水平控制模式

姿态角控制模式：在该模式下，水平方向的指令为飞行器的姿态角。(在机体坐标系下，该角度为 roll 和 pitch)
速度控制模式：在该模式下，水平方向的指令为飞行器的速度
位置控制模式：在该模式下，水平方向的指令为飞行器的位置

说明： 当位置指令的模不为 0 时，飞行器会以指定的速度向前飞行，否则，飞行器将悬停在指定的位置。

角速度控制：在该模式下，水平方向的指令为飞行器的旋转角速度

设置飞行器悬停模式

仅在水平控制模式中的速度控制模式下，开发者可以设置飞行器的悬停模式：

开启稳定模式：开启稳定模式后，飞行器将在指定的位置上悬停
关闭稳定模式：关闭稳定模式后，飞行器将按照速度命令飞行，当飞行器的前进速度为 0 时候，飞行器可能会随风飘动

设置垂直控制模式

速度控制模式：控制飞行器垂直方向的速度
位置控制模式：控制飞行器垂直方向的位置，该位置为相对于起飞点的绝对位置
油门控制模式：控制飞行器的油门

设置 yaw 角度控制模式

角度控制模式：在该模式下，yaw 方向旋转的指令为 yaw 的角度
角速率控制模式：在该模式下，yaw 方向旋转的指令为 yaw 的角速率

飞行器高度

DJI 的飞行器在执行飞行任务过程中的基础高度概念如表. 飞行器高度概念所示。

表. 飞行器高度说明

	相对起飞高度	融合高度	GPS 高度	RTK 高度	对地高度
基本含义	飞行器相对于起飞点的高度变化量	以气压计值为初始值，使用融合算法并借助飞行器上所有传感器的数据输出的高度值	基于 GPS 卫星输出的实时椭球高	基于 RTK 技术输出的实时椭球高	借助飞行器上的测距传感器获得的飞行器与当前下方物体的距离
数据来源	IMU(可能包括视觉传感器、GPS 和 RTK) 和气压计	IMU(可能包括视觉传感器、GPS 和 RTK) 和气压计	GPS 卫星	RTK 卫星	TOF 或超声波和 IMU
高度 0 点	飞行器离地起飞的点	标准气压的海平面	WGS84 坐标系	RTK 坐标系（C2000，WGS84 或用户指定的坐标系）	飞行器正下方坚硬物体的表面（如地面）
应用场景	用于获取飞行器当前已飞行的高度	用于更加精准稳定地控制飞行器执行飞行任务	用于分析飞行器的飞行任务和飞行状态	用于精准测绘和航点飞行等飞行任务	用于飞行器安全降落（避障和降速）
有效量程	数千米	数千米	数千米	数千米	10 米内
获取方法	通过 DJI Pilot 获取该信息基于 MSDK 开发的移动端 App 可订阅该信息	基于 MSDK 开发的移动端 App 可订阅该信息	基于 MSDK 开发的移动端 App 可订阅该信息	基于 MSDK 开发的移动端 App 可订阅该信息	通过 DJI Pilot 获取该信息基于 MSDK 开发的移动端 App 可订阅该信息
数据误差	有，受飞行器飞行距离的影响，高度精度会受影响	有，受飞行器飞行距离的影响，高度精度会受影响	无	无	无
可靠度	不可靠	不可靠	基本可靠	可靠	可靠
误差（噪声）水平	低	低	高	低	低
影响因素	受温度，湿度，气压影响	受温度，湿度，气压影响	受 GPS 卫星强度影响，短期误差大，长期误差相对较小	受 RTK 卫星工作状态和 RTK 基站精度的影响，总体误差小	飞行器正下方坚硬物体表面的性质（如质地和纹理）

控制权说明

在调用飞行器控制接口之前，需要先调用DjiFlightController_ObtainJoystickCtrlAuthority 接口来获取飞行器的控制权，在飞行器控制权生效前，飞行器控制命令接口将无法生效。

控制权：是指当前具备控制飞行器的权限，控制对象有 MSDK、PSDK、遥控器、大疆机场等，控制权允许抢夺和主动释放；

失控行为：当飞行器在控制过程中由于各种突发因素导致的不受控状态（如遥控器断联、PSDK 程序崩溃、飞行器低电量等场景）。进入失控模式后，飞行器的控制权会发生变化（部分机型）并执行对应的预设失控行为（如返航、降落、悬停）。

M300 RTK/M350 RTK 控制权逻辑

控制权主动切换的场景：PSDK 主动释放控制权、遥控器切换档位、遥控器触发返航、遥控器暂停

控制权被动切换的场景：低电量返航 / 降落、PSDK 异常断联

表. M300 RTK/M350 RTK 控制权逻辑说明

RC 状态	控制权状态	异常场景	预期执行策略
连接	PSDK	遥控器断开连接	不执行任何策略
连接	PSDK	PSDK 断开连接	根据 DA2 的参数策略（遥控失控不切 SDK），打开就执行 DA2 上设置的失控行为，不切控制权
连接	遥控器	遥控器断开连接	执行遥控器上 APP 设置的失控行为
连接	遥控器	PSDK 断开连接	不执行任何策略
未连接	PSDK	PSDK 断开连接（程序崩溃或硬件断连）	根据 DA2 的参数策略（遥控失控不切 SDK），打开就执行 DA2 上设置的失控行为，不切控制权
未连接	PSDK	遥控器重新连接	不执行任何策略
未连接	PSDK	触发避障	执行预设的避障行为，不切控制权
未连接	PSDK	低电量返航 / 降落	执行预设的低电量行为，不切控制权
未连接	PSDK	靠近禁飞区附近	无法控制进入禁飞区，不响应 Joystick 动作，需要执行返航
未连接	PSDK	RTK/GPS 信号丢失	视觉悬停，若视觉失效，飞行器可能会飘

M30/M30T/M3E/M3T 控制权逻辑

控制权主动切换的场景：PSDK 主动释放控制权、遥控器切换档位、遥控器触发返航、遥控器暂停

控制权被动切换的场景：低电量返航 / 降落、PSDK 异常断联

表. M30/M30T/M3E/M3T 控制权逻辑说明

RC 状态	控制权状态	异常场景	预期执行策略
连接	PSDK	遥控器断开连接	不执行任何策略
连接	PSDK	PSDK 断开连接	切换走控制权，执行 PSDK 端设置的失控行为（和 APP 上的失控行为同步）
连接	遥控器	遥控器断开连接	执行遥控器上 APP 设置的失控行为
连接	遥控器	PSDK 断开连接	不执行任何策略
未连接	PSDK	PSDK 断开连接	切换走控制权，执行 PSDK 端设置的失控行为（和 APP 上的失控行为同步）
未连接	PSDK	遥控器重新连接	不执行任何策略
未连接	PSDK	触发避障	执行预设的避障行为，不切控制权
未连接	PSDK	低电量返航 / 降落	执行预设的低电量行为，切换控制权
未连接	PSDK	靠近禁飞区附近	无法控制进入禁飞区，切换控制权，不响应 josstick 动作，需要执行返航
未连接	PSDK	RTK/GPS 信号丢失	视觉悬停，若视觉失效，飞行器可能会飘

使用飞行控制功能

使用飞行器飞行控制功能需要先初始化飞行控制模块，再设置飞行器的基础参数，最后再使用 Joystick 中的功能。

说明： 当完全脱离遥控器使用 PSDK 控制飞机时，需要通过订阅飞机端的关键数据，严格检查飞机的当前状态是否允许进行飞行控制。务必检查当前电量、返航点、失控行为、RTK 开关、避障开关等关键飞行参数。此外，还需要严格遵守当地的飞行器法规要求，如美国需要强制上报 RID 信息，否则会导致飞机阻飞，需要第三方开发者保证 RID 信息的准确性。在飞行任务过程中，需要实时监控飞行器实时的状态和告警信息，用来辅助远程驾驶员或者程序的决策，参考 “信息管理” 和 “HMS 功能” 章节。

1. 初始化飞行控制模块

在使用飞行控制功能的相关接口之前，需要先调用DjiTest_FlightControlInit 接口初始化飞行控制模块。

returnCode = DjiTest_FlightControlInit(ridInfo);
if (returnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS) {
    USER_LOG_ERROR("Init flight Control sample failed,error code:0x%08llX", returnCode);
    return returnCode;
}

2. 获取飞行器控制权

returnCode = DjiFlightController_ObtainJoystickCtrlAuthority();
if (returnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS) {
    USER_LOG_ERROR("Obtain joystick authority failed, error code: 0x%08X", returnCode);
    goto out;
}
s_osalHandler->TaskSleepMs(1000);

3. 获取和设置飞行器参数

在控制飞行器自动飞行之前，需要先检查和设置飞行器的相关参数，飞行控制模块提供了一系列 Set 和 Get 的接口，可以获取和设置飞行器参数，实时生效。

/*! Turn on horizontal vision avoid enable */
    USER_LOG_INFO("--> Step 1: Turn on horizontal visual obstacle avoidance");
    DjiTest_WidgetLogAppend("--> Step 1: Turn on horizontal visual obstacle avoidance");
    returnCode = DjiFlightController_SetHorizontalVisualObstacleAvoidanceEnableStatus(
        DJI_FLIGHT_CONTROLLER_ENABLE_OBSTACLE_AVOIDANCE);
    if (returnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS) {
        USER_LOG_ERROR("Turn on horizontal visual obstacle avoidance failed, error code: 0x%08X", returnCode);
        goto out;
    };
    s_osalHandler->TaskSleepMs(1000);

    USER_LOG_INFO("--> Step 2: Get horizontal horizontal visual obstacle status\r\n");
    DjiTest_WidgetLogAppend("--> Step 2: Get horizontal horizontal visual obstacle status\r\n");
    returnCode = DjiFlightController_GetHorizontalVisualObstacleAvoidanceEnableStatus(
        &horizontalVisualObstacleAvoidanceStatus);
    if (returnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS) {
        USER_LOG_ERROR("Get horizontal visual obstacle avoidance failed, error code: 0x%08X", returnCode);
        goto out;
    }
    USER_LOG_INFO("Current horizontal visual obstacle avoidance status is %d\r\n",
                  horizontalVisualObstacleAvoidanceStatus);
    s_osalHandler->TaskSleepMs(1000);

    /*! Turn on horizontal radar avoid enable */
    USER_LOG_INFO("--> Step 3: Turn on horizontal radar obstacle avoidance");
    DjiTest_WidgetLogAppend("--> Step 3: Turn on horizontal radar obstacle avoidance");
    returnCode = DjiFlightController_SetHorizontalRadarObstacleAvoidanceEnableStatus(
        DJI_FLIGHT_CONTROLLER_ENABLE_OBSTACLE_AVOIDANCE);
    if (returnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS) {
        USER_LOG_ERROR("Turn on horizontal radar obstacle avoidance failed, error code: 0x%08X", returnCode);
        goto out;
    };
    s_osalHandler->TaskSleepMs(1000);

4. 控制飞行器执行基础飞行动作

开发者使用 PSDK 提供的接口，即可根据实际的控制需求，控制飞行器执行指定的飞行动作。考虑到飞行控制安全，建议通过数据订阅订阅相关的飞行器参数进行闭环检查和控制，确保飞行动作执行正确。

T_DjiReturnCode djiStat;

//! Start takeoff
djiStat = DjiFlightController_StartTakeoff();
if (djiStat != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS) {
    USER_LOG_ERROR("Request to take off failed, error code: 0x%08X", djiStat);
    return false;
}

//! Motors start check
if (!DjiTest_FlightControlMotorStartedCheck()) {
    USER_LOG_ERROR("Takeoff failed. Motors are not spinning.");
    return false;
} else {
    USER_LOG_INFO("Motors spinning...");
}
//! In air check
if (!DjiTest_FlightControlTakeOffInAirCheck()) {
    USER_LOG_ERROR("Takeoff failed. Aircraft is still on the ground, but the "
                   "motors are spinning");
    return false;
} else {
    USER_LOG_INFO("Ascending...");
}
//! Finished takeoff check
if (!takeoffFinishedCheck()) {
    USER_LOG_ERROR("Takeoff finished, but the aircraft is in an unexpected mode. "
                   "Please connect DJI GO.");
    return false;
}

5. 使用 Joystick 中的功能

使用 Joystick 功能需要先设置 Joystick 的模式和对应的控制指令，再执行 Joystick 指令，实现对飞行器的控制。如下代码是使用 Joystick 功能控制飞行器在水平和垂直方向上运动到指定的位置。

设置 joystick 的模式

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,
    };

DjiFlightController_SetJoystickMode(joystickMode);

说明： 若在飞行器飞行的过程中不改变 Joystick 中的设置项，则仅在飞行器开始执行飞行任务时设置 Joystick 模式即可。

设置 Joystick 控制指令并执行 Joystick 功能

说明： joystickCommand中 z 轴方向的位置是相对于起飞点高度的绝对高度。

    while (elapsedTimeInMs < timeoutInMilSec) {
        T_DjiFcSubscriptionPositionFused currentGPSPosition = DjiTest_FlightControlGetValueOfPositionFused();
        T_DjiFcSubscriptionQuaternion currentQuaternion = DjiTest_FlightControlGetValueOfQuaternion();
        dji_f32_t currentHeight = DjiTest_FlightControlGetValueOfRelativeHeight();
        if (originHeightBaseHomePoint == -1) {
            USER_LOG_ERROR("Relative height is invalid!");
            return false;
        }

        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;
        }
        s_osalHandler->TaskSleepMs(cycleTimeInMs);
        elapsedTimeInMs += cycleTimeInMs;
    }

    while (brakeCounter < withinControlBoundsTimeReqmt) {
        s_osalHandler->TaskSleepMs(cycleTimeInMs);
        brakeCounter += cycleTimeInMs;
    }

    if (elapsedTimeInMs >= timeoutInMilSec) {
        USER_LOG_ERROR("Task timeout!");
        return false;
    }

6. 释放飞行器控制权

在完成对飞行器的一系列控制之后，需要调用DjiFlightController_ReleaseJoystickCtrlAuthority 释放飞行器控制权以便其他设备接管飞行器控制。

returnCode = DjiFlightController_ReleaseJoystickCtrlAuthority();
if (returnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS) {
    USER_LOG_ERROR("Release joystick authority failed, error code: 0x%08X", returnCode);
    goto out;
}

飞行控制

# 概述

# 基础概念

# 飞行器基础信息

# 飞行器基础飞行动作

# Joystick 功能

# 设置坐标系

# 设置水平控制模式

# 设置飞行器悬停模式

# 设置垂直控制模式

# 设置 yaw 角度控制模式

# 飞行器高度

# 控制权说明

# M300 RTK/M350 RTK 控制权逻辑

# M30/M30T/M3E/M3T 控制权逻辑

# 使用飞行控制功能

# 1. 初始化飞行控制模块

# 2. 获取飞行器控制权

# 3. 获取和设置飞行器参数

# 4. 控制飞行器执行基础飞行动作

# 5. 使用 Joystick 中的功能

# 6. 释放飞行器控制权

概述