玩轉四旋翼無人機(DJI SDK LIB)

NO IMAGE

建議開發者將姿態控制命令以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_longtiorigin_lati為原點位置經緯度,開發者可根據實際使用情況設定,建議採用飛行器起飛位置為原點位置;longtilati為飛行器當前所在位置;xy為解算出在北和東兩個方向上相距原點的座標,單位米。

#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⟹\LongrightarrowDJI_Pro_App_Send_Data(API_CTRL_REQUEST)
DJI_Pro_Gimbal_Angle_Control⟹\LongrightarrowDJI_Pro_App_Send_Data(API_GIMBAL_CTRL_ANGLE_REQUEST)
DJI_Sample_Gimbal_Ctrl_Thread_Func⟹\Longrightarrow

  • DJI_Sample_Gimbal_AngelCtrl
  • DJI_Sample_Gimbal_SpeedCtrl
  • DJI_Sample_Gimbal_AngelCtrl

DJI_Sample_Atti_Ctrl_Thread_Func⟹\Longrightarrow

  • 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⟹\Longrightarrow

  • 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)