大疆Onboard SDK 3.2玫瑰线例程浅析

大疆Onboard SDK 3.2玫瑰线例程浅析

前不久参加2017英飞凌杯无人机竞赛，研究了一下大疆的OnbardSDK，现在比赛结束了，简单写一下其中自带的玫瑰线例程分析，也算对比赛进行一点总结。技术渣，有错误的地方还请各位看官指出。

• 基于英飞凌XMC4700的Onboard SDK 3.2版本
• 调用dji N3飞控api接口实现飞行器控制

---

使用API

using namespace DJI::onboardSDK;

HardDriver* driver = new XMC_4700;//适配XMC4700
CoreAPI defaultAPI = CoreAPI(driver);
CoreAPI *coreApi = &defaultAPI;//利用构造函数初始化CoreAPI类

Flight flight = Flight(coreApi);//Flight类封装了所有飞行控制相关功能。
FlightData flightData;

广播(Broadcast)

3.2是Onboard SDK中较老的版本，要想获取N3飞控信息，只能通过广播（Broadcast）机制，在新版本中还有遥测（Telemetry ）和订阅（Subscription ）两种机制，在此不提。

在使用广播前，要设定广播数据发送频率，默认如下（在DJI assistant软件中也可设定）

uint8_t myFreq[16] =
{
  BROADCAST_FREQ_10HZ, //0 - Timestamp
  BROADCAST_FREQ_10HZ, //1 - Attitude Quaterniouns
  BROADCAST_FREQ_10HZ, //2 - Acceleration
  BROADCAST_FREQ_50HZ, //3 - Velocity (Ground Frame)
  BROADCAST_FREQ_10HZ, //4 - Angular Velocity (Body Frame)
  BROADCAST_FREQ_50HZ, //5 - Position
  BROADCAST_FREQ_10HZ, //6 - Magnetometer
  BROADCAST_FREQ_10HZ, //7 - RC Channels Data
  BROADCAST_FREQ_10HZ, //8 - Gimbal Data
  BROADCAST_FREQ_10HZ, //9 - Flight Status
  BROADCAST_FREQ_10HZ, //10 - Battery Level
  BROADCAST_FREQ_10HZ  //11 - Control Information
};

1、可以看到myFreq[16]，总共有16个空间，但这里只填了11个频率设置，其余的5个频率是为A3附加套件预留的， 加装后可以构成双IMU冗余控制，而且可以获得更精确的GPS && RTK定位信息。

2、广播会以最高的频率发送数据包，频率较慢的数据每数个发送周期更新。例如上图设定，广播数据包以10Hz频率更新。Timestamp每次都会更新，而Position每5次更新一次。

3、频率定好后，调用API设定即可

coreApi->setBroadcastFreq(myFreq);

4、最后看一下可以从广播获得什么信息

typedef struct BroadcastData
{
  unsigned short dataFlag;
  TimeStampData timeStamp;
  //四元数
  QuaternionData q; 
  //解算到局部笛卡尔系
  CommonData a 
  VelocityData v;
  CommonData w;
  //位置信息（包含经纬度、海拔、增量高度、信号强度）（重要）
  PositionData pos;
  //磁罗盘信息
  MagnetData mag;
  //仅A3支持
  GPSData gps;
  //仅A3支持
  RTKData rtk;
  //遥控器实时输入信息
  RadioData rc;
  //姿态
  GimbalData gimbal;
  FlightStatus status; //! @todo define enum
  BatteryData battery;
  CtrlInfoData ctrlInfo;

  //! @note this variable is not set by the FC but populated by the API
  uint8_t activation;
} BroadcastData;

飞行控制

控制协议

OnboardSDK可通过一系列16进制命令控制N3飞控完成相应的飞行动作，默认支持如下：链接

功能	指令
获取SDK版本	0xFA 0xFB 0x00 0xFE
激活	0xFA 0xFB 0x01 0xFE
获取控制权	0xFA 0xFB 0x02 0x01 0xFE
释放控制权	0xFA 0xFB 0x02 0x00 0xFE
启动电机（Arm）	0xFA 0xFB 0x03 0x01 0xFE
停转电机	0xFA 0xFB 0x03 0x00 0xFE
移动控制（分高低16位发）	0xFA 0xFB 0x04 0x01 Flag x_H x_L y_H y_L z_H z_L yaw_H yaw_L 0xFE
移动控制（dry-run）（分高低16位发）	0xFA 0xFB 0x04 0x02 Flag x_H x_L y_H y_L z_H z_L yaw_H yaw_L 0xFE
返航	0xFA 0xFB 0x05 0x01 0xFE
自动起飞	0xFA 0xFB 0x05 0x02 0xFE
自动降落	0xFA 0xFB 0x05 0x03 0xFE
虚拟遥控（模式A）	0xFA 0xFB 0x06 0x01 0xFE
虚拟遥控（模式F）	0xFA 0xFB 0x06 0x02 0xFE
虚拟遥控关闭	0xFA 0xFB 0x06 0x03 0xFE
开始热点环绕任务	0xFA 0xFB 0x07 0x00 Altitude Angular_Speed Radius 0xFE
关闭热点环绕任务	0xFA 0xFB 0x07 0x01 0xFE
获取广播信息	0xFA 0xFB 0x08 0x00 0xFE
开启飞行例程	0xFA 0xFB 0x09 0x01 0xFE
结束飞行例程	0xFA 0xFB 0x09 0x00 0xFE

其中0xFA 0xFB是协议头，0xFE是结尾。一但读到0xFE，指令将立即执行
程序中是通过逐位判断的方法解析指令的，具体如下

void TerminalCommand::terminalCommandHandler(CoreAPI* api, Flight* flight)
{
  if (cmdReadyFlag == 1)
  {
    cmdReadyFlag = 0;
  }
  else
  { // No full command has been received yet.
    return;
  }

  if ((cmdIn[0] != 0xFA) || (cmdIn[1] != 0xFB))
  { // Command header doesn't match
    return;
  }

  switch(cmdIn[2])
  {
  case 0x00:
    api->getDroneVersion();
    break;

  case 0x01:
    User_Activate();
    break;

  case 0x02:
    api->setControl(cmdIn[3]);
    break;

  case 0x03:
    flight->setArm(cmdIn[3]);
    break;

  case 0x04:
    if (cmdIn[3] == 0x01)
    {
      flightData.flag = cmdIn[4];
      flightData.x = hex2Float(cmdIn[5], cmdIn[6]);
      flightData.y = hex2Float(cmdIn[7], cmdIn[8]);
      flightData.z = hex2Float(cmdIn[9], cmdIn[10]);
      flightData.yaw = hex2Float(cmdIn[11], cmdIn[12]);
      flight->setFlight(&flightData);//旧接口的写法
      XMC_CCU4_SLICE_StartTimer(SLICE1_PTR);
      printf("roll_or_x =%f\n", hex2Float(cmdIn[5], cmdIn[6]));
      printf("pitch_or_y =%f\n", hex2Float(cmdIn[7], cmdIn[8]));
      printf("thr_z =%f\n", hex2Float(cmdIn[9], cmdIn[10]));
      printf("yaw =%f\n\n", hex2Float(cmdIn[11], cmdIn[12]));
    }
    else if (cmdIn[3] == 0x02)
    {//for display ur hex2int
      printf("roll_or_x =%f\n", hex2Float(cmdIn[5], cmdIn[6]));
      printf("pitch_or_y =%f\n", hex2Float(cmdIn[7], cmdIn[8]));
      printf("thr_z =%f\n", hex2Float(cmdIn[9], cmdIn[10]));
      printf("yaw =%f\n\n", hex2Float(cmdIn[11], cmdIn[12]));
    }
    break;

  case 0x05:
    switch(cmdIn[3])
    {
    case 0x01:
      flight->task(Flight::TASK_GOHOME);
      break;
    case 0x02:
      flight->task(Flight::TASK_TAKEOFF);
            droneState.homePositionLLA = coreApi->getBroadcastData().pos;
            droneState.homePositionSetFlag=1;
      break;
    case 0x03:
      flight->task(Flight::TASK_LANDING);
      break;
    }
    break;

  case 0x06:
    if (cmdIn[3] == 0x00)              //0x06 0x00 to stop VRC
    {
      VRCResetData();
      XMC_CCU4_SLICE_StopTimer(SLICE0_PTR);
    }
    else if (cmdIn[3] == 0x02)            //0x06 0x01 to turn VRC to F gear
    {
      VRC_TakeControl();
      XMC_CCU4_SLICE_StartTimer(SLICE0_PTR);
    }
    else if (cmdIn[3] == 0x01)          //0x06 0x02 to reset data
    {
      VRCResetData();
      XMC_CCU4_SLICE_StartTimer(SLICE0_PTR);
    };
    break;

  case 0x07:
    if(cmdIn[3] == 0x00)
    {
      tryHotpoint(cmdIn[4], cmdIn[5], cmdIn[6]);
    }
    else
    {
      stopHotpoint();
    }
    break;
  case 0x08:
    printf("TimeStamp is %d\r\n", api->getBroadcastData().timeStamp.time);
    printf("Battery capacity remains %d percent\r\n",
        api->getBroadcastData().battery);//具体想要获得什么信息可以自己改
    break;

  case 0x09:
    if (cmdIn[3] == 0x01)
    {
      if(cmdIn[3] == 0x00)
                startLocalNavExample(0);
            else if(cmdIn[3] == 0x01)
                startLocalNavExample(1);
    }
    else if(cmdIn[3] == 0x00)
    {
      stopLocalNavExample();
    }
    break;

 case 0x10:
     ......;//仿照以上格式，可以自定义指令
  default:
    break;
  }
}

1、利用无线串口，将指令通过上位机发给MCU，再通过串口发给N3飞控，即可实现飞行控制；也可以利用遥控器多余通道，通过广播数据获取控制信息，直接调用各种控制函数，进行飞行控制。
2、注意，Onboard SDK的控制权限是最低的，需要接管时，遥控器调整到P模式，并把所有摇杆回中，发指令让MCU获取控制权才行
3、附上与上位机通讯部分的接受中断服务函数如下：

void USIC0_5_IRQHandler(void)
{

    uint8_t oneByte = XMC_UART_CH_GetReceivedData(pc_uart);
    if (myTerminal.rxIndex == 0)
    {
      if (oneByte == 0xFA)
      {
        myTerminal.cmdIn[myTerminal.rxIndex] = oneByte;
        myTerminal.rxIndex = 1;
      }
      else
      {;}
    }
    else
    {
      if (oneByte == 0xFE)    //receive a 0xFE would lead to a command-execution
      {
        myTerminal.cmdIn[myTerminal.rxIndex] = oneByte;
        myTerminal.rxLength = myTerminal.rxIndex + 1;
        myTerminal.rxIndex = 0;
        myTerminal.cmdReadyFlag = 1;
      }
      else
      {
        myTerminal.cmdIn[myTerminal.rxIndex] = oneByte;
        myTerminal.rxIndex++;
      }
    }
}

移动控制协议

重点说一下上面的移动控制部分，这是最常用的一个功能，也比较复杂（注意使用此命令需要先起飞）

移动控制指令由以下几部分组成（按顺序）

0、协议头 0xFA 0xFB
1、控制模式标志
2、X轴控制值（高字节和低字节）
3、Y轴控制值（高字节和低字节）
4、Z轴控制值（高字节和低字节）
5、偏航控制值（高字节和低字节）
6、协议尾0xFE

控制模式标志

控制模型标志确定如何解释控制数据的每个分量（X，Y，Z，Yaw），具体如下：链接

位	功能
0	1->增稳模式 0->不增稳
位2：1	00->地面坐标系 01->机身坐标系
第3位	0->yaw角度控制 1->yaw角速度控制
位5：4	00->垂直速度控制 01->垂直位置控制 10->垂直推力控制
位7：6	00->俯仰&滚转角度控制 01->水平速度控制 10->水平位置控制

请点击上方链接查看每种设定的详细解释及注意事项

以下是运动控制命令的示例：
1、0xFA 0xFB 0x04 0x01 0x48 0x00 0x64 0x00 0xC8 0x00 0x64 0x00 0x05 0xFE
控制位0x48，即01001000，对照上表知为：不增稳，地面系，yaw角速度控制，垂直速度控制，水平速度控制

程序将每个通道的高字节和低字节组装为整数， 然后将其除以100 。 所以在上面的例子中，X（北）（0x64）速度设置为1 m / s，Y（东）( 0xC8)速度设置为2 m / s，Z速度（0x64）设置为1 m / s并设置偏航率(0x05 )至0.05 rad / s。

2、0xFA 0xFB 0x04 0x01 0x91 0x00 0x00 0x00 0x00 0x00 0xC8 0x00 0x00 0xFE
控制位0x91，即1001 0001，对照上表知为：增稳，地面系，yaw角度控制，垂直位置控制，水平位置控制
飞机将原地悬停在2m处

移动控制接口

在3.2版本中，一共可以看到3种向api传输控制指令的方法，如下：

void control(uint8_t flag, float32_t x, float32_t y, float32_t z, float32_t yaw);//旧接口
void setMovementControl(uint8_t flag, float32_t x, float32_t y, float32_t z, float32_t yaw);
void setFlight(FlightData *data);//旧接口

三种接口形式不同，但是本质上发送的都是上面那些数据，具体请到程序中查看

飞行例程

SDK中自带一段玫瑰线飞行例程，通过0xFA 0xFB 0x09 0x01 0xFE指令启动，具体程序在LocalNavigation.c文件中，下面分段简单介绍一下

1、开始例程

/*
 * @brief To start local navigation (curve following), you need to take off first.
 */
uint32_t startLocalNavExample()
{
  if (droneState.homePositionSetFlag != 1 ||           //home点已设定
      droneState.currentPositionLLA.height < 0.5f ||   //已经起飞
      droneState.curFlightStatus != 3)                 //飞行状态为in air
  {
    droneState.roseCurveTheta =  0.0f;   //玫瑰线公式中的参数
    droneState.localNavExampleRunningFlag = 0;
    printf("Please take off first!\r\n");
    return LOCAL_NAV_FAIL;
  }
  else
  {
    droneState.roseCurveTheta =  0.0f;
    droneState.localNavExampleRunningFlag = 1;
    return LOCAL_NAV_SUCCESS;
  }
}

补充一下droneState.curFlightStatus的状态枚举。
当时我在debug仿真时，此值有时和飞行状态不匹配，但暂时不清楚问题出在什么地方

值	状态
1	stanby
2	take off
3	in air
4	langding
5	finish land

2、结束例程

uint32_t stopLocalNavExample()
{
  droneState.roseCurveTheta =  0.0f;
  droneState.localNavExampleRunningFlag = 0;
  return 0;
}

3、回调函数
OnboardSDK使用轮询线程与N3进行通信。飞行逻辑写在回调函数中

void myRecvCallback(DJI::onboardSDK::CoreAPI * myApi, Header *myHeader, DJI::UserData myUserData)
{
//数据从协议头后开始（这是XMC4700与N3的通信协议，和上面的飞行控制协议完全不同，具体上OnboardSDK官网查看）
  unsigned char *pdata = ((unsigned char *)myHeader) + sizeof(Header);
  unsigned short *enableFlag;

  LocalNavigationStatus *s = (LocalNavigationStatus *)myUserData;//空指针myUserData指向LocalNavigationStatus结构体

  if ((*(pdata) == SET_BROADCAST) && (*(pdata+1) == CODE_BROADCAST))//广播飞行数据
  {
    pdata += 2;//右移到bit2
    enableFlag = (unsigned short *)pdata;//16位数据
    if ( (*enableFlag) & (1<<5) )//bit7: RTK detailed information(定位信息) 
    {
      s->currentPositionLLA = myApi->getBroadcastData().pos;//设定当前坐标
      if (s->localNavExampleRunningFlag == 1 &&    //localNavExample进行中
          s->curFlightStatus == 3 &&               //in air
          s->homePositionSetFlag ==1)              //home已设定
      {
        updateCurveFollowing();//飞一段微元玫瑰线
      }
    }

    if ( (*enableFlag) & (1<<9) ) //bit 9 Flight Status 
    {
      s->prevFlightStatus = s->curFlightStatus;
      s->curFlightStatus  = myApi->getFlightStatus();

      if ((s->prevFlightStatus == 1) && (s->curFlightStatus == 3 ))//起飞完成
      {
        s->homePositionLLA = myApi->getBroadcastData().pos;//设定home点
        if(s->homePositionSetFlag == 0)
        {
          printf("Home Location Set!\r\n");
        }
        else
        {
          printf("Home Location Updated!\r\n");
        }
        s->homePositionSetFlag = 1;
      }
    }
  }
}

这里的pdata中包含一系列enableFlag，程序只取了bit7和bit9判断，其余详细信息请查询OnboardSDK官网

Flag值	状态
1	此数据包含相应数据项
0	此数据不包含相应数据项

4、GPS坐标转局部笛卡尔系坐标
前面已经提到了，SDK封装的飞行控制函数都是基于笛卡尔系的，因此需要将N3广播的GPS经纬度、海拔坐标，转换为规定原点的局部笛卡尔系中（x,y,z）形式的空间坐标才可使用。具体如下：

static void gps_convert_ned_rad(float32_t &ned_x, float32_t &ned_y,
    float32_t gps_t_lon, float32_t gps_t_lat,
    float32_t gps_r_lon, float32_t gps_r_lat)
{
  float32_t d_lon = gps_t_lon - gps_r_lon;
  float32_t d_lat = gps_t_lat - gps_r_lat;
  ned_x = d_lat * C_EARTH;
  ned_y = d_lon * C_EARTH * arm_cos_f32(gps_t_lat);
}；

/*
 * @brief Convert from GPS Lat Lon Alt to local Cartesian coordinate with origin given by homeLocationLLA
 */
static LocalPosition gps_convert_ned(PositionData loc, PositionData homeLocationLLA)
{
  LocalPosition local;
  gps_convert_ned_rad(local.x, local.y,
      (float32_t)loc.longitude, (float32_t)loc.latitude,
      (float32_t)homeLocationLLA.longitude, (float32_t)homeLocationLLA.latitude
    );
    local.z = loc.altitude;
    return local;
}

5、飞行微元距离玫瑰线
SDK不断调用回调函数，每次调用都会飞一小段，但是这里是纯GPS定向，没有PID控制比较容易飘

/*
 * @brief Follow a rose shaped curve at height 25m using movement control 飞行一小段玫瑰线
 */
void updateCurveFollowing(){
  if (droneState.currentPositionLLA.height < 24.5f)//先升到25m，这里没有PID，很容易被风吹飘
  {
    flight.setMovementControl(0x91, 0.0f, 0.0f, 25.0f, 0.0);//0x91->1001 0001
    return;
  }
  else
  {
    droneState.roseCurveTheta += 0.002f;//微元增量
    float32_t rtgt = 25.0f * arm_sin_f32(2.0f*droneState.roseCurveTheta);
    float32_t xtgt = rtgt * arm_cos_f32(droneState.roseCurveTheta);
    float32_t ytgt = rtgt * arm_sin_f32(droneState.roseCurveTheta);//玫瑰线函数

        LocalPosition lp = gps_convert_ned(droneState.currentPositionLLA, droneState.homePositionLLA);//ÀûÓÃGPS¼ÆË㵱ǰµÄxyz×ø±ê

        //移动是增量式的
    flight.setMovementControl(0x91, xtgt-lp.x, ytgt-lp.y, 25.0f, 0.0);

    if (droneState.roseCurveTheta >  6.28f)//飞完了
    {
      droneState.roseCurveTheta =  0.0f;
      droneState.localNavExampleRunningFlag = 0;
    }
  }
}

引入PID控制

在增稳模式（好像是P模式）下，飞机已经自带了PID控制，但是控制效果比较弱，不适合做自动飞行控制。但只要引入简单的单环PID控制，就可以大大提高自动飞行的表现。

一个简单的定点悬停实例如下：

//定义写在.h文件中
typedef struct PidStructure
{
    float32_t ref;
    float32_t fdb;
    float32_t P;
    float32_t I;
    float32_t D;
    float32_t Ek;                              
    float32_t Sk; 
    float32_t Dk; 
    float32_t Ek_1;
    float32_t Ek_2; 
    float32_t out; 
    float32_t outMax;//100 
    float32_t outMin;//-100 
public:
    PidStructure(double REF,double FDB,double p,double i,double d,double EK,double SK,double DK,double EK1,double EK2,double O,double OMax,double OMin)
    {
        ref=REF,fdb=FDB;
        P=p,I=i,D=d; 
        Ek=EK,SK=SK,Dk=DK;
        Ek_1=EK1,Ek_2=EK2;
        out=O,outMax=OMax,outMin=OMin;  
        }
}PidStructure;

//以下写在.c文件
#define kp 3      //自行整定PID参数    
#define ki 0       
#define kd -650  

PidStructure PID_x(0.0,0.0,1.0*kp,1.0*ki,1.0*kd,0.0,0.0,0.0,0.0,0.0,0.0,100.0,-100.0);
PidStructure PID_y(0.0,0.0,1.0*kp,1.0*ki,1.0*kd,0.0,0.0,0.0,0.0,0.0,0.0,100.0,-100.0);


float32_t PID_cal(float32_t REF,float32_t FDB,PidStructure A)//位置环
{
    A.Ek=REF-FDB;
    A.Sk+=A.Ek;
    A.out=1.0*(A.P*A.Ek+A.I*A.Sk+A.D*A.Dk);
    if(A.out>A.outMax)
        A.out=A.outMax;
    if(A.out<A.outMin)
        A.out=A.outMin;

    A.Ek_1=A.Ek_2;
    A.Ek_2=A.Ek;

    A.Dk=A.Ek_2-A.Ek_1; 

    return A.out;
}

//在回调函数中不断循环的地方这样改写
//以悬停起点为原点，转换当前点在局部笛卡尔系的坐标
LocalPosition lp = gps_convert_ned(droneState.currentPositionLLA, XuanTing_Point);
//悬停在2.6米，原点处，yaw角度为0（机头朝向正北）
flight.setMovementControl(0x91,PID_cal(0.0,lp.x,PID_x), PID_cal(0.0,lp.y,PID_y), 2.6f, 0.0f);   

以上就是全部内容了，谢谢大家看到这里。