Flight Control

2022-08-09
No Rating

Overview

Using the drone flight control function of PSDK, you can set and obtain various basic parameters of the drone, control the drone to perform basic flight actions, and control the drone to perform complex flight actions through the Joystick function.

Basic Concepts

drone basic information

PSDK opens an interface for setting the basic parameters of the drone. By setting and obtaining the basic parameters of the drone, developers can achieve precise control of the drone:

  • Enable or disable RTK function and obstacle avoidance function (horizontal obstacle avoidance and top obstacle avoidance)
  • Set the home point and return altitude of the drone

An application with the function of Information Managementopen in new window can facilitate users to obtain information about the basic parameters of the drone:

  • Get the status of RTK function and obstacle avoidance function (horizontal obstacle avoidance and top obstacle avoidance)
  • Get the home point and return altitude of the drone

Basic drone flight action

The basic flight actions of the drone mainly include three categories: drone locking, drone take-off and landing, and drone return. Developers can use the interface provided by PSDK to control the drone to execute the specified operation according to the actual control requirements. flight action.

Table 1. Basic flight maneuvers

function typebasic skillsfunction description
drone lockunlockAfter the drone is unlocked, the drone's propellers will spin at idle speed, but will not fly off the ground
lockedAfter the drone is locked, the propeller of the drone changes from an idle rotating state to a stationary state
takeoff and landingautomatic takeoffWhen using this function, the drone will automatically take off 1.2 meters (this height cannot be adjusted)
automatic landingWhen using this function, the drone will automatically land
Cancel automatic landingWhen using this function, the drone will stop landing and hover in the air during the descent
Landing confirmationWhen the drone has landed to a certain distance from the ground, the user can use this function to confirm that the drone has landed on the ground
forced landingIgnore the status of the landing surface and force the drone to land (the landing speed is faster)
go homego homeWhen using this function, the drone will automatically return to home
cancel go homeWhen using this feature, the drone hovers in the air

illustrate

  • Different models of drones may have different heights when performing take-off, landing and return operations. For details, please refer to the instruction manual of the optional model.
  • After the drone is unlocked, if the take-off command is not received, the drone will automatically lock and the propeller will stop rotating. If the drone fails to unlock, please check the reason for the failure according to the error code.

Joystick function

Joystick is a comprehensive control function for drones. When using the Joystick function, the developer can set the coordinate system used by the drone, the horizontal control mode, and the vertical control mode by calling the interface in the Joystick according to the actual application needs. Mode, yaw angle control mode and hover mode**, can design the drone flight control logic that meets the needs of use.

Set coordinate system

  • Body coordinate system The body coordinate system takes the center of gravity of the drone as the origin, the forward direction of the drone head is the X axis, the right side of the forward direction of the nose is the Y axis, the Z axis, the X axis and the Y axis are perpendicular to each other at the center of gravity and point to Below the drone (following the "Right Hand Rule"). In the body coordinates, the flight action of the drone when it rotates around the X, Y and Z axes can be called roll (the drone only rotates around the X axis), pitch (the drone only rotates around the Y axis) and yaw (the drone only rotates around the Z axis).

  • Geodetic coordinate system The geodetic coordinate system is also called the world coordinate system or the local horizontal coordinate system. In this coordinate system, the direction of the drone pointing to the true north of the earth is the X-axis, the direction of the true east is the Y-axis, the X-axis and the Y-axis are perpendicular to each other, and the Z-axis is perpendicular to each other. The axis points vertically below the drone. On the premise of satisfying the "right-hand rule", the Z-axis will adjust the angle according to the actual situation of the drone's flight, so this coordinate system is also called "North-East-Earth (NED) coordinates. Tie".

Set horizontal control mode

  • Attitude angle control mode: In this mode, the command in the horizontal direction is the attitude angle of the drone. (In the body coordinate system, the angle is roll and pitch)
  • Speed control mode: In this mode, the command in the horizontal direction is the speed of the drone
  • Position control mode: In this mode, the command in the horizontal direction is the position of the drone

Note: When the modulo of the position command is not 0, the drone will fly forward at the specified speed, otherwise, the drone will hover at the specified position.

  • Angular velocity control: In this mode, the command in the horizontal direction is the rotational angular velocity of the drone
Set the drone hover mode

Only in the speed control mode in the **horizontal control mode, the developer can set the hovering mode of the drone:

  • Turn on stable mode: After turning on stable mode, the drone will hover at the specified position
  • Turn off the stable mode: After turning off the stable mode, the drone will fly according to the speed command. When the forward speed of the drone is 0, the drone may flutter with the wind

Set vertical control mode

  • Speed control mode: control the speed of the drone in the vertical direction
  • Position control mode: control the vertical position of the drone, which is an absolute position relative to the take-off point
  • Throttle control mode: Control the throttle of the drone

Set yaw angle control mode

  • Angle control mode: In this mode, the command to rotate in the yaw direction is the angle of the yaw
  • Angular rate control mode: In this mode, the command to rotate in the yaw direction is the angular rate of yaw

Drone altitude

The basic concept of altitude for DJI's drones during flight missions is shown in Table 2. The concept of drone altitude.

Table 2. Drone altitude description

relative takeoff altitudefusion heightGPS altitudeRTK altitudeheight above ground
basic meaningThe altitude change of the drone relative to the take-off pointUsing the barometer value as the initial value, use the fusion algorithm to output the altitude value with the help of the data from all the sensors on the droneReal-time ellipsoid height based on GPS satellite outputReal-time ellipsoid height output based on RTK technologyThe distance between the drone and the object currently below, obtained by the ranging sensor on the drone
Data SourcesIMU (may include vision sensors, GPS and RTK) and barometerIMU (may include vision sensors, GPS and RTK) and barometerGPS satelliteRTK satelliteTOF or Ultrasound and IMU
height 0 pointsThe point where the drone takes off from the groundsea level at standard pressureWGS84 Coordinate System RTK coordinate system (C2000, WGS84 or user-specified coordinate system)A solid surface (such as the ground) directly below the drone
ScenarioUsed to get the current altitude of the droneFor more precise and stable control of drones to perform flight tasksUsed to analyze the flight mission and flight status of the droneFor precision mapping and waypoint flight missionsUsed for safe landing of the drone (obstacle avoidance and deceleration)
Valid rangeThousands of metersThousands of metersThousands of metersThousands of metersWithin 10 meters
Get methodGet this information through DJI Pilot
The Mobile App developed based on MSDK can subscribe to this information
Mobile App developed based on MSDK can subscribe to this informationMobile App developed based on MSDK can subscribe to this informationMobile App developed based on MSDK can subscribe to this informationGet this information through DJI Pilot
The Mobile App developed based on MSDK can subscribe to this information
Data errorYes, the altitude accuracy will be affected by the flying distance of the droneYes, the altitude accuracy will be affected by the flying distance of the dronenonenonenone
ReliabilityUnreliableUnreliableBasically reliableReliableReliable
Error (noise) levelLowLowHigh LowLow
FactorsAffected by temperature, humidity, air pressureAffected by temperature, humidity, air pressureAffected by the strength of GPS satellites, the short-term error is large, and the long-term error is relatively smallInfluenced by the working state of RTK satellites and the accuracy of RTK base stations, the overall error is smallProperties (such as texture and texture) of hard surfaces directly below the drone

Using the flight controls

To use the aircraft flight control function, you need to initialize the flight control module first, then set the basic parameters of the aircraft, and finally use the functions in the Joystick.

1. Initialize the flight control module

Before using the related interfaces of the flight control function, you need to call the DjiTest_FlightControlInit interface to initialize the flight control module.

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

2. Gain control of the aircraft

Before calling the aircraft control interface, you need to call the DjiFlightController_ObtainJoystickCtrlAuthority interface to obtain the control authority of the aircraft. Before the aircraft control authority takes effect, the aircraft control command interface will not take effect.

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. Get and set aircraft parameters

Before controlling the automatic flight of the aircraft, it is necessary to check and set the relevant parameters of the aircraft. The flight control module provides a series of Set and Get interfaces, which can obtain and set the aircraft parameters and take effect in real time.

/*! 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. Control the aircraft to perform basic flight actions

Using the interface provided by PSDK, the developer can control the drone to perform the specified flight action according to the actual control requirements. Considering the safety of flight control, it is recommended to subscribe the relevant aircraft parameters through data subscription for closed-loop inspection and control to ensure the correct execution of flight actions.

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. Using the functions in Joystick

To use the Joystick function, you need to set the Joystick mode and corresponding control commands first, and then execute the Joystick commands to control the drone. The following code is to use the Joystick function to control the drone to move to the specified position in the horizontal and vertical directions.

  1. Set the mode of the 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);

Note: If you do not change the settings in the Joystick during the flight of the drone, you can only set the Joystick mode when the drone starts to fly.

  1. Set up Joystick control commands and perform Joystick functions

Note: The z-axis position in joystickCommand is the absolute height relative to the height of the take-off point.

    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. Release the aircraft control

After completing a series of controls on the aircraft, you need to call DjiFlightController_ReleaseJoystickCtrlAuthority to release the control of the aircraft so that other devices can take over the control of the aircraft.

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