bool DJISDKNode::attitude_control_callback(dji_sdk::AttitudeControl::Request& request, dji_sdk::AttitudeControl::Response& response)
{
attitude_data_t user_ctrl_data;
user_ctrl_data.ctrl_flag = request.flag;
user_ctrl_data.roll_or_x = request.x;
user_ctrl_data.pitch_or_y = request.y;
user_ctrl_data.thr_z = request.z;
user_ctrl_data.yaw = request.yaw;
DJI_Pro_Attitude_Control(&user_ctrl_data);
response.result = true;
return true;
}
bool DJISDKNode::local_position_control_callback(dji_sdk::LocalPositionControl::Request& request, dji_sdk::LocalPositionControl::Response& response)
{
float dst_x = request.x;
float dst_y = request.y;
float dst_z = request.z;
attitude_data_t user_ctrl_data;
user_ctrl_data.ctrl_flag = 0x90;
user_ctrl_data.roll_or_x = dst_x - local_position.x;
user_ctrl_data.pitch_or_y = dst_y - local_position.y;
user_ctrl_data.thr_z = dst_z;
user_ctrl_data.yaw = request.yaw;
DJI_Pro_Attitude_Control(&user_ctrl_data);
response.result = true;
return true;
}
bool DJISDKNode::velocity_control_callback(dji_sdk::VelocityControl::Request& request, dji_sdk::VelocityControl::Response& response)
{
attitude_data_t user_ctrl_data;
if (request.frame)
//world frame
user_ctrl_data.ctrl_flag = 0x40;
else
//body frame
user_ctrl_data.ctrl_flag = 0x42;
user_ctrl_data.roll_or_x = request.vx;
user_ctrl_data.pitch_or_y = request.vy;
user_ctrl_data.thr_z = request.vz;
user_ctrl_data.yaw = request.yawAngle;
DJI_Pro_Attitude_Control(&user_ctrl_data);
response.result = true;
return true;
}
void* flyFunc(void*){
attitude_data_t user_ctrl_data;
int i;
for(i = 0; i < 100; i ++)
{
user_ctrl_data.ctrl_flag = HORIZ_VEL|VERT_VEL|YAW_ANG|HORIZ_GND|YAW_GND; // 0x40
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); // 50HZ
}
sleep(1);
}
bool DJISDKNode::global_position_control_callback(dji_sdk::GlobalPositionControl::Request& request, dji_sdk::GlobalPositionControl::Response& response)
{
float dst_x;
float dst_y;
float dst_z = request.altitude;
gps_convert_ned(dst_x,
dst_y,
request.longitude, request.latitude,
global_position.longitude, global_position.latitude);
attitude_data_t user_ctrl_data;
user_ctrl_data.ctrl_flag = 0x90;
user_ctrl_data.roll_or_x = dst_x - local_position.x;
user_ctrl_data.pitch_or_y = dst_y - local_position.y;
user_ctrl_data.thr_z = dst_z;
user_ctrl_data.yaw = request.yaw;
DJI_Pro_Attitude_Control(&user_ctrl_data);
response.result = true;
return true;
}
/*
* 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;
}