void random_test_cmd(bool &is_init)
{
cmd_agency_data_t cmd;
cmd.cmd_data = (unsigned char)(random()%22);
cmd.cmd_sequence = (unsigned char)(random()%5 +1);
if(int(random()%2) == 0)
{
App_Send_Data(2, 1, MY_CTRL_CMD_SET, API_CMD_REQUEST, (unsigned char*)&cmd, sizeof(cmd), random_test_ack_cmd_callback, 500, 10);
}
else
{
unsigned char req_status = cmd.cmd_sequence;
App_Send_Data(2, 1, MY_CTRL_CMD_SET, API_CMD_STATUS_REQUEST, (unsigned char*)&req_status, sizeof(req_status),random_test_ack_cmd_callback, 500, 10);
}
}
/*
* @Function: Send Contrl Command by UART
* @Input:
* @Output:
*/
static int Control_Cmd_Send(void)
{
api_ctrl_without_sensor_data_t send_data;
memset(&send_data, 0, sizeof(send_data));
#ifdef XY_SPEED_YAW_RATE
send_data.ctrl_flag = 0x4b; //Vetical:velocity Horizon:velocity Yaw:angular rate
#endif
#ifdef XY_SPEED_YAW_ANGLE
send_data.ctrl_flag = 0x43; //veti_velo hori_velo yaw_angle
#endif
#ifdef XY_POS_YAW_ANGLE
send_data.ctrl_flag = 0x83; //veti_velo hori_posi yaw_angle
#endif
#ifdef XY_ATTI_YAW_RATE
send_data.ctrl_flag = 0x0b;
#endif
send_data.roll_or_x = drone_ctrl_data.x;
send_data.pitch_or_y = drone_ctrl_data.y;
send_data.thr_z = drone_ctrl_data.z;
send_data.yaw = drone_ctrl_data.w;
App_Send_Data(0, 0, MY_CTRL_CMD_SET, API_CTRL_REQUEST, (unsigned char*)&send_data, sizeof(send_data), NULL, 0, 0);
return 0;
}
void set_velocity(dji_sdk::velocity msg)
{
api_ctrl_without_sensor_data_t send_data = {0};
send_data.ctrl_flag = 68; // mode 4
send_data.roll_or_x = msg.velx;
send_data.pitch_or_y = msg.vely;
send_data.thr_z = msg.velz; //m/s
send_data.yaw = gimbal::gimbal_yaw_control_sp;
App_Send_Data(0, 0, MY_CTRL_CMD_SET, API_CTRL_REQUEST, (uint8_t *) &send_data, sizeof(send_data), NULL, 0, 0);
}
void DJI_Onboard_API_Control(unsigned char arg)
{
unsigned char send_data = arg;
if(arg == 1 || arg == 0)
{
}
else
{
printf("Parameter [ERROR]\n");
}
App_Send_Data(1,1,MY_CTRL_CMD_SET,API_OPEN_SERIAL,(unsigned char*)&send_data,
sizeof(send_data),sdk_ack_nav_open_close_callback,1000,0);
}
void DJI_Onboard_API_Ctr(unsigned char flagmode,unsigned int n,float a,float b,float c,float d)
{
api_ctrl_without_sensor_data_t send_data={0};
unsigned char j;
send_data.ctrl_flag = flagmode;
send_data.roll_or_x = a;
send_data.pitch_or_y = b;
send_data.thr_z = c; //m/s
send_data.yaw = d;
for (j=0;j<n;j++)
{
App_Send_Data(0, 0, MY_CTRL_CMD_SET, API_CTRL_REQUEST, (unsigned char*)&send_data, sizeof(send_data));
delay(1);
}
}
void DJI_Onboard_API_Ctr_drawcircle(void)
{
api_ctrl_without_sensor_data_t send_data={0};
unsigned int j;
for(j=0;j<3000;j++)
{
send_data.ctrl_flag = 0x50;
send_data.roll_or_x = 1*cos(C_PI*0.002*j);
send_data.pitch_or_y = 1*sin(C_PI*0.002*j);
send_data.thr_z = 10; //m/s
send_data.yaw = 0;
App_Send_Data(0, 0, MY_CTRL_CMD_SET, API_CTRL_REQUEST, (unsigned char*)&send_data, sizeof(send_data));
delay(1);
}
}
//send data to ground
void send_data_air2ground(uav_data_to_ground data)
{
uint8_t *pbuf = NULL;
//float temp[5];
data.x = data.x / 640.0;
data.y = data.y / 360.0;
data.w = data.w / 640.0;
data.h = data.h / 360.0;
int len = sizeof(data);
pbuf = (uint8_t*)(&data);
std::cout<<"send bytes to ground : "<<len<<std::endl;
App_Send_Data(0, 0, 0x00, 0xfe, pbuf, len, NULL, 0, 1);
}
//send data to ground
void send_data_air2ground_test(int* data)//uav_data_to_ground
{
uint8_t *pbuf = NULL;
float temp[5];
temp[0] = data[0] / 640.0;
temp[1] = data[1] / 360.0;
temp[2] = data[2] / 640.0;
temp[3] = data[3] / 360.0;
temp[4] = data[4];
int len = sizeof(temp);
pbuf = (uint8_t*)temp;
std::cout<<"send bytes to ground : "<<len<<std::endl;
App_Send_Data(0, 0, 0x00, 0xfe, pbuf, len, NULL, 0, 1);
}
void fly_to_localpos(dji_sdk::local_position los,
bool use_height
)
{
//TODO: ALT and HEIGHT
api_ctrl_without_sensor_data_t send_data = {0};
send_data.ctrl_flag = 0x90; // mode 4
send_data.roll_or_x = los.x - dji_variable::local_position_ref.x;
send_data.pitch_or_y = los.y - dji_variable::local_position_ref.y;
send_data.thr_z = los.height; //m/s
send_data.yaw = gimbal::gimbal_yaw_control_sp;
printf("%f %f %f \n",
los.x - dji_variable::local_position_ref.x,
los.y - dji_variable::local_position_ref.y,
los.height
);
App_Send_Data(0, 0, MY_CTRL_CMD_SET, API_CTRL_REQUEST, (uint8_t *) &send_data, sizeof(send_data), NULL, 0, 0);
}
void ros_ctrl_data_callback(float x,float y,float z,float w)
{
api_ctrl_without_sensor_data_t send_data = {0};
printf("mode %f yaw %f pitch %f roll %f vel %f\n", ctrl_mode, z,x,y,w);
if(ctrl_mode == 1)
{
send_data.ctrl_flag = 0x0a; //mode 2
send_data.roll_or_x = y;
send_data.pitch_or_y = x;
send_data.thr_z = w; //m/s
send_data.yaw = z;
}
else if (ctrl_mode == 2)
{
send_data.ctrl_flag = 0x48; //mode 4
send_data.roll_or_x = y;
send_data.pitch_or_y = x;
send_data.thr_z = w; //m/s
send_data.yaw = z;
}
App_Send_Data(0, 0, MY_CTRL_CMD_SET, API_CTRL_REQUEST, (unsigned char*)&send_data, sizeof(send_data), NULL, 0, 0);
}
static void * CmdRecvThread(void * arg)
{
arg = arg; //For eliminating the warning messages.
while(1)
{
if(cmd_unit.is_send_cmd)
{
printf("[DEBUG] in send\n");
cmd_unit.is_send_cmd = 0;
App_Send_Data(2,1,MY_CTRL_CMD_SET, API_CMD_REQUEST,(uint8_t*)&cmd_unit.cmd,sizeof(cmd_unit.cmd),sdk_ack_cmd_callback, 10, 0);
printf("[DEBUG] send req cmd ok\n");
sleep(2);
if( (cmd_unit.ack_result&0xFF00) == 0xFF00 )
{
cmd_unit.ack_callback(&cmd_unit.ack_result);
continue;
}
else if(cmd_unit.ack_result == REQ_TIME_OUT)
{
printf("[DEBUG] recv ack cmd time out \n");
cmd_unit.ack_callback(&cmd_unit.ack_result);
continue;
}
else if(cmd_unit.ack_result == REQ_REFUSE)
{
cmd_unit.ack_callback(&cmd_unit.ack_result);
continue;
}
else if (cmd_unit.ack_result == CMD_RECIEVE)
{
printf("[DEBUG] CMD_RECIEVE \n");
uint8_t req_status = cmd_unit.cmd.cmd_sequence; // can be anything
sleep(7);
App_Send_Data(2, 1,MY_CTRL_CMD_SET, API_CMD_STATUS_REQUEST,(uint8_t*)&req_status,sizeof(uint8_t),sdk_ack_cmd_callback, 10, 0);
printf("[DEBUG] send req status ok\n");
sleep(1);
printf("[DEBUG] recv ack1 status ok\n");
if( is_sys_error(cmd_unit.ack_result))
{
printf("SDK_SYS_ERROR!!! \n");
continue;
}
else
{
cmd_unit.ack_callback(&cmd_unit.ack_result);
}
// for debug
if(cmd_unit.ack_result != STATUS_CMD_EXE_SUCCESS)
{
printf("[DEBUG] WARNING CMD UN-SECCUSS\n");
}
}
}
else
{
usleep(100000);
}
}
return NULL;
}
void DJI_Onboard_API_Control(unsigned char arg)
{
unsigned char send_data = arg;
App_Send_Data(1,1,MY_CTRL_CMD_SET,API_OPEN_SERIAL,(unsigned char*)&send_data,sizeof(send_data));
}
void DJI_Onboard_API_Activation(void)
{
App_Send_Data( 2, 0, MY_ACTIVATION_SET, API_USER_ACTIVATION,(unsigned char*)&activation_msg,sizeof(activation_msg));
}
void test_version_query(void)
{
unsigned char msg = 1;
App_Send_Data(2, 0, MY_ACTIVATION_SET, API_VERSION_QUERY, (unsigned char*)&msg,sizeof(msg), test_version_query_ack_cmd_callback, 1000, 0);
printf("[ACTIVATION] send version_query msg \n");
}
void Pro_API_Activation(void)
{
App_Send_Data( 2, 0, MY_ACTIVATION_SET, API_USER_ACTIVATION,(unsigned char*)&activation_msg,sizeof(activation_msg), test_activation_ack_cmd_callback, 1000, 1);
printf("[ACTIVATION] send acticition msg: %d %d %d %d \n", activation_msg.app_id, activation_msg.app_api_level, activation_msg.app_ver ,activation_msg.app_bundle_id[0]);
}
/*
* test mode 4: vert velocity, hori velocity, yaw angular rate
*/
void basic_test_mode4(bool &is_init)
{
static int cnt;
if(!is_init)
{
cnt = 0;
is_init= true;
}
else
{
api_ctrl_without_sensor_data_t send_data = {0};
cnt++;
if(cnt < 5*50)
{
send_data.ctrl_flag = 0x48;
send_data.roll_or_x = 0.5;
send_data.pitch_or_y = 0;
send_data.thr_z = 0; //m/s
send_data.yaw = 0;
}
else if(cnt < 10*50)
{
send_data.ctrl_flag = 0x48;
send_data.roll_or_x = -0.5;
send_data.pitch_or_y = 0;
send_data.thr_z = 0; //m/s
send_data.yaw = 0;
}
else if(cnt < 15*50)
{
send_data.ctrl_flag = 0x48;
send_data.roll_or_x = 0;
send_data.pitch_or_y = 0.5;
send_data.thr_z = 0; //m/s
send_data.yaw = 0;
}
else if(cnt < 20*50)
{
send_data.ctrl_flag = 0x48;
send_data.roll_or_x = 0;
send_data.pitch_or_y = -0.5;
send_data.thr_z = 0; //m/s
send_data.yaw = 0;
}
else if(cnt < 25*50)
{
send_data.ctrl_flag = 0x48;
send_data.roll_or_x = 0;
send_data.pitch_or_y = 0;
send_data.thr_z = 0.5; //m/s
send_data.yaw = 0;
}
else if(cnt < 30*50)
{
send_data.ctrl_flag = 0x48;
send_data.roll_or_x = 0;
send_data.pitch_or_y = 0;
send_data.thr_z = -0.5; //m/s
send_data.yaw = 0;
}
else if(cnt < 35*50)
{
send_data.ctrl_flag = 0x48;
send_data.roll_or_x = 0;
send_data.pitch_or_y = 0;
send_data.thr_z = 0; //m/s
send_data.yaw = 90;
}
else if(cnt < 40*50)
{
send_data.ctrl_flag = 0x48;
send_data.roll_or_x = 0;
send_data.pitch_or_y = 0;
send_data.thr_z = 0; //m/s
send_data.yaw = -90;
}
else
{
cnt = 0;
}
App_Send_Data(0, 0,MY_CTRL_CMD_SET, API_CTRL_REQUEST, (unsigned char*)&send_data, sizeof(send_data), NULL, 0, 0);
}
}
