DJI OSDK-flight_control_samples学习

flight_control_sample.hpp

/*! @file flight_control_sample.hpp
 *  @version 3.3
 *  @date Jun 05 2017
 *
 *  @brief
 *  Flight Control API usage in a Linux environment.
 *  Provides a number of helpful additions to core API calls,
 *  especially for position control, attitude control, takeoff,
 *  landing.
 *
 *  @Copyright (c) 2016-2017 DJI
 *
 * Permission is hereby granted, free of charge, to any person obtaining a copy
 * of this software and associated documentation files (the "Software"), to deal
 * in the Software without restriction, including without limitation the rights
 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
 * copies of the Software, and to permit persons to whom the Software is
 * furnished to do so, subject to the following conditions:
 *
 * The above copyright notice and this permission notice shall be included in
 * all copies or substantial portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
 * SOFTWARE.
 *
 */

#ifndef DJIOSDK_FLIGHTCONTROL_HPP
#define DJIOSDK_FLIGHTCONTROL_HPP

// System Includes
#include <cmath>

// DJI OSDK includes
#include "dji_status.hpp"
#include <dji_vehicle.hpp>

// Helpers
#include <dji_linux_helpers.hpp>

#define C_EARTH (double)6378137.0
#define DEG2RAD 0.01745329252

//!@note: All the default timeout parameters are for acknowledgement packets
//! from the aircraft. 所有默认超时参数均用于来自飞机的确认数据包。

/*! Monitored Takeoff
    This implementation of takeoff  with monitoring makes sure your aircraft
    actually took off and only returns when takeoff is complete.
    Use unless you want to do other stuff during takeoff - this will block
    the main thread.
!*/
bool monitoredTakeoff(DJI::OSDK::Vehicle* vehiclePtr, int timeout = 1); //声明监视起飞函数

// Examples of commonly used Flight Mode APIs

/*! Position Control. Allows you to set an offset from your current
    location. The aircraft will move to that position and stay there.
    Typical use would be as a building block in an outer loop that does not
    require many fast changes, perhaps a few-waypoint trajectory. For smoother
    transition and response you should convert your trajectory to attitude
    setpoints and use attitude control or convert to velocity setpoints
    and use velocity control.
!*/
bool moveByPositionOffset(DJI::OSDK::Vehicle *vehicle, float xOffsetDesired,  //声明位置控制函数
                          float yOffsetDesired, float zOffsetDesired,
                          float yawDesired, float posThresholdInM = 0.2,
                          float yawThresholdInDeg = 1.0);

/*! Monitored Landing (Blocking API call). Return status as well as ack.
    This version of takeoff makes sure your aircraft actually took off
    and only returns when takeoff is complete.

!*/
bool monitoredLanding(DJI::OSDK::Vehicle* vehiclePtr, int timeout = 1); //声明监视着陆函数

// Helper Functions

/*! Very simple calculation of local NED offset between two pairs of GPS
 * coordinates.
 *
 * Accurate when distances are small.
!*/
void localOffsetFromGpsOffset(DJI::OSDK::Vehicle*             vehicle,  //声明利用Gps偏移计算本地偏移的函数
                              DJI::OSDK::Telemetry::Vector3f& deltaNed,
                              void* target, void* origin);

DJI::OSDK::Telemetry::Vector3f toEulerAngle(void* quaternionData);      //声明四元数转化为欧拉角函数
bool startGlobalPositionBroadcast(DJI::OSDK::Vehicle* vehicle);         //声明全局位置广播函数


#endif // DJIOSDK_FLIGHTCONTROL_HPP

main.cpp

/*! @file flight-control/main.cpp
 *  @version 3.3
 *  @date Jun 05 2017
 *
 *  @brief
 *  main for Flight Control API usage in a Linux environment.
 *  Provides a number of helpful additions to core API calls,
 *  especially for position control, attitude control, takeoff,
 *  landing.
 *
 *  @Copyright (c) 2016-2017 DJI
 *
 * Permission is hereby granted, free of charge, to any person obtaining a copy
 * of this software and associated documentation files (the "Software"), to deal
 * in the Software without restriction, including without limitation the rights
 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
 * copies of the Software, and to permit persons to whom the Software is
 * furnished to do so, subject to the following conditions:
 *
 * The above copyright notice and this permission notice shall be included in
 * all copies or substantial portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
 * SOFTWARE.
 *
 */

#include "flight_control_sample.hpp"

using namespace DJI::OSDK;
using namespace DJI::OSDK::Telemetry;

/*! main
 *
 */
int
main(int argc, char** argv)
{
   
  // Initialize variables
  int functionTimeout = 1;

  // Setup OSDK.
  LinuxSetup linuxEnvironment(argc, argv);
  Vehicle*   vehicle = linuxEnvironment.getVehicle();
  if (vehicle == NULL)
  {
   
    std::cout << "Vehicle not initialized, exiting.\n";
    return -1;
  }

  // Obtain Control Authority
  vehicle->obtainCtrlAuthority(functionTimeout);

  // Display interactive prompt
  std::cout
    << "| Available commands:                                            |"
    << std::endl;
  std::cout
    << "| [a] Monitored Takeoff + Landing                                |"
    << std::endl;
  std::cout
    << "| [b] Monitored Takeoff + Position Control + Landing             |"
    << std::endl;
  char inputChar;
  std::cin >> inputChar;

  switch (inputChar)
  {
   
    case 'a':
      monitoredTakeoff(vehicle);
      monitoredLanding(vehicle);
      break;
    case 'b':
      monitoredTakeoff(vehicle);
      moveByPositionOffset(vehicle, 0, 6, 6, 30);
      moveByPositionOffset(vehicle, 6, 0, -3, -30);
      moveByPositionOffset(vehicle, -6, -6, 0, 0);
      monitoredLanding(vehicle);
      break;
    default:
      break;
  }

  return 0;
}

flight_control_sample.cpp

/*! @file flight_control_sample.cpp
 *  @version 3.3
 *  @date Jun 05 2017
 *
 *  @brief
 *  Flight Control API usage in a Linux environment.
 *  Provides a number of helpful additions to core API calls,
 *  especially for position control, attitude control, takeoff,
 *  landing.
 *  在Linux环境中使用Flight Control API。 为核心API的调用提供了许多有用的补充，尤其是在位置控制，姿态控制，起飞，着陆方面。
 *  @Copyright (c) 2016-2017 DJI
 *
 * Permission is hereby granted, free of charge, to any person obtaining a copy
 * of this software and associated documentation files (the "Software"), to deal
 * in the Software without restriction, including without limitation the rights
 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
 * copies of the Software, and to permit persons to whom the Software is
 * furnished to do so, subject to the following conditions:
 *
 * The above copyright notice and this permission notice shall be included in
 * all copies or substantial portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
 * SOFTWARE.
 *
 */

#include "flight_control_sample.hpp"

using namespace DJI::OSDK;
using namespace DJI::OSDK::Telemetry;

/*! Monitored Takeoff (Blocking API call). Return status as well as ack.
    This version of takeoff makes sure your aircraft actually took off
    and only returns when takeoff is complete.
    Use unless you want to do other stuff during takeoff - this will block
    the main thread.
	监视起飞（阻止API调用）。返回状态以及确认。此版本的起飞确保您的飞机
	实际起飞并且仅在起飞完成后才返回。除非您要在起飞过程中做其他事情，否
	则请使用-这会阻塞主线程。
!*/
bool
monitoredTakeoff(Vehicle* vehicle, int timeout) //定义监视起飞函数，vehicle定义为一个指针，类型是Vehicle
{
   
  //@todo: remove this once the getErrorCode function signature changes
  char func[50];                                                        //声明func[50]，用于存储返回的消息字符串，类型为char       
  int  pkgIndex;                                                        //声明pkgIndex，类型为int

  if (!vehicle->isM100() && !vehicle->isLegacyM600())                   //M100和M600均向后不兼容，执行if语句
  {
   
    // Telemetry: Verify the subscription                               //校验订阅
    
	ACK::ErrorCode subscribeStatus;                                     //声明订阅状态，类型为ACK::ErrorCode
    subscribeStatus = vehicle->subscribe->verify(timeout);              //verify(timeout)版本匹配的阻塞调用，返回版本匹配后的确认消息ack

    if (ACK::getError(subscribeStatus) != ACK::SUCCESS)                 //如果调用失败，通过ACK::getError()和ACK::getErrorCodeMessage()函数分析失败的
    {
                                                                      //原因，调用成功则不执行if判断
      ACK::getErrorCodeMessage(subscribeStatus, func);                  
      return false;                                                     //返回false
    }

    // Telemetry: Subscribe to flight status and mode at freq 10 Hz     //遥测：以10Hz的频率订阅飞行状态和模式这两个话题
    pkgIndex                  = 0;                                      //初始化功能包的ID，pkgIndex=0
    int       freq            = 10;                                     //初始化频率freq=10
    TopicName topicList10Hz[] = {
    TOPIC_STATUS_FLIGHT,                  //声明订阅的话题列表，TOPIC_STATUS_FLIGHT提供高达50Hz的飞机内部飞行状态。
                                  TOPIC_STATUS_DISPLAYMODE };           //TOPIC_STATUS_DISPLAYMODE提供高达50Hz的各种任务/飞行模式的精细状态表示。
    int  numTopic        = sizeof(topicList10Hz) / sizeof(topicList10Hz[0]);  //声明话题的个数为2
    bool enableTimestamp = false;                                       //不允许使用时间戳

    bool pkgStatus = vehicle->subscribe->initPackageFromTopicList(  
      pkgIndex, numTopic, topicList10Hz, enableTimestamp, freq);        //声明初始化功能包函数，返回值赋给pkgStatus
	                                                                    //initPackageFromTopicList:这是用户生成订阅功能包的最终接口。
																		//pkgIndex：将生成功能包的ID。topicList10Hz：
																		//在功能包中需要订阅的话题名称的列表返回一个bool值
    if (!(pkgStatus))                                                   //初始化成功则不执行if判断，否则执行if判断，并且返回pkgStatus的bool值
    {
   
      return pkgStatus;
    }
    subscribeStatus = vehicle->subscribe->startPackage(pkgIndex, timeout); //开始功能包的阻塞调用，返回一个ack的确认消息，并传输到subscribeStatus
    if (ACK::getError(subscribeStatus) != ACK::SUCCESS)                 //如果调用成功，则不执行if语句，调用失败，则通过ACK::getErrorCodeMessage()	                                                                   
    {
                                                                      //函数返回失败的原因
      ACK::getErrorCodeMessage(subscribeStatus, func);                 
      // Cleanup before return                                          //在返回之前清理掉调用失败的功能包
      vehicle->subscribe->removePackage(pkgIndex, timeout);             //移除功能包的非阻塞调用
      return false;
    }
  }

  // Start takeoff                                                      //开始起飞
  ACK::ErrorCode takeoffStatus = vehicle->control->takeoff(timeout);    //声明起飞状态takeoffStatus，类型是ACK::ErrorCode，
																	    //takeoff(timeout)：起飞的封装函数
  if (ACK::getError(takeoffStatus) != ACK::SUCCESS)                     //如果调用失败，通过ACK::getErrorCodeMessage()来返回失败的原因
  {
   
    ACK::getErrorCodeMessage(takeoffStatus, func);                      	                                                                    
    return false;
  }

  // First check: Motors started                                        //检查电机是否启动
  int motorsNotStarted = 0;                                             //声明motorsNotStarted
  int timeoutCycles    = 20;                                            //声明周期阈值

  if (!vehicle->isM100() && !vehicle->isLegacyM600())                   //M100和M600版本都不向后兼容，则执行if语句
  {
   
    while (vehicle->subscribe->getValue<TOPIC_STATUS_FLIGHT>() !=       //飞机处于STOPPED模式
             VehicleStatus::FlightStatus::ON_GROUND &&                  // VehicleStatus::FlightStatus:STOPED = 0, ON_GROUND = 1, IN_AIR = 2 
           vehicle->subscribe->getValue<TOPIC_STATUS_DISPLAYMODE>() !=  //飞机电机没有启动
             VehicleStatus::DisplayMode::MODE_ENGINE_START &&           //MODE_ENGINE_START:电动机启动模式。 MODE_ENGINE_START = 41,
           motorsNotStarted < timeoutCycles)                            //电机启动时间小于设定的阈值
    {
   
      motorsNotStarted++;                                               //增加时间
      usleep(100000);                                                   //usleep函数能把线程挂起一段时间(0.1s)，单位是微秒（千分之一毫秒）
    }

    if (motorsNotStarted == timeoutCycles)                              //时间未到达阈值，不执行
    {
   
      std::cout << "Takeoff failed. Motors are not spinning." << std::endl; //输出起飞失败，电机没有旋转
      // Cleanup                                                        //清理package
      if (!vehicle->isM100() && !vehicle->isLegacyM600())               //M100和M600版本都不向后兼容，则执行if语句
      {
   
        vehicle->subscribe->removePackage(0, timeout);                  //移除功能包0的非阻塞调用
      }
      return false;                                                     //返回false
    }
    else                                                                //飞机处于ON_GROUND模式，并且电机已经启动
    {
   
      std::cout << "Motors spinning...\n";                              //电机正在旋转
    }
  }
  else if (vehicle->isLegacyM600())                                     //M600向后兼容（M600和M100用的是广播）
  {
   
    while ((vehicle->broadcast->getStatus().flight <                    //Get Status (flight status, mode, gear and error) from local cache
            DJI::OSDK::VehicleStatus::FlightStatus::ON_GROUND) &&
           motorsNotStarted < timeoutCycles)
    {
   
      motorsNotStarted++;
      usleep(100000);
    }

    i