玩转四旋翼无人机(DJI SDK LIB)
来源:互联网 发布:照片转换卡通软件 编辑:程序博客网 时间:2024/06/13 01:32
建议开发者将姿态控制命令以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.
- attr(C++11) - optional sequence of any number of attributes
- 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 - 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_Gimbal_Angle_Control
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)
- 玩转四旋翼无人机(DJI SDK LIB)
- 玩转四旋翼无人机(DJI SDK 使用)
- 玩转四旋翼无人机(DJI OnBoard SDK ROS)
- 玩转四旋翼无人机(DJI ROS SDK开发包及使用方法)
- Android调试DJI大疆无人机方法
- 【dji sdk】mobile sdk开发
- Android DJI Mobile-SDK 开发
- 【dji sdk】mobile sdk 视频部分
- 【dji sdk】mobile sdk 摇杆部分
- DJI SDK iOS 开发之一:前言
- Dji Mobile SDK 基础实现(一)
- Dji Mobile SDK 基础实现(二)
- @DJI大疆创新 @Spark 无人机超级派对!| RobotX Meetup No.44
- DJI phantom SDK 开发——开发前准备
- DJI SDK开发——第一个程序
- DJI Mobile SDK(1):获取App Key
- App开发日报 2015-05-15 大疆无人机IOS开发之搭建DJI Phantom和iOS视觉平台
- DJI Mobile SDK笔记(一):移植SDK库到自己的工程
- IOS内存管理3&之保留环&弱引用&属性保存
- 网站技巧
- 峰顶虽高虽远不易及,却有更浩瀚的视界【转自论坛】
- mac下使用adb链接安卓手机教程
- Prof. Li Feifei写给她学生的一封信
- 玩转四旋翼无人机(DJI SDK LIB)
- ARM64-memcpy.S 汇编源码分析
- JavaScript的Ajax第一次
- python入门: 快速python语法总结
- CSS3 基础知识
- 机器学习:FP-growth 发现频繁项集
- 冒泡排序
- 程序员常去的14个顶级开发社区!
- POJ1364.King(差分约束系统)