bool action_callback(
dji_ros::action::Request& request,
dji_ros::action::Response& response
)
{
if(request.action== 4)
{
//takeoff
DJI_Pro_Status_Ctrl(4,0);
response.result= true;
}
else if(request.action == 6)
{
//landing
DJI_Pro_Status_Ctrl(6,0);
response.result= true;
}
else if(request.action == 1)
{
//gohome
DJI_Pro_Status_Ctrl(1,0);
response.result= true;
}
else
response.result= false;
return true;
}
int main(int argc, char *argv[])
{
OurAllocConsole();
listSerialPort(); // 列举串口
OurSetUp(); // init
OurDoSerialPort(OUR_OPEN_PORT); // 打开串口端口
OurActivate(); // 激活
OurDoControl(OUR_OBTAIN_CONTROL); // 获取控制权
/******************************************************************************************************/
// 以下是飞控程序
// 自动起飞,在异步线程中进行,sleep(8) 保证执行完成
DJI_Pro_Status_Ctrl(4, 0);
//sleep(8);
// fly();
sleep(2);
// ***待修改,应当根据返回的高度参数确认是否releaseControl,不应使用sleep
// 自动降落,也可选择自动返航
//DJI_Pro_Status_Ctrl(6, 0);
//sleep(8);
//while(true);
/******************************************************************************************************/
OurDoControl(OUR_RELEASE_CONTROL); // 释放控制权
OurDoSerialPort(OUR_CLOSE_PORT); // 关闭串口端口
sleep(2); // 等待所有操作完成
//getchar();
return 0;
}
bool DJISDKNode::drone_task_control_callback(dji_sdk::DroneTaskControl::Request& request, dji_sdk::DroneTaskControl::Response& response)
{
if (request.task== 4) {
//takeoff
DJI_Pro_Status_Ctrl(4, 0);
response.result = true;
}
else if (request.task == 6) {
//landing
DJI_Pro_Status_Ctrl(6, 0);
response.result = true;
}
else if (request.task == 1) {
//gohome
DJI_Pro_Status_Ctrl(1, 0);
response.result = true;
}
else
response.result = false;
return true;
}
void set_return2home()
{
uint8_t send_data = 1;
printf("cmd %d\n", send_data);
if (send_data > 21)
return;
if (cmd_send_flag) {
DJI_Pro_Status_Ctrl(send_data, cmd_send_cb);
cmd_send_flag = 0;
}
else {
printf("[CMD] wating! \n");
}
dji_variable::wp_m.pause_flying();
}
/*
* 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;
}