大疆DJI_OSDK控制无人机飞行

大疆官方提供的软件包的主要有3个部分

1. 核心的API部分dji-sdk-lib，用于串口通信，建立各种任务的线程，读取信息的线程。
2. 用于封装核心API为ROS接口的dji_sdk
3. 简单的demo，给出了dji_sdk的使用方法。

1.dji_sdk_node.cpp

该节点为dji_sdk包的核心，成员变量包括无人机的状态信息，服务信息，控制信息，话题发布和订阅信息。

1）状态信息

参数列表，包括串口名称，波特率，开发者ID及密码，直接在sdk.launch中进行修改即可。

  nh_private.param("serial_name",   serial_device, std::string("/dev/ttyUSB0"));
  nh_private.param("baud_rate",     baud_rate, 921600);
  nh_private.param("app_id",        app_id,    123456);
  nh_private.param("app_version",   app_version, 1);
  nh_private.param("enc_key",       enc_key, std::string("abcd1234"));
  nh_private.param("drone_version", drone_version, std::string("M100")); // choose M100 as default
  nh_private.param("gravity_const", gravity_const, 9.801);
  nh_private.param("align_time",    align_time_with_FC, true);
  nh_private.param("use_broadcast", user_select_BC, false);

三个经常要使用的位置参数，经度，纬度，海拔（海拔和姿态极易混淆，注意区分）

local_pos_ref_latitude = local_pos_ref_longitude = local_pos_ref_altitude = 0;

2）服务信息

drone_activation_server   = nh.advertiseService("dji_sdk/activation",                     &DJISDKNode::droneActivationCallback,        this);
  drone_arm_server          = nh.advertiseService("dji_sdk/drone_arm_control",              &DJISDKNode::droneArmCallback,               this);
  drone_task_server         = nh.advertiseService("dji_sdk/drone_task_control",             &DJISDKNode::droneTaskCallback,              this);
  sdk_ctrlAuthority_server  = nh.advertiseService("dji_sdk/sdk_control_authority",          &DJISDKNode::sdkCtrlAuthorityCallback,       this);
  camera_action_server      = nh.advertiseService("dji_sdk/camera_action",                  &DJISDKNode::cameraActionCallback,           this);

对应的回调函数可在dji_sdk_node_services.cpp中找到。

3）控制信息

  flight_control_sub = nh.subscribe(
    "dji_sdk/flight_control_setpoint_generic", 10, 
    &DJISDKNode::flightControlSetpointCallback,   this);

对应的回调函数可在dji_sdk_node_control中找到。

void DJISDKNode::flightControlSetpointCallback(
  const sensor_msgs::Joy::ConstPtr& pMsg)
{ 
  float xSP    = pMsg->axes[0];
  float ySP    = pMsg->axes[1];
  float zSP    = pMsg->axes[2];
  float yawSP  = pMsg->axes[3];
  uint8_t flag = (uint8_t)(pMsg->axes[4]);
 
  flightControl(flag, xSP, ySP, zSP, yawSP);
}

如该函数通过订阅JOY消息来将无人机移动到指定位置，其中flag用来表示控制模式，即FAP。

4）话题发布和订阅信息

  height_publisher =
    nh.advertise("dji_sdk/height_above_takeoff", 10);
 
  velocity_publisher =
    nh.advertise("dji_sdk/velocity", 10);

这里发布的许多话题都是我们比较感兴趣的，如attitude（四元组），imu，gps_position（经纬度，海拔），height_above_takeoff，velocity等等。

2.dji_sdk_demo

以demo_flight_control为例

首先是初始化

  // Subscribe to messages from dji_sdk_node
  ros::Subscriber attitudeSub = nh.subscribe("dji_sdk/attitude", 10, &attitude_callback);    //话题，队列长度，回调函数
  ros::Subscriber gpsSub      = nh.subscribe("dji_sdk/gps_position", 10, &gps_callback);
  ros::Subscriber flightStatusSub = nh.subscribe("dji_sdk/flight_status", 10, &flight_status_callback);
  ros::Subscriber displayModeSub = nh.subscribe("dji_sdk/display_mode", 10, &display_mode_callback);

从dji_sdk_node处订阅消息并赋给已经定义好的全局变量。

  // Publish the control signal
  ctrlPosYawPub = nh.advertise("/dji_sdk/flight_control_setpoint_ENUposition_yaw", 10);
 
  ctrlBrakePub = nh.advertise("dji_sdk/flight_control_setpoint_generic", 10);
  
  // Basic services
  sdk_ctrl_authority_service = nh.serviceClient ("dji_sdk/sdk_control_authority");
  drone_task_service         = nh.serviceClient("dji_sdk/drone_task_control");
  query_version_service      = nh.serviceClient("dji_sdk/query_drone_version");

之后发布控制指令及启动基本服务。

  if(takeoff_result)
  {
    square_mission.reset();
    square_mission.start_gps_location = current_gps;
    square_mission.setTarget(0, 20, 3, 60);
    square_mission.state = 1;
    ROS_INFO("##### Start route %d ....", square_mission.state);
  }

最后是具体任务规划的函数，事实上这里只是定义了初始状态，具体过程是通过不断调用回调函数gps_callback来完成的

void gps_callback(const sensor_msgs::NavSatFix::ConstPtr& msg)
{
  static ros::Time start_time = ros::Time::now();
  ros::Duration elapsed_time = ros::Time::now() - start_time;
  current_gps = *msg;
 
  // Down sampled to 50Hz loop
  if(elapsed_time > ros::Duration(0.02))
  {
    start_time = ros::Time::now();
    switch(square_mission.state)
    {
      case 0:
        break;
 
      case 1:
        if(!square_mission.finished)
        {
          square_mission.step();
        }
        else
        {
          square_mission.reset();
          square_mission.start_gps_location = current_gps;
          square_mission.setTarget(20, 0, 0, 0);
          square_mission.state = 2;
          ROS_INFO("##### Start route %d ....", square_mission.state);
        }
        break;
 
      case 2:
        if(!square_mission.finished)
        {
          square_mission.step();
        }
        else
        {
          square_mission.reset();
          square_mission.start_gps_location = current_gps;
          square_mission.setTarget(0, -20, 0, 0);
          square_mission.state = 3;
          ROS_INFO("##### Start route %d ....", square_mission.state);
        }
        break;
      case 3:
        if(!square_mission.finished)
        {
          square_mission.step();
        }
        else
        {
          square_mission.reset();
          square_mission.start_gps_location = current_gps;
          square_mission.setTarget(-20, 0, 0, 0);
          square_mission.state = 4;
          ROS_INFO("##### Start route %d ....", square_mission.state);
        }
        break;
      case 4:
        if(!square_mission.finished)
        {
          square_mission.step();
        }
        else
        {
          ROS_INFO("##### Mission %d Finished ....", square_mission.state);
          square_mission.state = 0;
        }
        break;
    }
  }
}

通过state来分段控制，通过finished来结束过程。

其他几个demo也是类似的。

M100控制指令的构成

ROS下的控制指令依旧通过发布话题和订阅话题来完成

M100通过发布sensor_msgs::Joy来完成控制命令，例如

ctrlBrakePub = nh.advertise("dji_sdk/flight_control_setpoint_generic", 10);

具体如下

sensor_msgs::Joy controlVelYawRate;
    uint8_t flag = (DJISDK::VERTICAL_VELOCITY   |
                DJISDK::HORIZONTAL_VELOCITY |
                DJISDK::YAW_RATE            |
                DJISDK::HORIZONTAL_GROUND   |
                DJISDK::STABLE_ENABLE);
    controlVelYawRate.axes.push_back(0);
    controlVelYawRate.axes.push_back(0);
    controlVelYawRate.axes.push_back(0);
    controlVelYawRate.axes.push_back(0);
    controlVelYawRate.axes.push_back(flag);
 
    ctrlBrakePub.publish(controlVelYawRate);

这里的关键在这个FLAG上，FLAG是个8位整形数

1:水平控制模式

enum HorizontalLogic
  {
    /*!
     - Set the control-mode to control pitch & roll
     angle of the vehicle.
     - Need to be referenced to either the ground or
     body frame by HorizontalCoordinate setting.
     - Limit: 35 degree
     */
    HORIZONTAL_ANGLE = 0x00,
    /*!
     - Set the control-mode to control horizontal
     vehicle velocities.
     - Need to be referenced to either the ground
     or body frame by HorizontalCoordinate setting.
     - Limit: 30 m/s
     */
    HORIZONTAL_VELOCITY = 0x40,
    /*!
     - Set the control-mode to control position
     offsets of pitch & roll directions
     - Need to be referenced to either the ground
     or body frame by HorizontalCoordinate setting.
     - Limit: N/A
     */
    HORIZONTAL_POSITION = 0x80,
    /*!
     - Set the control-mode to control rate of
     change of the vehicle's attitude
     - Need to be referenced to either the ground
     or body frame by HorizontalCoordinate setting.
     - Limit: 150.0 deg/s
     */
    HORIZONTAL_ANGULAR_RATE = 0xC0
  };

2：控制垂直模式

enum VerticalLogic
  {
    /*!
     - Set the control-mode to control the vertical
       speed of UAV, upward is positive
     - Limit: -5 to 5 m/s
     */
    VERTICAL_VELOCITY = 0x00,
    /*!
     - Set the control-mode to control the height of UAV
     - Limit: 0 to 120 m
     */
    VERTICAL_POSITION = 0x10,
    /*!
     - Set the control-mode to directly control the thrust
     - Range: 0% to 100%
     */
    VERTICAL_THRUST = 0x20,
  };

3：控制YAW

enum YawLogic
  {
    /*!
     - Set the control-mode to control yaw angle.
     - Yaw angle is referenced to the ground frame.
     - In this control mode, Ground frame is enforeced in Autopilot.
     */
    YAW_ANGLE = 0x00,
    /*!
     - Set the control-mode to control yaw angular velocity.
     - Same reference frame as YAW_ANGLE.
     - Limite: 150 deg/s
     */
    YAW_RATE = 0x08
  };

4：参考系选择

enum HorizontalCoordinate
  {
    /*! Set the x-y of ground frame as the horizontal frame (NEU) */
    HORIZONTAL_GROUND = 0x00,
    /*! Set the x-y of body frame as the horizontal frame (FRU) */
    HORIZONTAL_BODY = 0x02
  };

其中1为东北天参考系，2为机体参考系

5：自动悬停选择

enum StableMode
  {
    STABLE_DISABLE = 0x00, /*!< Disable the stable mode */
    STABLE_ENABLE  = 0x01  /*!< Enable the stable mode */
  };