大疆Onboard-SDK-ROS-3.6.1代码解读（一）

demo_flight_control.cpp

大致观察代码运行的流程，每一部分后面后详细补充说明

设置全局变量，程序在任意时刻都会调用到，具体会在 int main() 主函数中定义。

/* 服务的客户端 */
ros::ServiceClient set_local_pos_reference;     // 惯性坐标
ros::ServiceClient sdk_ctrl_authority_service;  // SDK控制权限
ros::ServiceClient drone_task_service;          // 起飞、悬停、降落任务
ros::ServiceClient query_version_service;       // 

/* 发布者 */
ros::Publisher ctrlPosYawPub;                   // 位置（x,y,z）偏航角（yaw） 
ros::Publisher ctrlBrakePub;                    // 刹车

/* 订阅话题的全局变量 */
uint8_t flight_status = 255;
uint8_t display_mode  = 255;

/* 
    * 全局变量 
*/

/* 当前GPS坐标 */
sensor_msgs::NavSatFix current_gps;

/* 当前姿态 */
geometry_msgs::Quaternion current_atti;

/* 当前惯性坐标 */
geometry_msgs::Point current_local_pos;

/* 任务 */
Mission square_mission;

---

在main() 函数中完成定义和初始化一些变量

消息订阅者：姿态角、GPS、惯性坐标、飞行状态

消息发布者：控制量（x、y、z、yaw）、控制类型（flag 这个会在后面详细介绍）

服务客户端：请求控制权限、请求飞行任务、未知（待补充）、请求设置惯性参考系。（这些服务由 dji_sdk_node 节点开启）

  /* 
        * 消息订阅
  */

  /* 姿态角（欧拉角） */
  ros::Subscriber attitudeSub = nh.subscribe("dji_sdk/attitude", 10, &attitude_callback);
  /* GPS */
  ros::Subscriber gpsSub      = nh.subscribe("dji_sdk/gps_position", 10, &gps_callback);
  /* 惯性坐标 */
  ros::Subscriber localPosition = nh.subscribe("dji_sdk/local_position", 10, &local_position_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);
 

  /*
        * 消息发布
  */

  /* 目标位置与偏航角（全局变量）*/ 
  ctrlPosYawPub = nh.advertise("dji_sdk/flight_control_setpoint_ENUposition_yaw", 10);
  
  /*  发布控制指令  */
  ctrlBrakePub = nh.advertise("dji_sdk/flight_control_setpoint_generic", 10);
  
  /*
        * 服务的客户端
  */

  /* 请求控制权限 */
  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");
  
  /* 设置惯性系参考点（惯性系位置需要在程序开始时进行设置参考点可用）*/
  set_local_pos_reference    = nh.serviceClient ("dji_sdk/set_local_pos_ref");

请求控制权限，返回权限获取的结果

 /* 请求控制权限 */
  bool obtain_control_result = obtain_control();

obtain_control()函数详情

/* 请求控制权  obtain-获取 */
bool obtain_control()
{
  /* 定义服务的数据类型 */  
  dji_sdk::SDKControlAuthority authority;

  /* 使能控制权限 */
  authority.request.control_enable=1;

  /* 发送控制权请求 */
  sdk_ctrl_authority_service.call(authority);

  /* 请求失败 */
  if(!authority.response.result)
  {
    /* 获取控制权限失败 */
    ROS_ERROR("obtain control failed!");
    return false;
  }

  /* 请求成功 */
  return true;
}

定义一个起飞结果的 bool 型变量，当起飞成功后，在能执行后续的飞行任务。

  /* 起飞结果 */
  bool takeoff_result;

判断是否初始化惯性参考系 

  /* 判断是否设置惯性参考系 */
  if (!set_local_position()) // We need this for height
  {
    ROS_ERROR("GPS health insufficient - No local frame reference for height. Exiting.");
    return 1;
  }

set_local_position()函数详情

bool set_local_position()
{
  /* 服务的消息类型 */
  dji_sdk::SetLocalPosRef localPosReferenceSetter;

  /* 发送设置惯性参考系请求 */
  set_local_pos_reference.call(localPosReferenceSetter);
  
  /* 返回请求结果 */
  return localPosReferenceSetter.response.result;

}

判断飞机的类型，监控飞机的起飞过程，返回起飞结果。

 /* 如果是 M100 机型 */
  if(is_M100())
  {
    /* 打印：M100 正在起飞 */
    ROS_INFO("M100 taking off!");

    /* 监控 M100 起飞状态 ，返回起飞结果 */
    takeoff_result = M100monitoredTakeoff();
  }
  else
  {
    /* 因为我用的是 M100 机型，不考虑 A3/N3 飞控的情况 */
    ROS_INFO("A3/N3 taking off!");

    takeoff_result = monitoredTakeoff();
  }

is_M100()函数详情

/* 判断是否为M100机型 */
bool is_M100()
{
  /* 查询飞机版本服务的数据类型 */
  dji_sdk::QueryDroneVersion query;

  /* 发送请求 */
  query_version_service.call(query);
  
  /* 返回请求结果 */
  /* 判读是否为 M100 */
  if(query.response.version == DJISDK::DroneFirmwareVersion::M100_31)
  {
    return true;
  }

  return false;
}

monitoredTakeoff()函数详情

/* 监控起飞状态 */
bool M100monitoredTakeoff()
{
  /* 获取开始时间 */
  ros::Time start_time = ros::Time::now();

  /* 初始高度 */
  /* current_gps 是前面定义的全局变量，程序启动后，会持续的订阅 GPS 信息 */
  float home_altitude = current_gps.altitude;

  /* 如果请求起飞失败 */
  if(!takeoff_land(dji_sdk::DroneTaskControl::Request::TASK_TAKEOFF))
  {
    return false;
  }


  /* 请求起飞成功 */

  /* 延时 10ms ，继续订阅信息 */
  ros::Duration(0.01).sleep();
  ros::spinOnce();

  // Step 1: If M100 is not in the air after 10 seconds, fail.

  // 第一步，如果 M100 在 10s后没在空中，则起飞失败

  /* 等待 10s */
  while (ros::Time::now() - start_time < ros::Duration(10))
  {
    /* 循环延时 10ms ，持续订阅信息 */
    ros::Duration(0.01).sleep();
    ros::spinOnce();
  }

  /* 10s 后 */
  /* 如果飞机状态显示不在空中 或 高度小于 1m ,则起飞失败 */
  if(flight_status != DJISDK::M100FlightStatus::M100_STATUS_IN_AIR ||
      current_gps.altitude - home_altitude < 1.0)
  {
    ROS_ERROR("Takeoff failed.");
    return false;
  }

  /* 否则起飞成功 */
  else
  {
    start_time = ros::Time::now();
    ROS_INFO("Successful takeoff!");
    ros::spinOnce();
  }

  return true;
}

接下来就是核心的设置初始任务

/* 如果起飞 */
  if(takeoff_result)
  {
    /* 设置第一个任务点 */

    /* 重置 */
    square_mission.reset();
    
    /* 设置初始的GPS位置 */
    square_mission.start_gps_location = current_gps;

    /* 设置初始惯性系坐标 */
    square_mission.start_local_position = current_local_pos;
    
    /* 设置任务点 */
    square_mission.setTarget(0, 20, 3, 60);
    
    /* 设置任务状态为1 */
    square_mission.state = 1;

    ROS_INFO("##### Start route %d ....", square_mission.state);
  }

任务的执行过程在 gps_callback()函数中，后续会介绍。

这就是 demo_flight_control.cpp 的大概执行过程。