5.7 joystick飞行控制（flight_control）

5.7 joystick飞行控制

https://developer.dji.com/doc/payload-sdk-tutorial/cn/function-set/basic-function/flight-control.html

5.7.1 基本功能介绍

PSDK 3.x版本中的joystick飞行控制功能给机载端提供了飞行控制能力，独立于遥控器和APP的飞行控制，M300上为OSDK端口功能。机载计算机端通过串口向飞行器持续发送控制指令，水平方向上可以支持位置控制模式、速度控制模式、姿态角控制模式，垂直方向上可支持位置控制模式（相对起飞点高度），速度控制模式和油门控制模式。

其基本原理是，机载计算机获取控制权后，通过飞行控制API设定飞行模式，并在对应模式下传参，以一定的频率向飞行器发送控制指令。飞行器接收到持续的飞行控制指令，便按照设定参数进行移动。一旦停止发送指令，飞行器将立即停止移动。此设计可以避免OSDK在控制中通信突然中断，飞行器路线非预期而导致安全事故。

此功能最大程度的开放了DJI 无人机的自定义飞行，但是开发工作量会明显增加。区别与航线任务功能，joystick可以根据开发者自己的需求来脱离飞控的定位或导航功能，甚至无GPS环境下，也可以通过姿态角和油门控制模式，结合开发者自己的定位导航算法实现飞行控制。但速度控制模式下依旧会依赖飞机原有传感器的数据，位置控制模式可以不依赖飞控的定位功能。

5.7.2 主要代码文件

flight_control/

├── test_flight_control.c

└── test_flight_control.h

PSDK lib头文件：

dji_flight_controller.h

5.7.3 sample代码实现

flightCtrl部分sample代码封装较多，避免阅读冗长，以及实际使用上部分封装不是必要的，此部分将不再全部贴出源代码。同样依照sample的运行逻辑跟读代码，贴出必要的API和源代码。建议下载源代码同步查看。

joystick功能入口

通过widget列表控件进入有多个选项，实际与前面学习的功能基本一致。sample中比较全面的覆盖了joystick的使用模式和方式，可以通过不同的列表选项学习对应的功能。sample代码中也封装成了一个入口函数：DjiTest_FlightControlRunSample

初始化

飞行控制模块的初始化接口

/**
 * @brief Initialise flight controller module, and user should call this function
 * before using flight controller features.
 * @return Execution result.
 */
T_DjiReturnCode DjiFlightController_Init(void);

API简介：

使用飞行控制功能前，必须调用此API进行初始化。

飞行控制过程中，通常需要获取飞行器状态，辅助判断飞行过程，所以初始化中会初始化遥测订阅模块。具体遥测订阅的使用，请参考5.6 遥测订阅部分。

相比OSDK 4.1新增了一个回调可用于监控控制权状态的变化，

/**
 * @brief Subscribe to joystick control authority switch event with a callback function.
 * @note it will be triggered once the joystick control authority switch event occurs.
 * @return Execution result.
 */
T_DjiReturnCode DjiFlightController_RegJoystickCtrlAuthorityEventCallback(JoystickCtrlAuthorityEventCbFunc callback);

/**
 * @brief Prototype of callback function used to get joystick control authority switch event info.
 * @return Execution result.
 */
typedef T_DjiReturnCode (*JoystickCtrlAuthorityEventCbFunc)(
    T_DjiFlightControllerJoystickCtrlAuthorityEventInfo eventData);

API说明：

此API只是注册外部的回调，用于监控飞控控制权的变化。并非获取控制权。
具体枚举定义请参考代码文件：dji_flight_controller.h

应用功能

初始化完成后，不同的功能实现对应多个sample函数：

起飞降落

DjiTest_FlightControlTakeOffLandingSample

sample功能简单梳理：

功能：飞行器起飞到1.2m，悬停4s，降落到0.7m，APP上确认降落，降落停桨。
获取控制权
起飞函数：

DjiTest_FlightControlMonitoredTakeoff
降落函数

DjiTest_FlightControlMonitoredLanding
释放控制权

说明：

其中获取/释放控制权为PSDK lib API，起飞和降落是基于lib API封装的函数。
与旧版OSDK（4.x，3.x）一样，SDK端发送起飞、降落的指令，飞控执行。因为飞行器的起飞和降落是一个动态过程，通过实时订阅飞行器的状态变化，可以判断起飞和降落的状态。
起飞、降落指令不需要获取控制权。即上述步骤中无需获取控制权也可以完成起飞和降落动作。
起飞动作是，飞控收到指令自动起飞到1.2m高，其飞行速度高度不可更改或设置。
降落动作是从当前高度降落到0.7m（降落保护悬停），SDK调用确认降落API或APP上确认降落后，落地停桨，降落速度和状态不可设置或更改。

位置模式控制移动

DjiTest_FlightControlPositionControlSample

sample功能简单梳理：

函数功能：起飞，向NEU（0,6,6）移动，然后向NEU(6,0,-3)移动，然后向NEU（-6，-6，0）移动，然后降落。
起飞降落实现方式与前面一致。
起飞后通过函数DjiTest_FlightControlMoveByPositionOffset实现移动，此函数是基于PSDK lib的封装，是位置模式下的joystick功能，需要先获取控制权才可调用此函数。

重点说明一下DjiTest_FlightControlMoveByPositionOffset的实现（实现原理与OSDK 4.1也基本保持一致）：

PSDK lib API（PSDK与飞控交互的指令）

/**
 * @brief Set expected joystick mode before requesting joystick.
 * @param joystickMode: include horizontal/vertical/yaw control mode, stable control mode.
 */
void DjiFlightController_SetJoystickMode(T_DjiFlightControllerJoystickMode joystickMode);

API简介：

设置控制模式，水平方向模式，垂直方向模式，YAW机头控制模式，参考坐标系，稳定模式。

说明：

其中坐标系是对应的发送参数，并非设置飞机的坐标系。比如大地坐标系下，X/Y/Z参数分别对应NEU的参数值。
YAW角控制模式只对应大地坐标系，也就是设置为机体坐标系，该参数也是对应大地坐标系。
使用稳定模式下，姿态角可能会受限，如果发现姿态角控制不到设定值，可以检查此参数设置。

/**
 * @brief Request execute joystick action.
 * @param joystickCommand: include x/y/z/yaw.
 * @return Execution result.
 */
T_DjiReturnCode DjiFlightController_ExecuteJoystickAction(T_DjiFlightControllerJoystickCommand joystickCommand);

API简介：

向飞控发送控制指令，参数对应为x, y, z轴上的参数值，以及YAW角参数。

说明：

x，y，z代表的方向不是固定的，与设定的坐标系有关，大地坐标系分别对应NEU（北东天），机体坐标系下分别对应FRU（前右上）
x，y，z代表的数值与设置的模式有关，比如速度模式下为对应方向上的速度值，姿态模式下为姿态角，位置模式比较特殊，水平方向上的位置可以看做是移动方向（x,y）上的移动标志，垂直方向上的位置是相对起飞点的高度。具体参考下面的说明

结合基本功能介绍中描述的joystick原理，基于上述API的位置功能实现就可以大致概括为：

因为需要持续性的发送，一旦停止发送指令，无人机将会停止移动，所以给飞控发送指令的参数并不是飞机实际移动的位置，也就是只发送一次指令，并不能移动到该指令的目标位置上。无人机是动态移动中，就需要实时的订阅GPS位置坐标，然后计算无人机的偏移并与目标位置进行对比，当与目标位置距离低于某个阈值时，我们就认为飞行器已经到达我们指定的目标位置。可以粗略的将发送给飞控的指令中位置参数对应于方向上的移动标志，发送参数设置为0，对应方向上不再移动。同理，YAW角控制也是实时读取飞机YAW角，判断与目标角相对位置。

位置控制sample API：DjiTest_FlightControlMoveByPositionOffset就是基于上述API和原理的封装实现，结合代码，在while循环中会实时读取订阅的GPS位置，然后基于当前位置计算偏移，YAW通过实时读取四元数换算成Euler角。判断各个方向上的移动偏移，以及与目标位置相对位置低于设定的阈值，当各个方向且YAW符合阈值设定时，停止发送指令，即完成位置控制移动。

注：

sample中while循环设定了移动时间，当使用该函数API：DjiTest_FlightControlMoveByPositionOffset移动较长距离时，可能出现超时的报错，是因为此处设定的时间参数不够移动目标距离，可以根据实际情况调整while条件的设定。
指令发送频率通过延时参数控制：cycleTimeInMs = 1000 / controlFreqInHz

返航强制降落

DjiTest_FlightControlGoHomeForceLandingSample

sample功能简单梳理：

起飞，位置控制移动NEU（0,0,30），然后位置控制移动NEU（10,0,0），设置当前GPS位置为新的home点，设置home点高度，再位置移动NEU(20,0,0)，执行返航并降落，最后确认降落。

起飞、位置控制与前面部分一致，设置新的Home点有两个API

/**
 * @brief Set customized GPS(not RTK) home location.
 * @note Set customized home location failed reason may as follows:
 * 1. The distance between new home location and last home location is larger than MAX_FLY_RADIUS(20km).
 * 2. Record initial home location failed after start aircraft.
 * @param homeLocation: homeLocation include latitude and longitude
 * @return Execution result.
 */
T_DjiReturnCode DjiFlightController_SetHomeLocationUsingGPSCoordinates(T_DjiFlightControllerHomeLocation homeLocation);

/**
 * @brief Set home location using current aircraft GPS(not RTK) location.
 * @note Set home location failed reasons may as follows:
 * 1. Aircraft's gps level can't reach the condition of recording home location.
 * 2. Record initial home location failed after start aircraft.
 * @return Execution result.
 */
T_DjiReturnCode DjiFlightController_SetHomeLocationUsingCurrentAircraftLocation(void);

API简介

两个API分别为设置指定GPS坐标为home点，和指定当前GPS坐标为新的home点。

说明：

记录的home点是GPS位置，需要GPS信号良好，且与上一个home点距离小于20KM。
home点设置不能设置返航高度。

开始返航和降落，封装的函数：DjiTest_FlightControlGoHomeAndConfirmLanding，开启返航之后，通过订阅到的飞机状态进行检查，自动降落到距离地面0.7m左右时，调用确认降落API。

速度控制模式

DjiTest_FlightControlVelocityControlSample

sample功能简单梳理：

起飞，速度控制NEU(0,0,5)运行2s，刹车悬停2s，速度控制NEU（-1.5, 2, 0）2s，YAW旋转角速度20deg/s，刹车悬停2s，速度控制NEU（3, 0, 0）2.5s，刹车悬停2s，速度控制NEU（-1.6, -2, 0）2.2s，刹车悬停2s，降落。

主要介绍两个部分，一个是紧急刹车悬停，一个是速度控制的实现。

紧急悬停API：

/**
 * @brief Request emergency brake action.
 * @return Execution result.
 */
T_DjiReturnCode DjiFlightController_ExecuteEmergencyBrakeAction(void);

API简介：

紧急刹车，将飞行器速度变为0，进入悬停状态。

说明：

M300上调用此接口后将速度降为0，需要停止发送移动指令。发送移动指令，飞行器会继续移动。
M30上调用此接口后再移动，需要先cancel，再继续发送指令。

速度控制功能实现：

DjiTest_FlightControlVelocityAndYawRateCtrl

   T_DjiFlightControllerJoystickMode joystickMode = {
        DJI_FLIGHT_CONTROLLER_HORIZONTAL_VELOCITY_CONTROL_MODE,
        DJI_FLIGHT_CONTROLLER_VERTICAL_VELOCITY_CONTROL_MODE,
        DJI_FLIGHT_CONTROLLER_YAW_ANGLE_RATE_CONTROL_MODE,
        DJI_FLIGHT_CONTROLLER_HORIZONTAL_GROUND_COORDINATE,
        DJI_FLIGHT_CONTROLLER_STABLE_CONTROL_MODE_ENABLE,
    };

    DjiFlightController_SetJoystickMode(joystickMode);
    T_DjiFlightControllerJoystickCommand joystickCommand = {offsetDesired.x, offsetDesired.y, offsetDesired.z,
                                                            yawRate};

    while (elapsedTimeInMs <= timeMs) {
        DjiFlightController_ExecuteJoystickAction(joystickCommand);
        s_osalHandler->TaskSleepMs(2);
        s_osalHandler->GetTimeMs(&currentTime);
        elapsedTimeInMs = currentTime - originTime;
    }

API与位置控制完全一致，通过joystickMode指定为速度控制，大地坐标系，角速率控制。
速度控制实现中没有判断位置，仅通过限定时间timeMs来控制，50HZ频率发送指令。

阻飞

DjiTest_FlightControlArrestFlyingSample

有bug，待补充

设置或读取飞行设置相关参数

DjiTest_FlightControlSetGetParamSample

此sample是对飞行相关的RTK、视觉避障等参数的设置与读取等。PSDK 3.x版本中已经将PSDK端设置状态与遥控Pilot APP上的状态进行同步，即当PSDK调用API设置关闭避障后，Pilot上的避障控件会对应关闭。M300和M30均支持六向避障，以及RTK功能，需要注意RTK需要在起飞前打开，并收敛成功，无人机才会使用RTK定位。

具体API不再一一列举，参考头文件：dji_flight_controller.h中即可。

5.7.4 开发注意事项

起飞、降落、返航是飞控固有的动作，非joystick动作。所以起飞、降落、返航指令无需获取控制权。
PSDK获取控制权仅支持P模式下获取。无GPS环境，需要将无人机设定在P模式，然后使用OSDK的姿态角、油门控制。
因为返航动作是飞控固有的动作，使用SDK发送返航指令，也会遵循飞控的返航逻辑。比如，返航距离超过50m，上升到返航高度然后返航，详见用户手册。
joystick也可以用于起飞降落，通过垂直方向上的参数z，自定义起飞降落过程。
使用joystick功能自定义起飞降落时，起飞前需要先调用启动电机函数，降落后需要调用关闭电机函数。注意需要先获取控制权，再使用joystick（此操作只支持M300、M350机型，其他机型不支持）
无GPS环境下，仅推荐使用油门、姿态角控制无人机，否则因为无GPS会影响位置和速度控制。
位置控制模式下，不可以精准控制速度，若一定需要，可以基于位置控制原理引入PID算法。基于位置控制和速度控制原理，可以考虑通过向飞控发送速度控制指令，然后实时读取GPS位置并计算偏移。即将sample中的位置和速度控制组合，从而实现位置控制中的速度控制。
向飞控发送控制指令需要以一定的频率发送，推荐使用最大频率50HZ。当发送频率较小时，会发现飞行器存在加速、减速不断交替，出现飞行不连续一顿一顿的现象。
速度控制模式中，API的最大速度为30m/s，实际受限于飞行器支持的最大速度。即，若飞行器最大速度支持18m/s，即使API设定速度为30m/s，其实际飞行速度也是18m/s。
油门控制模式下，油门值为百分比，油门值对应的不是飞机固有的状态，比如60%的油门值下可能向上飞行，也可能是悬停状态。与飞机负重和使用环境（风力）等有关。使用姿态角，油门控制时，飞控内部没有闭环，建议考虑在机载端实现控制闭环。即结合外部传感器，通过反馈实时调整飞机控制参数值。
获取控制权，使用joystick控制飞行后，遥控器将无法直接接入控制飞行。需要通过切换飞行模式（T-S-P）来抢夺控制权，之后若PSDK需要继续控制无人机，需要再次调用API来获取控制权。

此组别内的文章