玩转四旋翼无人机（DJI SDK LIB)

建议开发者将姿态控制命令以50Hz的频率发送，用户可根据自己的开发环境通过如usleep(20000)、ros::Duration(1/50)等方式实现。

typedef struct
{
    signed short yaw_angle;
    signed short roll_angle;
    signed short pitch_angle;
    struct
    {
        unsigned char base : 1;
        unsigned char yaw_cmd_ignore : 1;
        unsigned char roll_cmd_ignore : 1;
        unsigned char pitch_cmd_ignore : 1;
        unsigned char reserve : 4;
    }ctrl_byte;
    unsigned char duration;
}gimbal_custom_control_angle_t;

解释这里使用了bit field方法，为C++11内容，如下：
Declares a class data member with explicit size, in bits. Adjacent bit field members may be packed to share and straddle the individual bytes.
A bit field declaration is a class data member declaration which uses the following declarator:

identifier(optional) attr(optional) : size (1)

The type of the bit field is introduced by the decl-specifier-seq of the declaration syntax.

1. attr(C++11) - optional sequence of any number of attributes
2. identifier - the name of the bit field that is being declared. The
   name is optional: nameless bit fields introduce the specified number
   of bits of padding
3. size - an integral constant expression with a
   value greater or equal to zero. When greater than zero, this is the
   number of bits that this bit field will occupy. The value zero is
   only allowed for nameless bitfields and has special meaning: it
   specifies that the next bit field in the class definition will begin
   at an allocation unit’s boundary.

typedef struct
{
    signed short yaw_angle_rate;
    signed short roll_angle_rate;
    signed short pitch_angle_rate;
    struct
    {
        unsigned char reserve : 7;
        unsigned char ctrl_switch : 1;//decide increment mode or absolute mode
    }ctrl_byte;
}gimbal_custom_speed_t;

/*
 *struct of api contrl
 */

typedef struct
{
    fp32 q0;
    fp32 q1;
    fp32 q2;
    fp32 q3;
}api_quaternion_data_t;//四元数

typedef struct
{
    fp32 x;
    fp32 y;
    fp32 z;
}api_common_data_t;//三维坐标

/*
 *struct of vellocity data
 */

typedef struct
{
    fp32 x;
    fp32 y;
    fp32 z;
    unsigned char health_flag         :1;
    unsigned char feedback_sensor_id  :4;
    unsigned char reserve             :3;
}api_vel_data_t;

当且仅当GPS信号正常（health_flag >=3）时，才可以使用水平*位置控制（HORI_POS）相关的控制指令
- 当GPS信号正常（health_flag >=3），或者Gudiance系统正常工作（连接安装正确）时，可以使用水平速度控制（HORI_VEL）相关的控制指令

typedef struct
{
    fp64 lati;
    fp64 longti;
    fp32 alti;
    fp32 height;
    unsigned char health_flag;
}api_pos_data_t;

typedef struct
{
    signed short roll;
    signed short pitch;
    signed short yaw;
    signed short throttle;
    signed short mode;
    signed short gear;
}api_rc_data_t;

typedef struct
{
    signed short x;
    signed short y;
    signed short z;
}api_mag_data_t;

typedef struct
{
    unsigned char cur_ctrl_dev_in_navi_mode   :3;/*0->rc  1->app  2->serial*/
    unsigned char serial_req_status           :1;/*1->opensd  0->close*/
    unsigned char reserved                    :4;
}api_ctrl_info_data_t;

typedef struct
{
    unsigned int time_stamp;
    api_quaternion_data_t q;
    api_common_data_t a;
    api_vel_data_t v;
    api_common_data_t w;
    api_pos_data_t pos;
    api_mag_data_t mag;
    api_rc_data_t rc;
    api_common_data_t gimbal;
    unsigned char status;
    unsigned char battery_remaining_capacity;
    api_ctrl_info_data_t ctrl_info;
    uint8_t obtained_control;
     uint8_t activation;
}sdk_std_msg_t;

自动起飞、降落及返航

该函数的回调函数中应答值对应查询飞行状态切换结果的内容（下面的示例代码中未使用回调函数）。

/* Take off */
DJI_Pro_Status_Ctrl(4,NULL);
/* Land */
DJI_Pro_Status_Ctrl(6,NULL);
/* Return to home */
DJI_Pro_Status_Ctrl(1,NULL);

数据读取

如果希望读取飞控数据，请在N1调参软件中打开相对应的输出选项。同时请确认部分数据所在坐标系。
开发者在程序中需要声明一个相应的变量并使用对应的函数获取飞控的状态信息。
以获取姿态四元数为例：
1、声明四元数结构体

api_quaternion_data_t quat;

2、获取姿态四元数

DJI_Pro_Get_Quaternion(&quat);

飞控外发的其他数据类型及获取相应数据的函数请参考DJI_Pro_App.h

GPS坐标解算

将经纬度换算成北东坐标系。（GPS经纬度为弧度值，北东坐标系单位为米）
实例中,origin_longti及origin_lati为原点位置经纬度，开发者可根据实际使用情况设定，建议采用飞行器起飞位置为原点位置；longti及lati为飞行器当前所在位置；x，y为解算出在北和东两个方向上相距原点的坐标，单位米。

#define C_EARTH (double) 6378137.0
/* From GPS to Ground */
{
    double dlati = lati-origin_lati;
    double dlongti= longti-origin_longti;

    double x = dlati * C_EARTH;
    double y = dlongti * C_EARTH * cos(lati / 2.0 + origin_lati / 2.0);
}

姿态四元数的解算

将姿态四元数换算成Body系下的roll, pitch, yaw的弧度值。

    api_quaternion_data_t q;
    DJI_Pro_Get_Quaternion(&q);

    float roll  = atan2(2.0 * (q.q3 * q.q2 + q.q0 * q.q1) , 1.0 - 2.0 * (q.q1 * q.q1 + q.q2 * q.q2));
    float pitch = asin(2.0 * (q.q2 * q.q0 - q.q3 * q.q1));
    float yaw   = atan2(2.0 * (q.q3 * q.q0 + q.q1 * q.q2) , - 1.0 + 2.0 * (q.q0 * q.q0 + q.q1 * q.q1));

位置控制(HORI_POS)

当水平方向采用位置控制模式（HORI_POS）时，水平方向上的输入量为当前位置与目标位置的偏移（offset）单位：米。
实例中,采用Ground坐标系，target为目标位置坐标，current为飞行器位置坐标，开发者可通过GPS、Guidance或其他传感器计算出相关坐标信息。例如采用本文当中“GPS坐标解算“部分通过GPS解算位置信息。
由于飞控接受的最大控制频率为50 Hz，因此为保证控制效果，offset的计算频率应至少大于50 Hz。

void update_offset()
{
    offset_x = target_x - current_x;
    offset_y = target_y - current_y;
}



/* Command thread */
attitude_data_t user_ctrl_data;

/* HORI_POS|VERT_VEL|YAW_RATE|HORI_GROUND_FRAME|YAW_GROUND_FRAME */
user_ctrl_data.ctrl_flag = 0x88;
user_ctrl_data.thr_z = 0;
user_ctrl_data.yaw = 0;

while(1)                                            
{
    update_offset();
    if (/*offset is small enough*/)
        break;
    user_ctrl_data.roll_or_x = offset_x;
    user_ctrl_data.pitch_or_y = offset_y;

    DJI_Pro_Attitude_Control(&user_ctrl_data);

    usleep(20000);                                  /* 50 Hz */
}

姿态控制
/*
* This method is responsible for testing atttitude
*/
static void * DJI_Sample_Atti_Ctrl_Thread_Func(void *arg)
{
int i;
attitude_data_t user_ctrl_data;

/* takeoff */
DJI_Pro_Status_Ctrl(4,0);
sleep(8);
/* attitude control, go up */
for(i = 0; i < 100; i ++)
{
    user_ctrl_data.ctrl_flag = 0x40;
    user_ctrl_data.roll_or_x = 0;
    user_ctrl_data.pitch_or_y = 0;
    if(i < 90)
        user_ctrl_data.thr_z = 2.0;
    else
        user_ctrl_data.thr_z = 0.0;
    user_ctrl_data.yaw = 0;
    DJI_Pro_Attitude_Control(&user_ctrl_data);
    usleep(20000);    
}
sleep(1);
for(i = 0; i < 200; i ++)
{
    user_ctrl_data.ctrl_flag = 0x40;
    if(i < 180)
        user_ctrl_data.roll_or_x = 2;
    else
        user_ctrl_data.roll_or_x = 0;
    user_ctrl_data.pitch_or_y = 0;
    user_ctrl_data.thr_z = 0;
    user_ctrl_data.yaw = 0;
    DJI_Pro_Attitude_Control(&user_ctrl_data);
    usleep(20000);
}
sleep(1);
for(i = 0; i < 200; i ++)
{
    user_ctrl_data.ctrl_flag = 0x40;
    if(i < 180)
        user_ctrl_data.roll_or_x = -2;
    else
        user_ctrl_data.roll_or_x = 0;
    user_ctrl_data.pitch_or_y = 0;
    user_ctrl_data.thr_z = 0;
    user_ctrl_data.yaw = 0;
    DJI_Pro_Attitude_Control(&user_ctrl_data);
    usleep(20000);
}
sleep(1);
for(i = 0; i < 200; i ++)
{
    user_ctrl_data.ctrl_flag = 0x40;
    user_ctrl_data.roll_or_x = 0;
    if(i < 180)
        user_ctrl_data.pitch_or_y = 2;
    else
        user_ctrl_data.pitch_or_y = 0;
    user_ctrl_data.thr_z = 0;
    user_ctrl_data.yaw = 0;
    DJI_Pro_Attitude_Control(&user_ctrl_data);
    usleep(20000);
}
sleep(1);
for(i = 0; i < 200; i ++)
{
    user_ctrl_data.ctrl_flag = 0x40;
    user_ctrl_data.roll_or_x = 0;
    if(i < 180)
        user_ctrl_data.pitch_or_y = -2;
    else
        user_ctrl_data.pitch_or_y = 0;
    user_ctrl_data.thr_z = 0;
    user_ctrl_data.yaw = 0;
    DJI_Pro_Attitude_Control(&user_ctrl_data);
    usleep(20000);
}
sleep(1);
for(i = 0; i < 200; i ++)
{
    user_ctrl_data.ctrl_flag = 0x40;
    user_ctrl_data.roll_or_x = 0;
    user_ctrl_data.pitch_or_y = 0;
    if(i < 180)
        user_ctrl_data.thr_z = 0.5;
    else
        user_ctrl_data.thr_z = 0;
    user_ctrl_data.yaw = 0;
    DJI_Pro_Attitude_Control(&user_ctrl_data);
    usleep(20000);
}
sleep(1);
for(i = 0; i < 200; i ++)
{
    user_ctrl_data.ctrl_flag = 0x40;
    user_ctrl_data.roll_or_x = 0;
    user_ctrl_data.pitch_or_y = 0;
    if(i < 180)
        user_ctrl_data.thr_z = -0.5;
    else
        user_ctrl_data.thr_z = 0;
    user_ctrl_data.yaw = 0;
    DJI_Pro_Attitude_Control(&user_ctrl_data);
    usleep(20000);
}
sleep(1);
for(i = 0; i < 200; i ++)
{
    user_ctrl_data.ctrl_flag = 0xA;
    user_ctrl_data.roll_or_x = 0;
    user_ctrl_data.pitch_or_y = 0;
    user_ctrl_data.thr_z = 0;
    if(i < 180)
        user_ctrl_data.yaw = 90;
    else
        user_ctrl_data.yaw = 0;
    DJI_Pro_Attitude_Control(&user_ctrl_data);
    usleep(20000);
}
sleep(1);
for(i = 0; i < 200; i ++)
{
    user_ctrl_data.ctrl_flag = 0xA;
    user_ctrl_data.roll_or_x = 0;
    user_ctrl_data.pitch_or_y = 0;
    user_ctrl_data.thr_z = 0;
    if(i < 180)
        user_ctrl_data.yaw = -90;
    else
        user_ctrl_data.yaw = 0;
    DJI_Pro_Attitude_Control(&user_ctrl_data);
    usleep(20000);
}
sleep(1);
/* gohome */
DJI_Pro_Status_Ctrl(1,0);

atti_ctrl_sample_flag = -1;
return (void*)NULL;

}

attitude
DJI_Pro_Attitude_Control ⟹ DJI_Pro_App_Send_Data(API_CTRL_REQUEST)
DJI_Pro_Gimbal_Angle_Control ⟹ DJI_Pro_App_Send_Data(API_GIMBAL_CTRL_ANGLE_REQUEST)
DJI_Sample_Gimbal_Ctrl_Thread_Func ⟹

• DJI_Sample_Gimbal_AngelCtrl
• DJI_Sample_Gimbal_SpeedCtrl
• DJI_Sample_Gimbal_AngelCtrl

DJI_Sample_Atti_Ctrl_Thread_Func ⟹

• DJI_Pro_Status_Ctrl(4,0);
• user_ctrl_data.ctrl_flag = 0x40;
• user_ctrl_data.roll_or_x = 0;
• user_ctrl_data.pitch_or_y = 0;
• user_ctrl_data.thr_z = 2.0;
• DJI_Pro_Attitude_Control;

DJI_Sample_Gimbal_AngelCtrl ⟹

• gimbal_angel.yaw_angle = yaw_angle;
• gimbal_angel.roll_angle = roll_angle;
• gimbal_angel.pitch_angle = pitch_angle;
• gimbal_angel.ctrl_byte.yaw_cmd_ignore = 0;
• gimbal_angel.ctrl_byte.roll_cmd_ignore = 0;
• gimbal_angel.ctrl_byte.pitch_cmd_ignore = 0;
• gimbal_angel.duration = duration;
• DJI_Pro_App_Send_Data(API_GIMBAL_CTRL_ANGLE_REQUEST)